35 namespace sixDoFRigidBodyMotionRestraints
86 vector oldDir = refQ_ & refDir;
90 if (
mag(oldDir & axis_) > 0.95 ||
mag(newDir & axis_) > 0.95)
96 vector oldDir = refQ_ & refDir;
102 oldDir -= (axis_ & oldDir)*axis_;
103 oldDir /= (
mag(oldDir) + VSMALL);
105 newDir -= (axis_ & newDir)*axis_;
106 newDir /= (
mag(newDir) + VSMALL);
108 scalar theta =
mag(
acos(
min(oldDir & newDir, 1.0)));
111 vector a = (oldDir ^ newDir);
114 a = (a & axis_)*axis_;
116 scalar magA =
mag(a);
128 restraintMoment = -stiffness_*theta*a - damping_*(motion.
omega() & a)*a;
138 Info<<
" angle " << theta*
sign(a & axis_)
139 <<
" force " << restraintForce
140 <<
" moment " << restraintMoment
153 refQ_ = sDoFRBMRCoeffs_.lookupOrDefault<
tensor>(
"referenceOrientation",
I);
159 "Foam::sixDoFRigidBodyMotionRestraints::linearAxialAngularSpring::"
162 "const dictionary& sDoFRBMRDict"
165 <<
"referenceOrientation " << refQ_ <<
" is not a rotation tensor. "
166 <<
"mag(referenceOrientation) - sqrt(3) = "
171 axis_ = sDoFRBMRCoeffs_.lookup(
"axis");
173 scalar magAxis(
mag(axis_));
175 if (magAxis > VSMALL)
183 "Foam::sixDoFRigidBodyMotionRestraints::linearAxialAngularSpring::"
186 "const dictionary& sDoFRBMCDict"
189 <<
"axis has zero length"
193 sDoFRBMRCoeffs_.lookup(
"stiffness") >> stiffness_;
195 sDoFRBMRCoeffs_.lookup(
"damping") >> damping_;