quaternionSixDOF using general constraints

src/ODE/sixDOF/geometricSixDOF/geometricSixDOF.C

@@ -27,9 +27,6 @@ Class
 Description
     6-DOF solver using a geometric method for integration of rotations.
 
-    Run-time selectable constraints are handled via Lagrangian multipliers using
-    the interface from sixDOFConstraint class.
-
 Author
     Viktor Pandza, FSB Zagreb.  All rights reserved.
     Vuko Vukcevic, FSB Zagreb.  All rights reserved.
@@ -107,7 +104,7 @@ Foam::dimensionedVector Foam::geometricSixDOF::A
         const translationalConstraint& curTc = translationalConstraints_[tcI];
 
         // Get matrix contribution from constraint
-        const vector mc = curTc.matrixContribution(t, R);
+        const vector mc = curTc.matrixContribution(t, R.T());
 
         // Get matrix index
         const label index = tcI + 3;
@@ -382,12 +379,12 @@ Foam::geometricSixDOF::geometricSixDOF(const IOobject& io)
     // Read rotation constraints if they are present
     if (dict().found("rotationalConstraints"))
     {
-        PtrList<rotationalConstraint> tcList
+        PtrList<rotationalConstraint> rcList
         (
             dict().lookup("rotationalConstraints"),
             rotationalConstraint::iNew()
         );
-        rotationalConstraints_.transfer(tcList);
+        rotationalConstraints_.transfer(rcList);
     }
 }
 
@@ -574,7 +571,6 @@ void Foam::geometricSixDOF::update(const scalar delta)
     Uval.y() = coeffs_[4];
     Uval.z() = coeffs_[5];
 
-    // Constrain velocity and re-set coefficients
     coeffs_[3] = Uval.x();
     coeffs_[4] = Uval.y();
     coeffs_[5] = Uval.z();
@@ -601,8 +597,6 @@ void Foam::geometricSixDOF::update(const scalar delta)
     coeffs_[10] = 0;
     coeffs_[11] = 0;
 
-    // Consider calculating average omega using rotational tensor and rotational
-    // increment tensors
     omegaAverage_.value() = 0.5*(omegaVal + omegaOld);
 }
 

src/ODE/sixDOF/geometricSixDOF/geometricSixDOF.H

@@ -28,7 +28,7 @@ Description
     6-DOF solver using a geometric method for integration of rotations.
 
     Run-time selectable constraints are handled via Lagrangian multipliers using
-    the interface from sixDOFConstraint class.
+    the interface from translationa/rotationalConstraint classes.
 
     Reference (bibtex):
 
@@ -44,7 +44,8 @@ Description
     }
 
     Note on convention: rotation tensor (R, or rotation_) defines
-    body-to-inertial coordinate system transformation (local-to-global).
+    body-to-inertial coordinate system transformation
+    (local-to-global). Opposite as in quaternionSixDOF.
 
 Author
     Viktor Pandza, FSB Zagreb.  All rights reserved.
@@ -104,8 +105,6 @@ class geometricSixDOF
             dimensionedVector omegaAverage_;
 
 
-        // ODE controls
-
         //- ODE coefficients
         scalarField coeffs_;
 
@@ -219,14 +218,14 @@ public:
 
         // Access member functions
 
-            //- Return const reference to translation constraints
+            //- Return const reference to translational constraints
             const PtrList<translationalConstraint>&
             translationalConstraints() const
             {
                 return translationalConstraints_;
             }
 
