rigidBodyDynamics/rigidBodySolvers: Added support for the integration of quaternion joints

src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelState.C

@@ -33,7 +33,7 @@ Foam::RBD::rigidBodyModelState::rigidBodyModelState
 )
 :
     q_(model.nDoF(), Zero),
-    w_(model.nw(), Zero),
+    w_(model.nw(), 1),
     qDot_(model.nDoF(), Zero),
     qDdot_(model.nDoF(), Zero)
 {}
@@ -46,7 +46,7 @@ Foam::RBD::rigidBodyModelState::rigidBodyModelState
 )
 :
     q_(dict.lookupOrDefault("q", scalarField(model.nDoF(), Zero))),
-    w_(dict.lookupOrDefault("w", scalarField(model.nw(), 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/rigidBodyMotion/rigidBodyMotion.H

@@ -137,6 +137,9 @@ public:
             //- Return the motion state
             inline const rigidBodyModelState& state() const;
 
+            //- Return the motion state for modification
+            inline rigidBodyModelState& state();
+
 
         // Edit
 

src/rigidBodyDynamics/rigidBodyMotion/rigidBodyMotionI.H

@@ -38,6 +38,13 @@ Foam::RBD::rigidBodyMotion::state() const
 }
 
 
+inline Foam::RBD::rigidBodyModelState&
+Foam::RBD::rigidBodyMotion::state()
+{
+    return motionState_;
+}
+
+
 inline void Foam::RBD::rigidBodyMotion::newTime()
 {
     motionState0_ = motionState_;

src/rigidBodyDynamics/rigidBodySolvers/CrankNicolson/CrankNicolson.C

@@ -82,6 +82,8 @@ void Foam::RBD::rigidBodySolvers::CrankNicolson::solve
     // Correct position
     q() = q0() + deltaT()*(voc_*qDot() + (1 - voc_)*qDot0());
 
+    correctQuaternionJoints();
+
     // Update the body-state
     model_.forwardDynamicsCorrection(state());
 }

src/rigidBodyDynamics/rigidBodySolvers/Newmark/Newmark.C

@@ -92,6 +92,8 @@ void Foam::RBD::rigidBodySolvers::Newmark::solve
       + deltaT()*qDot0()
       + sqr(deltaT())*beta_*qDdot() + sqr(deltaT())*(0.5 - beta_)*qDdot0();
 
+    correctQuaternionJoints();
+
     // Update the body-state
     model_.forwardDynamicsCorrection(state());
 }

src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolver.C

@@ -52,4 +52,40 @@ Foam::RBD::rigidBodySolver::~rigidBodySolver()
 {}
 
 
+// * * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * //
+
+void Foam::RBD::rigidBodySolver::correctQuaternionJoints()
+{
+    if (model_.nw())
+    {
+        forAll (model_.joints(), i)
+        {
+            const label qi = model_.joints()[i].qIndex();
+
+            if (model_.joints()[i].nw() == 1)
+            {
+                // Calculate the change in the normalized quaternion axis
+                vector dv((q().block<vector>(qi) - q0().block<vector>(qi)));
+                scalar magDv = mag(dv);
+
+                if (magDv > SMALL)
+                {
+                    // Calculate the quaternion corresponding to the change
+                    quaternion dQuat(dv/magDv, cos(magDv), true);
+
+                    // Transform the previous time quaternion
+                    quaternion quat
+                    (
+                        normalize(model_.joints()[i](q0(), w0())*dQuat)
+                    );
+
+                    // Update the joint state
+                    model_.joints()[i](quat, q(), w());
+                }
+            }
+        }
+    }
+}
+
+
 // ************************************************************************* //

src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolver.H

@@ -100,6 +100,9 @@ protected:
         //- Acceleration damping coefficient (for steady-state simulations)
         inline scalar aDamp() const;
 
+        //- Correct the quaternion joints based on the current change in q
+        void correctQuaternionJoints();
+
 
 public:
 

src/rigidBodyDynamics/rigidBodySolvers/symplectic/symplectic.C

@@ -73,6 +73,8 @@ void Foam::RBD::rigidBodySolvers::symplectic::solve
     qDot() = qDot0() + aDamp()*0.5*deltaT0()*qDdot();
     q() = q0() + deltaT()*qDot();
 
+    correctQuaternionJoints();
+
     // Update the body-state prior to the evaluation of the restraints
     model_.forwardDynamicsCorrection(state());