34 namespace sixDoFRigidBodyMotionConstraints
72 const vector& existingConstraintForce,
73 const vector& existingConstraintMoment,
75 vector& constraintPosition,
76 vector& constraintForceIncrement,
77 vector& constraintMomentIncrement
85 existingConstraintMoment,
89 scalar theta =
acos(
min(predictedDir & fixedAxis_, 1.0));
91 vector rotationAxis = fixedAxis_ ^ predictedDir;
93 scalar magRotationAxis =
mag(rotationAxis);
95 if (magRotationAxis > VSMALL)
97 rotationAxis /= magRotationAxis;
102 rotationAxis = Q.
T() & rotationAxis;
104 constraintMomentIncrement =
110 constraintMomentIncrement = Q & constraintMomentIncrement;
113 constraintMomentIncrement -=
114 (constraintMomentIncrement & fixedAxis_)*fixedAxis_;
121 bool converged(
mag(theta) < tolerance_);
123 if (sixDoFRigidBodyMotionConstraint::debug)
125 Info<<
" angle " << theta
126 <<
" force " << constraintForceIncrement
127 <<
" moment " << constraintMomentIncrement;
135 Info<<
" not converged";
152 sDoFRBMCCoeffs_.lookup(
"axis") >> fixedAxis_;
154 scalar magFixedAxis(
mag(fixedAxis_));
156 if (magFixedAxis > VSMALL)
158 fixedAxis_ /= magFixedAxis;
164 "Foam::sixDoFRigidBodyMotionConstraints::fixedAxis::read"
166 "const dictionary& sDoFRBMCDict"
169 <<
"axis has zero length"