-            //- Return const reference to translation constraints
+            //- Return const reference to rotational constraints
             const PtrList<rotationalConstraint>&
             rotationalConstraints() const
             {

src/ODE/sixDOF/quaternionSixDOF/quaternionSixDOF.C

@@ -32,9 +32,6 @@ Author
     Hrvoje Jasak, FSB Zagreb.  All rights reserved.
     Vuko Vukcevic, FSB Zagreb.  All rights reserved.
 
-SourceFiles
-    quaternionSixDOF.C
-
 \*---------------------------------------------------------------------------*/
 
 #include "quaternionSixDOF.H"
@@ -57,43 +54,139 @@ Foam::dimensionedVector Foam::quaternionSixDOF::A
 (
     const dimensionedVector& xR,
     const dimensionedVector& uR,
-    const HamiltonRodriguezRot& rotation
+    const HamiltonRodriguezRot& rotation,
+    const scalar t
 ) const
 {
-    // Fix the total force in global coordinate system
+    // Create a scalar square matrix representing Newton equations with
-    dimensionedVector fAbs =
+    // constraints and the corresponding source (right hand side vector).
-        // External force
+    // Note: size of the matrix is 3 + number of constraints
-        force()
+    scalarField rhs(translationalConstraints_.size() + 3, 0.0);
-        // Spring force in global coordinate system
+    scalarSquareMatrix M(rhs.size(), 0.0);
-      - (linSpringCoeffs() & xR)
-        // Damping force in global coordinate system
-      - (linDampingCoeffs() & uR);
 
-    // Constrain translation
+    // Insert mass and explicit forcing into the system. Note: translations are
-    constrainTranslation(fAbs.value());
+    // solved in the global coordinate system
+    const dimensionedVector explicitForcing
+    (
+        force() // External force
+      - (linSpringCoeffs() & xR) // Spring force
+      - (linDampingCoeffs() & uR) // Damping force
+    );
+    const vector& efVal = explicitForcing.value();
+    const scalar& m = mass().value();
 
-    return fAbs/mass();
+    forAll(efVal, dirI)
+    {
+        M[dirI][dirI] = m;
+
+        rhs[dirI] = efVal[dirI];
+    }
+
+    // Insert contributions from the constraints
+    forAll(translationalConstraints_, tcI)
+    {
+        // Get reference to current constraint
+        const translationalConstraint& curTc = translationalConstraints_[tcI];
+
+        // Get matrix contribution from constraint
+        const vector mc = curTc.matrixContribution(t, rotation.R());
+
+        // Get matrix index
+        const label index = tcI + 3;
+
+        // Insert contributions into the matrix
+        forAll(mc, dirI)
+        {
+            M[dirI][index] = mc[dirI];
+            M[index][dirI] = mc[dirI];
+        }
+
+        // Insert source contribution (remainder of the constraint function)
+        rhs[index] = curTc.sourceContribution(t);
+    }
+
+    // Solve the matrix using LU decomposition. Note: solution is in the rhs and
+    // it contains accelerations in the first three entries and corresponding
+    // Lagrangian multipliers in other entries.
+    scalarSquareMatrix::LUsolve(M, rhs);
+
+    return
+        dimensionedVector
+        (
+            "A",
+            force().dimensions()/mass().dimensions(),
+            vector(rhs[0], rhs[1], rhs[2])
+        );
 }
 
 
 Foam::dimensionedVector Foam::quaternionSixDOF::OmegaDot
 (
     const HamiltonRodriguezRot& rotation,
-    const dimensionedVector& omega
+    const dimensionedVector& omega,
+    const scalar t
 ) const
 {
-    // Fix the global moment for global rotation constraints
+    // Create a scalar square matrix representing Euler equations with
-    dimensionedVector mAbs = moment();
+    // constraints and the corresponding source (right hand side vector).
+    // Note: size of the matrix is 3 + number of constraints
+    scalarField rhs(rotationalConstraints_.size() + 3, 0.0);
+    scalarSquareMatrix J(rhs.size(), 0.0);
 
-    // Constrain rotation
+    // Get current inertial-to-local transformation. Note: different convention
-    constrainRotation(mAbs.value());
+    // (R represents coordinate transformation from global to local)
+    const dimensionedTensor R("R", dimless, rotation.R());
+
+    // Insert moment of inertia and explicit forcing into the system
+    const dimensionedVector explicitForcing
+    (
+        E(omega) // Euler part
+      + (R & moment()) // External torque
+    );
+    const vector& efVal = explicitForcing.value();
+    const diagTensor& I = momentOfInertia().value();
+
+    forAll(efVal, dirI)
+    {
+        J[dirI][dirI] = I[dirI];
+
+        rhs[dirI] = efVal[dirI];
+    }
+
+    // Insert contributions from the constraints
+    forAll(rotationalConstraints_, rcI)
+    {
+        // Get reference to current constraint
+        const rotationalConstraint& curRc = rotationalConstraints_[rcI];
+
+        // Get matrix contribution from the constraint
+        const vector mc = curRc.matrixContribution(t, R.value());
+
+        // Get matrix index
+        const label index = rcI + 3;
+
+        // Insert contributions into the matrix
+        forAll(mc, dirI)
+        {
+            J[dirI][index] = mc[dirI];
+            J[index][dirI] = mc[dirI];
+        }
+
+        // Insert source contribution (remainder of the constraint function)
+        rhs[index] = curRc.sourceContribution(t);
+    }
+
+    // Solve the matrix using LU decomposition. Note: solution is in the rhs and
+    // it contains OmegaDot's in the first three entries and corresponding
+    // Lagrangian multipliers in other entries.
+    scalarSquareMatrix::LUsolve(J, rhs);
 
     return
-        inv(momentOfInertia())
+        dimensionedVector
-      & (
+        (
-            E(omega)
+            "OmegaDot",
-            // To relative
+            moment().dimensions()/momentOfInertia().dimensions(),
-          + (rotation.R() & mAbs)
+            vector(rhs[0], rhs[1], rhs[2])
         );
 }
 
@@ -107,86 +200,6 @@ Foam::dimensionedVector Foam::quaternionSixDOF::E
 }
 
 
-void Foam::quaternionSixDOF::constrainRotation(vector& vec) const
-{
-    // Constrain the vector with respect to referent or global coordinate system
-    if (referentMotionConstraints_)
-    {
-        vector consVec(referentRotation_.R() & vec);
-
-        if (fixedRoll_)
-        {
-            consVec.x() = 0;
-        }
-        if (fixedPitch_)
-        {
-            consVec.y() = 0;
-        }
-        if (fixedYaw_)
-        {
-            consVec.z() = 0;
-        }
-
-        vec = referentRotation_.invR() & consVec;
-    }
-    else
-    {
-        if (fixedRoll_)
-        {
-            vec.x() = 0;
-        }
-        if (fixedPitch_)
-        {
-            vec.y() = 0;
-        }
-        if (fixedYaw_)
-        {
-            vec.z() = 0;
-        }
-    }
-}
-
-
-void Foam::quaternionSixDOF::constrainTranslation(vector& vec) const
-{
-    // Constrain the vector in respect to referent or global coordinate system
-    if (referentMotionConstraints_)
-    {
-        vector consVec(referentRotation_.R() & vec);
-
-        if (fixedSurge_)
-        {
-            consVec.x() = 0;
-        }
-        if (fixedSway_)
-        {
-            consVec.y() = 0;
-        }
-        if (fixedHeave_)
-        {
-            consVec.z() = 0;
-        }
-
-        vec = referentRotation_.invR() & consVec;
-    }
-    else
-    {
-        if (fixedSurge_)
-        {
-            vec.x() = 0;
-        }
-        if (fixedSway_)
-        {
-            vec.y() = 0;
-        }
-        if (fixedHeave_)
-        {
-            vec.z() = 0;
-        }
-    }
-}
-
-
 // * * * * * * * * * * * Protected Member Functions * * * * * * * * * * * * * //
 
 void Foam::quaternionSixDOF::setState(const sixDOFODE& sd)
@@ -204,20 +217,14 @@ void Foam::quaternionSixDOF::setState(const sixDOFODE& sd)
     rotation_ = qsd.rotation_;
     omega_ = qsd.omega_;
     omegaAverage_ = qsd.omegaAverage_;
-    omegaAverageAbsolute_ = qsd.omegaAverageAbsolute_;
 
     // Copy ODE coefficients: this carries actual ODE state
     // HJ, 23/Mar/2015
     coeffs_ = qsd.coeffs_;
 
-    fixedSurge_ = qsd.fixedSurge_;
+    // Copy constraints
-    fixedSway_ = qsd.fixedSway_;
+    translationalConstraints_ = qsd.translationalConstraints_;
-    fixedHeave_ = qsd.fixedHeave_;
+    rotationalConstraints_ = qsd.rotationalConstraints_;
-    fixedRoll_ = qsd.fixedRoll_;
-    fixedPitch_ = qsd.fixedPitch_;
-    fixedYaw_ = qsd.fixedYaw_;
-    referentMotionConstraints_ = qsd.referentMotionConstraints_;
-    referentRotation_ = qsd.referentRotation_;
 }
 
 
