rigidBodyDynamics: Simplify handling of quaternions by maintaining a unit quaternion in the joint state field 'q'

'w' is now obtained from 'v' using the relation w = sqrt(1 - |sqr(v)|)
and 'v' is stored in the joint state field 'q' and integrated in the
usual manner but corrected using quaternion transformations.

applications/test/rigidBodyDynamics/sphericalJoint/sphericalJoint.C

@@ -64,8 +64,7 @@ int main(int argc, char *argv[])
     sphericalJoint.joints()[1]
     (
         quaternion(quaternion::ZYX, vector(0.3, 0, 0)),
-        sphericalJoint.state().q(),
+        sphericalJoint.state().q()
-        sphericalJoint.state().w()
     );
 
     // Set the gravitational acceleration
@@ -90,8 +89,7 @@ int main(int argc, char *argv[])
             << t << " "
             << sphericalJoint.joints()[1]
                (
-                   sphericalJoint.state().q(),
+                   sphericalJoint.state().q()
-                   sphericalJoint.state().w()
                ).eulerAngles(quaternion::ZYX).x()
             << endl;
     }

src/rigidBodyDynamics/joints/Pa/Pa.C

@@ -85,7 +85,6 @@ void Foam::RBD::joints::Pa::jcalc
 (
     joint::XSvc& J,
     const scalarField& q,
-    const scalarField& w,
     const scalarField& qDot
 ) const
 {

src/rigidBodyDynamics/joints/Pa/Pa.H

@@ -92,7 +92,6 @@ public:
         (
             joint::XSvc& J,
             const scalarField& q,
-            const scalarField& w,
             const scalarField& qDot
         ) const;
 

src/rigidBodyDynamics/joints/Px/Px.C

@@ -84,7 +84,6 @@ void Foam::RBD::joints::Px::jcalc
 (
     joint::XSvc& J,
     const scalarField& q,
-    const scalarField& w,
     const scalarField& qDot
 ) const
 {

src/rigidBodyDynamics/joints/Px/Px.H

@@ -92,7 +92,6 @@ public:
         (
             joint::XSvc& J,
             const scalarField& q,
-            const scalarField& w,
             const scalarField& qDot
         ) const;
 };

src/rigidBodyDynamics/joints/Pxyz/Pxyz.C

@@ -88,7 +88,6 @@ void Foam::RBD::joints::Pxyz::jcalc
 (
     joint::XSvc& J,
     const scalarField& q,
-    const scalarField& w,
     const scalarField& qDot
 ) const
 {

src/rigidBodyDynamics/joints/Pxyz/Pxyz.H

@@ -92,7 +92,6 @@ public:
         (
             joint::XSvc& J,
             const scalarField& q,
-            const scalarField& w,
             const scalarField& qDot
         ) const;
 };

src/rigidBodyDynamics/joints/Py/Py.C

@@ -84,7 +84,6 @@ void Foam::RBD::joints::Py::jcalc
 (
     joint::XSvc& J,
     const scalarField& q,
-    const scalarField& w,
     const scalarField& qDot
 ) const
 {

src/rigidBodyDynamics/joints/Py/Py.H

@@ -92,7 +92,6 @@ public:
         (
             joint::XSvc& J,
             const scalarField& q,
-            const scalarField& w,
             const scalarField& qDot
         ) const;
 };

src/rigidBodyDynamics/joints/Pz/Pz.C

@@ -84,7 +84,6 @@ void Foam::RBD::joints::Pz::jcalc
 (
     joint::XSvc& J,
     const scalarField& q,
-    const scalarField& w,
     const scalarField& qDot
 ) const
 {

src/rigidBodyDynamics/joints/Pz/Pz.H

@@ -92,7 +92,6 @@ public:
         (
             joint::XSvc& J,
             const scalarField& q,
-            const scalarField& w,
             const scalarField& qDot
         ) const;
 };

src/rigidBodyDynamics/joints/Ra/Ra.C

@@ -85,7 +85,6 @@ void Foam::RBD::joints::Ra::jcalc
 (
     joint::XSvc& J,
     const scalarField& q,
-    const scalarField& w,
     const scalarField& qDot
 ) const
 {

src/rigidBodyDynamics/joints/Ra/Ra.H

@@ -92,7 +92,6 @@ public:
         (
             joint::XSvc& J,
             const scalarField& q,
-            const scalarField& w,
             const scalarField& qDot
         ) const;
 

src/rigidBodyDynamics/joints/Rs/Rs.C

@@ -94,11 +94,10 @@ void Foam::RBD::joints::Rs::jcalc
 (
     joint::XSvc& J,
     const scalarField& q,
-    const scalarField& w,
     const scalarField& qDot
 ) const
 {
-    J.X.E() = operator()(q, w).R().T();
+    J.X.E() = operator()(q).R().T();
     J.X.r() = Zero;
 
     J.S = Zero;

src/rigidBodyDynamics/joints/Rs/Rs.H

@@ -97,7 +97,6 @@ public:
         (
             joint::XSvc& J,
             const scalarField& q,
-            const scalarField& w,
             const scalarField& qDot
         ) const;
 };

src/rigidBodyDynamics/joints/Rx/Rx.C

@@ -84,7 +84,6 @@ void Foam::RBD::joints::Rx::jcalc
 (
     joint::XSvc& J,
     const scalarField& q,
-    const scalarField& w,
     const scalarField& qDot
 ) const
 {

src/rigidBodyDynamics/joints/Rx/Rx.H

@@ -92,7 +92,6 @@ public:
         (
             joint::XSvc& J,
             const scalarField& q,
-            const scalarField& w,
             const scalarField& qDot
         ) const;
 };

src/rigidBodyDynamics/joints/Rxyz/Rxyz.C

@@ -88,7 +88,6 @@ void Foam::RBD::joints::Rxyz::jcalc
 (
     joint::XSvc& J,
     const scalarField& q,
-    const scalarField& w,
     const scalarField& qDot
 ) const
 {

src/rigidBodyDynamics/joints/Rxyz/Rxyz.H

@@ -93,7 +93,6 @@ public:
         (
             joint::XSvc& J,
             const scalarField& q,
-            const scalarField& w,
             const scalarField& qDot
         ) const;
 };

src/rigidBodyDynamics/joints/Ry/Ry.C

@@ -84,7 +84,6 @@ void Foam::RBD::joints::Ry::jcalc
 (
     joint::XSvc& J,
     const scalarField& q,
-    const scalarField& w,
     const scalarField& qDot
 ) const
 {

src/rigidBodyDynamics/joints/Ry/Ry.H

@@ -92,7 +92,6 @@ public:
         (
             joint::XSvc& J,
             const scalarField& q,
-            const scalarField& w,
             const scalarField& qDot
         ) const;
 };

src/rigidBodyDynamics/joints/Ryxz/Ryxz.C

@@ -88,7 +88,6 @@ void Foam::RBD::joints::Ryxz::jcalc
 (
     joint::XSvc& J,
     const scalarField& q,
-    const scalarField& w,
     const scalarField& qDot
 ) const
 {

src/rigidBodyDynamics/joints/Ryxz/Ryxz.H

@@ -93,7 +93,6 @@ public:
         (
             joint::XSvc& J,
             const scalarField& q,
-            const scalarField& w,
             const scalarField& qDot
         ) const;
 };

src/rigidBodyDynamics/joints/Rz/Rz.C

@@ -84,7 +84,6 @@ void Foam::RBD::joints::Rz::jcalc
 (
     joint::XSvc& J,
     const scalarField& q,
-    const scalarField& w,
     const scalarField& qDot
 ) const
 {

src/rigidBodyDynamics/joints/Rz/Rz.H

@@ -92,7 +92,6 @@ public:
         (
             joint::XSvc& J,
             const scalarField& q,
-            const scalarField& w,
             const scalarField& qDot
         ) const;
 };

src/rigidBodyDynamics/joints/Rzyx/Rzyx.C

@@ -88,7 +88,6 @@ void Foam::RBD::joints::Rzyx::jcalc
 (
     joint::XSvc& J,
     const scalarField& q,
-    const scalarField& w,
     const scalarField& qDot
 ) const
 {

src/rigidBodyDynamics/joints/Rzyx/Rzyx.H

@@ -93,7 +93,6 @@ public:
         (
             joint::XSvc& J,
             const scalarField& q,
-            const scalarField& w,
             const scalarField& qDot
         ) const;
 };

src/rigidBodyDynamics/joints/composite/compositeJoint.C

@@ -96,11 +96,10 @@ void Foam::RBD::joints::composite::jcalc
 (
     joint::XSvc& J,
     const scalarField& q,
-    const scalarField& w,
     const scalarField& qDot
 ) const
 {
-    last().jcalc(J, q, w, qDot);
+    last().jcalc(J, q, qDot);
 }
 
 

src/rigidBodyDynamics/joints/composite/compositeJoint.H

@@ -109,7 +109,6 @@ public:
         (
             joint::XSvc& J,
             const scalarField& q,
-            const scalarField& w,
             const scalarField& qDot
         ) const;
 

src/rigidBodyDynamics/joints/joint/joint.H

@@ -236,7 +236,6 @@ public:
         (
             XSvc& J,
             const scalarField& q,
-            const scalarField& w,
             const scalarField& qDot
         ) const = 0;
 
@@ -250,8 +249,7 @@ public:
         //  if it uses a quaternion representation for rotation
         inline quaternion operator()
         (
-            const scalarField& q,
+            const scalarField& q
-            const scalarField& w
         ) const;
 
         //- Set the state quaternion for this joint
@@ -259,8 +257,7 @@ public:
         inline void operator()
         (
             const quaternion& quat,
-            scalarField& q,
+            scalarField& q
-            scalarField& w
         ) const;
 
 

src/rigidBodyDynamics/joints/joint/jointI.H

@@ -71,8 +71,7 @@ inline const Foam::List<Foam::spatialVector>& Foam::RBD::joint::S() const
 
 inline Foam::quaternion Foam::RBD::joint::operator()
 (
-    const scalarField& q,
+    const scalarField& q
-    const scalarField& w
 ) const
 {
     if (nw() != 1)
@@ -82,15 +81,14 @@ inline Foam::quaternion Foam::RBD::joint::operator()
             << abort(FatalError);
     }
 
-    return quaternion(w[wIndex_], q.block<vector>(qIndex_));
+    return quaternion::unit(q.block<vector>(qIndex_));
 }
 
 
 inline void Foam::RBD::joint::operator()
 (
     const quaternion& quat,
-    scalarField& q,
+    scalarField& q
-    scalarField& w
 ) const
 {
     if (nw() != 1)
@@ -100,7 +98,6 @@ inline void Foam::RBD::joint::operator()
             << abort(FatalError);
     }
 
-    w[wIndex_] = quat.w();
     q[qIndex_] = quat.v().x();
     q[qIndex_+1] = quat.v().y();
     q[qIndex_+2] = quat.v().z();

src/rigidBodyDynamics/joints/null/nullJoint.C

@@ -80,7 +80,6 @@ void Foam::RBD::joints::null::jcalc
 (
     joint::XSvc& J,
     const scalarField& q,
-    const scalarField& w,
     const scalarField& qDot
 ) const
 {

src/rigidBodyDynamics/joints/null/nullJoint.H

@@ -92,7 +92,6 @@ public:
         (
             joint::XSvc& J,
             const scalarField& q,
-            const scalarField& w,
             const scalarField& qDot
         ) const;
 };

src/rigidBodyDynamics/rigidBodyModel/forwardDynamics.C

@@ -54,7 +54,6 @@ void Foam::RBD::rigidBodyModel::forwardDynamics
 ) const
 {
     const scalarField& q = state.q();
-    const scalarField& w = state.w();
     const scalarField& qDot = state.qDot();
     scalarField& qDdot = state.qDdot();
 
@@ -71,7 +70,7 @@ void Foam::RBD::rigidBodyModel::forwardDynamics
     for (label i=1; i<nBodies(); i++)
     {
         const joint& jnt = joints()[i];
-        jnt.jcalc(J, q, w, qDot);
+        jnt.jcalc(J, q, qDot);
 
         S_[i] = J.S;
         S1_[i] = J.S1;
@@ -208,7 +207,6 @@ void Foam::RBD::rigidBodyModel::forwardDynamicsCorrection
     DebugInFunction << endl;
 
     const scalarField& q = state.q();
-    const scalarField& w = state.w();
     const scalarField& qDot = state.qDot();
     const scalarField& qDdot = state.qDdot();
 
@@ -223,7 +221,7 @@ void Foam::RBD::rigidBodyModel::forwardDynamicsCorrection
         const joint& jnt = joints()[i];
         const label qi = jnt.qIndex();
 
-        jnt.jcalc(J, q, w, qDot);
+        jnt.jcalc(J, q, qDot);
 
         S_[i] = J.S;
         S1_[i] = J.S1;

src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelState.C

@@ -33,7 +33,6 @@ Foam::RBD::rigidBodyModelState::rigidBodyModelState
 )
 :
     q_(model.nDoF(), Zero),
-    w_(model.nw(), 1),
     qDot_(model.nDoF(), Zero),
     qDdot_(model.nDoF(), Zero)
 {}
@@ -46,7 +45,6 @@ Foam::RBD::rigidBodyModelState::rigidBodyModelState
 )
 :
     q_(dict.lookupOrDefault("q", scalarField(model.nDoF(), Zero))),
