From 64df5382639cba452aaea3fe244604379af7083b Mon Sep 17 00:00:00 2001
From: Hrvoje Jasak <h.jasak@wikki.co.uk>
Date: Wed, 20 Jan 2016 16:31:51 +0000
Subject: [PATCH] Bugfix: specified motion and 6-DOF constraints

Conflicts:
	src/ODE/sixDOF/sixDOFqODE/sixDOFqODE.C
	src/ODE/sixDOF/sixDOFqODE/sixDOFqODE.H
---
 src/ODE/sixDOF/sixDOFqODE/sixDOFqODE.C | 50 ++++++++++++++------------
 src/ODE/sixDOF/sixDOFqODE/sixDOFqODE.H | 11 ------
 2 files changed, 27 insertions(+), 34 deletions(-)

diff --git a/src/ODE/sixDOF/sixDOFqODE/sixDOFqODE.C b/src/ODE/sixDOF/sixDOFqODE/sixDOFqODE.C
index fe292bf08..86f1647d7 100644
--- a/src/ODE/sixDOF/sixDOFqODE/sixDOFqODE.C
+++ b/src/ODE/sixDOF/sixDOFqODE/sixDOFqODE.C
@@ -76,9 +76,10 @@ Foam::dimensionedVector Foam::sixDOFqODE::A
 {
     // Fix the global force for global rotation constraints
     dimensionedVector fAbs = force();
+    vector& fAbsVal = fAbs.value();
 
-    // Constrain translation
-    constrainTranslation(fAbs.value());
+    // Constrain rotation
+    fAbsVal = constrainTranslation(fAbsVal);
 
     return
     (
@@ -99,9 +100,9 @@ Foam::dimensionedVector Foam::sixDOFqODE::OmegaDot
 {
     // Fix the global moment for global rotation constraints
     dimensionedVector mAbs = moment();
+    vector& mAbsVal = mAbs.value();
 
-    // Constrain rotation
-    constrainRotation(mAbs.value());
+    mAbsVal = constrainRotation(mAbsVal);
 
     return
         inv(momentOfInertia_)
@@ -123,7 +124,7 @@ Foam::dimensionedVector Foam::sixDOFqODE::E
 }
 
 
-void Foam::sixDOFqODE::constrainRotation(vector& vec) const
+Foam::vector Foam::sixDOFqODE::constrainRotation(vector& vec) const
 {
     vector consVec(vector::zero);
 
@@ -164,10 +165,12 @@ void Foam::sixDOFqODE::constrainRotation(vector& vec) const
             consVec.z() = 0;
         }
     }
+
+    return consVec;
 }
 
 
-void Foam::sixDOFqODE::constrainTranslation(vector& vec) const
+Foam::vector Foam::sixDOFqODE::constrainTranslation(vector& vec) const
 {
     vector consVec(vector::zero);
 
@@ -208,6 +211,8 @@ void Foam::sixDOFqODE::constrainTranslation(vector& vec) const
             consVec.z() = 0;
         }
     }
+
+    return consVec;
 }
 
 
@@ -258,7 +263,7 @@ Foam::sixDOFqODE::sixDOFqODE(const IOobject& io)
             false
         )
     ),
-    referentRotation_(vector(1, 0, 0), 0)
+    referentRotation_(vector::zero, 0)
 {
     setCoeffs();
 }
@@ -447,20 +452,19 @@ void Foam::sixDOFqODE::derivatives
     dydx[11] = curRotation.eDot(curOmega.value(), 2);
     dydx[12] = curRotation.eDot(curOmega.value(), 3);
 
-
-//    // Add rotational constraints by setting RHS of given components to zero
-//    if (fixedRoll_)
-//    {
-//        dydx[10] = 0; // Roll axis (roll quaternion evolution RHS)
-//    }
-//    if (fixedPitch_)
-//    {
-//        dydx[11] = 0; // Pitch axis (pitch quaternion evolution RHS)
-//    }
-//    if (fixedYaw_)
-//    {
-//        dydx[12] = 0; // Yaw axis (yaw quaternion evolution RHS)
-//    }
+    // Add rotational constraints by setting RHS of given components to zero
+    if (fixedRoll_)
+    {
+        dydx[10] = 0; // Roll axis (roll quaternion evolution RHS)
+    }
+    if (fixedPitch_)
+    {
+        dydx[11] = 0; // Pitch axis (pitch quaternion evolution RHS)
+    }
+    if (fixedYaw_)
+    {
+        dydx[12] = 0; // Yaw axis (yaw quaternion evolution RHS)
+    }
 }
 
 
@@ -498,7 +502,7 @@ void Foam::sixDOFqODE::update(const scalar delta)
     Uval.z() = coeffs_[5];
 
     // Constrain velocity
-    constrainTranslation(Uval);
+    Uval = constrainTranslation(Uval);
     coeffs_[3] = Uval.x();
     coeffs_[4] = Uval.y();
     coeffs_[5] = Uval.z();
@@ -511,7 +515,7 @@ void Foam::sixDOFqODE::update(const scalar delta)
     omegaVal.z() = coeffs_[8];
 
     // Constrain omega
-    constrainRotation(omegaVal);
+    omegaVal = constrainRotation(omegaVal);
     coeffs_[6] = omegaVal.x();
     coeffs_[7] = omegaVal.y();
     coeffs_[8] = omegaVal.z();
diff --git a/src/ODE/sixDOF/sixDOFqODE/sixDOFqODE.H b/src/ODE/sixDOF/sixDOFqODE/sixDOFqODE.H
index e61f54f87..a6caf36eb 100644
--- a/src/ODE/sixDOF/sixDOFqODE/sixDOFqODE.H
+++ b/src/ODE/sixDOF/sixDOFqODE/sixDOFqODE.H
@@ -197,14 +197,6 @@ class sixDOFqODE
                 const dimensionedVector& omega
             ) const;
 
-            //- Constrain rotation vector in referent or global coordinate
-            //  system
-            void constrainRotation(vector& vec) const;
-
-            //- Constrain translation vector in referent or global coordinate
-            //  system
-            void constrainTranslation(vector& vec) const;
-
 
 public:
 
@@ -300,9 +292,6 @@ public:
             //  coordinate system
             inline void setOmega(const vector& omega);
 
-            //- Set referent coordinate system to apply constraints
-            inline void setReferentRotation(const HamiltonRodriguezRot& rot);
-
 
         // Average motion per time-step