@@ -237,25 +244,11 @@ Foam::quaternionSixDOF::quaternionSixDOF(const IOobject& io)
     ),
     omega_(dict().lookup("omega")),
     omegaAverage_("omegaAverage", omega_),
-    omegaAverageAbsolute_("omegaAverageAbsolute", omega_),
 
     coeffs_(13, 0.0),
 
-    fixedSurge_(dict().lookup("fixedSurge")),
+    translationalConstraints_(),
-    fixedSway_(dict().lookup("fixedSway")),
+    rotationalConstraints_()
-    fixedHeave_(dict().lookup("fixedHeave")),
-    fixedRoll_(dict().lookup("fixedRoll")),
-    fixedPitch_(dict().lookup("fixedPitch")),
-    fixedYaw_(dict().lookup("fixedYaw")),
-    referentMotionConstraints_
-    (
-        dict().lookupOrDefault<Switch>
-        (
-            "referentMotionConstraints",
-            false
-        )
-    ),
-    referentRotation_(vector(1, 0, 0), 0)
 {
     // Set ODE coefficients from position and rotation
 
@@ -282,6 +275,30 @@ Foam::quaternionSixDOF::quaternionSixDOF(const IOobject& io)
     coeffs_[10] = rotation_.eInitial().e1();
     coeffs_[11] = rotation_.eInitial().e2();
     coeffs_[12] = rotation_.eInitial().e3();
+
+    // Read and construct constraints
+
+    // Read translation constraints if they are present
+    if (dict().found("translationalConstraints"))
+    {
+        PtrList<translationalConstraint> tcList
+        (
+            dict().lookup("translationalConstraints"),
+            translationalConstraint::iNew()
+        );
+        translationalConstraints_.transfer(tcList);
+    }
+
+    // Read rotation constraints if they are present
+    if (dict().found("rotationalConstraints"))
+    {
+        PtrList<rotationalConstraint> rcList
+        (
+            dict().lookup("rotationalConstraints"),
+            rotationalConstraint::iNew()
+        );
+        rotationalConstraints_.transfer(rcList);
+    }
 }
 
 
