34 namespace sixDoFRigidBodyMotionConstraints
39 sixDoFRigidBodyMotionConstraint,
71 const vector& existingConstraintForce,
72 const vector& existingConstraintMoment,
74 vector& constraintPosition,
75 vector& constraintForceIncrement,
76 vector& constraintMomentIncrement
81 scalar maxTheta = -SMALL;
91 refDir[(cmpt + 1) % 3] = 1;
96 existingConstraintMoment,
101 predictedDir -= (axis & predictedDir)*axis;
103 scalar theta = GREAT;
105 scalar magPredictedDir =
mag(predictedDir);
107 if (magPredictedDir > VSMALL)
109 predictedDir /= magPredictedDir;
111 theta =
acos(
min(predictedDir & refDir, 1.0));
114 axis = (refDir ^ predictedDir);
116 scalar magAxis =
mag(axis);
118 if (magAxis > VSMALL)
128 if (theta > maxTheta)
133 constraintMomentIncrement +=
143 bool converged(
mag(maxTheta) < tolerance_);
145 if (sixDoFRigidBodyMotionConstraint::debug)
147 Info<<
" max angle " << maxTheta
148 <<
" force " << constraintForceIncrement
149 <<
" moment " << constraintMomentIncrement;
157 Info<<
" not converged";