-    w_(dict.lookupOrDefault("w", scalarField(model.nw(), 1))),
     qDot_(dict.lookupOrDefault("qDot", scalarField(model.nDoF(), Zero))),
     qDdot_(dict.lookupOrDefault("qDdot", scalarField(model.nDoF(), Zero)))
 {}

src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelState.H

@@ -70,9 +70,6 @@ class rigidBodyModelState
         //- Joint position and orientation
         scalarField q_;
 
-        //- Joint quaternion
-        scalarField w_;
-
         //- Joint velocity
         scalarField qDot_;
 
@@ -103,7 +100,6 @@ public:
             inline const scalarField& q() const;
 
             //- Return access to the joint quaternion
-            inline const scalarField& w() const;
 
             //- Return access to the joint velocity
             inline const scalarField& qDot() const;
@@ -118,7 +114,6 @@ public:
             inline scalarField& q();
 
             //- Return access to the joint quaternion
-            inline scalarField& w();
 
             //- Return access to the joint velocity
             inline scalarField& qDot();

src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelStateI.H

@@ -31,12 +31,6 @@ inline const Foam::scalarField& Foam::RBD::rigidBodyModelState::q() const
 }
 
 
-inline const Foam::scalarField& Foam::RBD::rigidBodyModelState::w() const
-{
-    return w_;
-}
-
-
 inline const Foam::scalarField& Foam::RBD::rigidBodyModelState::qDot() const
 {
     return qDot_;
@@ -55,12 +49,6 @@ inline Foam::scalarField& Foam::RBD::rigidBodyModelState::q()
 }
 
 