@@ -299,22 +316,11 @@ Foam::quaternionSixDOF::quaternionSixDOF
     rotation_(qsd.rotation_),
     omega_(qsd.omega_.name(), qsd.omega_),
     omegaAverage_(qsd.omegaAverage_.name(), qsd.omegaAverage_),
-    omegaAverageAbsolute_
-    (
-        qsd.omegaAverageAbsolute_.name(),
-        qsd.omegaAverageAbsolute_
-    ),
 
     coeffs_(qsd.coeffs_),
 
-    fixedSurge_(qsd.fixedSurge_),
+    translationalConstraints_(qsd.translationalConstraints_),
-    fixedSway_(qsd.fixedSway_),
+    rotationalConstraints_(qsd.rotationalConstraints_)
-    fixedHeave_(qsd.fixedHeave_),
-    fixedRoll_(qsd.fixedRoll_),
-    fixedPitch_(qsd.fixedPitch_),
-    fixedYaw_(qsd.fixedYaw_),
-    referentMotionConstraints_(qsd.referentMotionConstraints_),
-    referentRotation_(qsd.referentRotation_)
 {}
 
 
@@ -420,7 +426,7 @@ void Foam::quaternionSixDOF::derivatives
         y[12]
     );
 
-    const vector accel = A(curX, curU, curRotation).value();
+    const vector accel = A(curX, curU, curRotation, x).value();
 
     dydx[3] = accel.x();
     dydx[4] = accel.y();
