30 void Foam::sixDoFRigidBodyMotion::applyRestraints()
32 if (restraints_.
empty())
43 Info<<
"Restraint " << restraintNames_[rI] <<
": ";
55 restraints_[rI].restrain(*
this, rP, rF, rM);
67 void Foam::sixDoFRigidBodyMotion::applyConstraints(scalar deltaT)
69 if (constraints_.empty())
78 bool allConverged =
true;
92 if (sixDoFRigidBodyMotionConstraint::debug)
94 Info<<
"Constraint " << constraintNames_[cI] <<
": ";
106 bool constraintConverged = constraints_[cI].constrain
117 allConverged = allConverged && constraintConverged;
123 cMA += cM + ((cP - centreOfMass()) ^ cF);
126 }
while(++iteration < maxConstraintIterations_ && !allConverged);
128 if (iteration >= maxConstraintIterations_)
132 "Foam::sixDoFRigidBodyMotion::applyConstraints"
135 <<
nl <<
"Maximum number of sixDoFRigidBodyMotion constraint "
137 << maxConstraintIterations_
138 <<
") exceeded." <<
nl
142 Info<<
"sixDoFRigidBodyMotion constraints converged in "
143 << iteration <<
" iterations" <<
endl;
147 Info<<
"Constraint force: " << cFA <<
nl
148 <<
"Constraint moment: " << cMA
158 tau() += Q().T() & cMA;
172 maxConstraintIterations_(0),
185 const point& centreOfMass,
192 const point& initialCentreOfMass,
213 maxConstraintIterations_(0),
214 initialCentreOfMass_(initialCentreOfMass),
216 momentOfInertia_(momentOfInertia),
231 maxConstraintIterations_(0),
234 dict.lookupOrDefault(
"initialCentreOfMass", centreOfMass())
238 dict.lookupOrDefault(
"initialOrientation", Q())
240 momentOfInertia_(dict.lookup(
"momentOfInertia")),
242 cDamp_(dict.lookupOrDefault<scalar>(
"accelerationDampingCoeff", 0.0)),
243 aLim_(dict.lookupOrDefault<scalar>(
"accelerationLimit", VGREAT)),
244 report_(dict.lookupOrDefault<
Switch>(
"report", false))
257 motionState_(sDoFRBM.motionState_),
258 restraints_(sDoFRBM.restraints_),
259 restraintNames_(sDoFRBM.restraintNames_),
260 constraints_(sDoFRBM.constraints_),
261 constraintNames_(sDoFRBM.constraintNames_),
262 maxConstraintIterations_(sDoFRBM.maxConstraintIterations_),
263 initialCentreOfMass_(sDoFRBM.initialCentreOfMass_),
264 initialQ_(sDoFRBM.initialQ_),
265 momentOfInertia_(sDoFRBM.momentOfInertia_),
266 mass_(sDoFRBM.mass_),
267 cDamp_(sDoFRBM.cDamp_),
268 aLim_(sDoFRBM.aLim_),
269 report_(sDoFRBM.report_)
286 if (dict.
found(
"restraints"))
292 restraints_.setSize(restraintDict.
size());
294 restraintNames_.setSize(restraintDict.
size());
308 restraintNames_[i] = iter().keyword();
314 restraints_.setSize(i);
316 restraintNames_.setSize(i);
326 if (dict.
found(
"constraints"))
332 constraints_.setSize(constraintDict.
size());
334 constraintNames_.setSize(constraintDict.
size());
348 constraintNames_[i] = iter().keyword();
354 constraints_.setSize(i);
356 constraintNames_.setSize(i);
358 if (!constraints_.empty())
360 maxConstraintIterations_ = readLabel
362 constraintDict.
lookup(
"maxIterations")
381 scalar aMag =
mag(aClip);
392 "void Foam::sixDoFRigidBodyMotion::updatePosition"
398 <<
"Limited acceleration " << a()
399 <<
" to " << aClip*aLim_
403 v() += 0.5*(1 - cDamp_)*deltaT0*aClip*
min(aMag, aLim_);
405 pi() += 0.5*(1 - cDamp_)*deltaT0*tau();
408 centreOfMass() += deltaT*v();
411 rotate(Q(),
pi(), deltaT);
432 tau() = (Q().
T() & tauGlobal);
436 applyConstraints(deltaT);
439 scalar aMag =
mag(aClip);
450 "void Foam::sixDoFRigidBodyMotion::updateForce"
452 "const vector& fGlobal, "
453 "const vector& tauGlobal, "
457 <<
"Limited acceleration " << a()
458 <<
" to " << aClip*aLim_
462 v() += 0.5*(1 - cDamp_)*deltaT*aClip*
min(aMag, aLim_);
464 pi() += 0.5*(1 - cDamp_)*deltaT*tau();
489 fGlobal =
sum(forces);
493 tauGlobal += (positions[i] - centreOfMass()) ^ forces[i];
497 updateForce(fGlobal, tauGlobal, deltaT);
503 const point& pInitial,
505 const vector& deltaMoment,
509 vector vTemp = v() + deltaT*(a() + deltaForce/mass_);
511 vector piTemp =
pi() + deltaT*(tau() + (Q().
T() & deltaMoment));
513 point centreOfMassTemp = centreOfMass() + deltaT*vTemp;
517 rotate(QTemp, piTemp, deltaT);
522 + (QTemp & initialQ_.
T() & (pInitial - initialCentreOfMass_))
530 const vector& deltaMoment,
534 vector piTemp =
pi() + deltaT*(tau() + (Q().
T() & deltaMoment));
538 rotate(QTemp, piTemp, deltaT);
540 return (QTemp & initialQ_.
T() & vInitial);
546 Info<<
"Centre of mass: " << centreOfMass() <<
nl
547 <<
"Linear velocity: " << v() <<
nl
548 <<
"Angular velocity: " << omega()