-inline Foam::scalarField& Foam::RBD::rigidBodyModelState::w()
-{
-    return w_;
-}
-
-
 inline Foam::scalarField& Foam::RBD::rigidBodyModelState::qDot()
 {
     return qDot_;

src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelStateIO.C

@@ -31,7 +31,6 @@ License
 void Foam::RBD::rigidBodyModelState::write(dictionary& dict) const
 {
     dict.add("q", q_);
-    dict.add("w", w_);
     dict.add("qDot", qDot_);
     dict.add("qDdot", qDdot_);
 }
@@ -40,7 +39,6 @@ void Foam::RBD::rigidBodyModelState::write(dictionary& dict) const
 void Foam::RBD::rigidBodyModelState::write(Ostream& os) const
 {
     os.writeKeyword("q") << q_ << token::END_STATEMENT << nl;
-    os.writeKeyword("w") << w_ << token::END_STATEMENT << nl;
     os.writeKeyword("qDot") << qDot_ << token::END_STATEMENT << nl;
     os.writeKeyword("qDdot") << qDdot_ << token::END_STATEMENT << nl;
 }
@@ -55,7 +53,6 @@ Foam::Istream& Foam::RBD::operator>>
 )
 {
     is  >> state.q_
-        >> state.w_
         >> state.qDot_
         >> state.qDdot_;
 
@@ -77,7 +74,6 @@ Foam::Ostream& Foam::RBD::operator<<
 )
 {
     os  << token::SPACE << state.q_
-        << token::SPACE << state.w_
         << token::SPACE << state.qDot_
         << token::SPACE << state.qDdot_;
 

src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolver.C

@@ -76,11 +76,11 @@ void Foam::RBD::rigidBodySolver::correctQuaternionJoints()
                     // Transform the previous time quaternion
                     quaternion quat
                     (
-                        normalize(model_.joints()[i](q0(), w0())*dQuat)
+                        normalize(model_.joints()[i](q0())*dQuat)
                     );
 
                     // Update the joint state
-                    model_.joints()[i](quat, q(), w());
+                    model_.joints()[i](quat, q());
                 }
             }
         }

src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolver.H

@@ -75,7 +75,6 @@ protected:
         inline scalarField& q();
 
         //- Return the current joint quaternion
-        inline scalarField& w();
 
         //- Return the current joint velocity
         inline scalarField& qDot();
@@ -88,7 +87,6 @@ protected:
         inline const scalarField& q0() const;
 
         //- Return the current joint quaternion
-        inline const scalarField& w0() const;
 
         //- Return the current joint velocity
         inline const scalarField& qDot0() const;

src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolverI.H

@@ -56,12 +56,6 @@ inline Foam::scalarField& Foam::RBD::rigidBodySolver::q()
 }
 
 
-inline Foam::scalarField& Foam::RBD::rigidBodySolver::w()
-{
-    return state().w();
-}
-
-
 inline Foam::scalarField& Foam::RBD::rigidBodySolver::qDot()
 {
     return state().qDot();
@@ -80,12 +74,6 @@ inline const Foam::scalarField& Foam::RBD::rigidBodySolver::q0() const
 }
 
 
-inline const Foam::scalarField& Foam::RBD::rigidBodySolver::w0() const
-{
-    return state0().w();
-}
-
-
 inline const Foam::scalarField& Foam::RBD::rigidBodySolver::qDot0() const
 {
     return state0().qDot();