@@ -434,7 +440,7 @@ void Foam::quaternionSixDOF::derivatives
         vector(y[6], y[7], y[8])
     );
 
-    const vector omegaDot = OmegaDot(curRotation, curOmega).value();
+    const vector omegaDot = OmegaDot(curRotation, curOmega, x).value();
 
     dydx[6] = omegaDot.x();
     dydx[7] = omegaDot.y();
@@ -444,26 +450,14 @@ void Foam::quaternionSixDOF::derivatives
     dydx[10] = curRotation.eDot(curOmega.value(), 1);
     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)
-    }
 }
 
 
 void Foam::quaternionSixDOF::update(const scalar delta)
 {
-    // Update position
+    // Translation
+
+    // Update displacement
     vector Xold = Xrel_.value();
 
     vector& Xval = Xrel_.value();
@@ -481,12 +475,12 @@ void Foam::quaternionSixDOF::update(const scalar delta)
     Uval.y() = coeffs_[4];
     Uval.z() = coeffs_[5];
 
-    // Constrain velocity
-    constrainTranslation(Uval);
     coeffs_[3] = Uval.x();
     coeffs_[4] = Uval.y();
     coeffs_[5] = Uval.z();
 
+    // Rotation
+
     // Update omega
     vector& omegaVal = omega_.value();
 
@@ -494,8 +488,6 @@ void Foam::quaternionSixDOF::update(const scalar delta)
     omegaVal.y() = coeffs_[7];
     omegaVal.z() = coeffs_[8];
 
-    // Constrain omega
-    constrainRotation(omegaVal);
     coeffs_[6] = omegaVal.x();
     coeffs_[7] = omegaVal.y();
     coeffs_[8] = omegaVal.z();
@@ -512,23 +504,6 @@ void Foam::quaternionSixDOF::update(const scalar delta)
     );
 
     omegaAverage_.value() = rotation_.omegaAverage(delta);
-
-    // Calculate and constrain omegaAverageAbsolute appropriately
-    vector& omegaAverageAbsoluteValue = omegaAverageAbsolute_.value();
-    omegaAverageAbsoluteValue = rotation_.omegaAverageAbsolute(delta);
-
-    if (fixedRoll_)
-    {
-        omegaAverageAbsoluteValue.x() = 0;
-    }
-    if (fixedPitch_)
-    {
-        omegaAverageAbsoluteValue.y() = 0;
-    }
-    if (fixedYaw_)
-    {
-        omegaAverageAbsoluteValue.z() = 0;
-    }
 }
 
 
@@ -553,18 +528,19 @@ bool Foam::quaternionSixDOF::writeData(Ostream& os) const
     os.writeKeyword("omega") << tab << omega_
         << token::END_STATEMENT << nl << nl;
 
-    os.writeKeyword("fixedSurge") << tab << fixedSurge_ <<
+    if (!translationalConstraints_.empty())
-        token::END_STATEMENT << nl;
+    {
-    os.writeKeyword("fixedSway") << tab << fixedSway_ <<
+        os.writeKeyword("translationalConstraints")
-        token::END_STATEMENT << nl;
+            << translationalConstraints_
-    os.writeKeyword("fixedHeave") << tab << fixedHeave_ <<
+            << token::END_STATEMENT << nl << endl;
-        token::END_STATEMENT << nl;
+    }
-    os.writeKeyword("fixedRoll") << tab << fixedRoll_ <<
+
-        token::END_STATEMENT << nl;
+    if (!rotationalConstraints_.empty())
-    os.writeKeyword("fixedPitch") << tab << fixedPitch_ <<
+    {
-        token::END_STATEMENT << nl;
+        os.writeKeyword("rotationalConstraints")
-    os.writeKeyword("fixedYaw") << tab << fixedYaw_ <<
+            << rotationalConstraints_
-        token::END_STATEMENT << nl << endl;
+            << token::END_STATEMENT << endl;
+    }
 
     return os.good();
 }

src/ODE/sixDOF/quaternionSixDOF/quaternionSixDOF.H

@@ -27,6 +27,13 @@ Class
 Description
     6-DOF solver using quaternions
 
+    Run-time selectable constraints are handled via Lagrangian multipliers using
+    the interface from translationa/rotationalConstraint classes.
+
+    Note on convention: rotation tensor R obtained from finiteRotation
+    (rotation_) defines inertial-to-body coordinate system transformation
+    (global-to-local). Opposite as in geometrixSixDOF.
+
 Author
     Dubravko Matijasevic, FSB Zagreb.  All rights reserved.
     Hrvoje Jasak, FSB Zagreb.  All rights reserved.
@@ -42,6 +49,8 @@ SourceFiles
 
 #include "sixDOFODE.H"
 #include "finiteRotation.H"
+#include "translationalConstraint.H"
+#include "rotationalConstraint.H"
 
 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
 
@@ -79,39 +88,18 @@ class quaternionSixDOF
             //  (evaluated at midstep)
             dimensionedVector omegaAverage_;
 
-            //- Average rotational velocity in absolute coordinate system
-            dimensionedVector omegaAverageAbsolute_;
-
 
         //- ODE coefficients
         scalarField coeffs_;
 
 
-        // Motion constraints (given as fixed motion components)
+        // Motion constraints
 
-            //- Fixed surge (x-translation)
+            //- List of translational constraints
-            Switch fixedSurge_;
+            PtrList<translationalConstraint> translationalConstraints_;
 
-            //- Fixed sway (y-translation)
+            //- List of rotational constraints
-            Switch fixedSway_;
+            PtrList<rotationalConstraint> rotationalConstraints_;
-
-            //- Fixed heave (z-translation)
-            Switch fixedHeave_;
-
-            //- Fixed roll (rotation around x)
-            Switch fixedRoll_;
-
-            //- Fixed pitch (rotation around y)
-            Switch fixedPitch_;
-
-            //- Fixed yaw (rotation around z)
-            Switch fixedYaw_;
-
-            //- Constraints in referent coordinate system
-            Switch referentMotionConstraints_;
-
-            //- Rotation of referent coordinate system
-            HamiltonRodriguezRot referentRotation_;
 
 
     // Private Member Functions
@@ -131,7 +119,8 @@ class quaternionSixDOF
             (
                 const dimensionedVector& xR,
                 const dimensionedVector& uR,
-                const HamiltonRodriguezRot& rotation
+                const HamiltonRodriguezRot& rotation,
+                const scalar t
             ) const;
 
 
@@ -140,7 +129,8 @@ class quaternionSixDOF
             dimensionedVector OmegaDot
             (
                 const HamiltonRodriguezRot& rotation,
-                const dimensionedVector& omega
+                const dimensionedVector& omega,
+                const scalar t
             ) const;
 
             //- Return the Euler part of moment equation
@@ -149,14 +139,6 @@ class quaternionSixDOF
                 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;
-
 
 protected:
 
@@ -197,6 +179,23 @@ public:
 
     // Member Functions
 
+        // Access member functions
+
+            //- Return const reference to translational constraints
+            const PtrList<translationalConstraint>&
+            translationalConstraints() const
+            {
+                return translationalConstraints_;
+            }
+
+            //- Return const reference to rotational constraints
+            const PtrList<rotationalConstraint>&
+            rotationalConstraints() const
+            {
+                return rotationalConstraints_;
+            }
+
+
         // Virtual interface for 6DOF motion state
 
             // Variables in relative coordinate system