Added stabilisation for constraints

Note: necessary for arbitrary DAE systems

src/ODE/sixDOF/geometricSixDOF/geometricSixDOF.C

@@ -536,35 +536,52 @@ void Foam::geometricSixDOF::update(const scalar delta)
 {
     // Translation
 
-    // Update displacement
+    // Get displacement
     const vector Xold = Xrel_.value();
 
     vector& Xval = Xrel_.value();
-
     Xval.x() = coeffs_[0];
     Xval.y() = coeffs_[1];
     Xval.z() = coeffs_[2];
 
-    // Update velocity
-    Uaverage_.value() = (Xval - Xold)/delta;
-
+    // Get velocity
     vector& Uval = U_.value();
-
     Uval.x() = coeffs_[3];
     Uval.y() = coeffs_[4];
     Uval.z() = coeffs_[5];
 
+    // Stabilise translational constraints if necessary
+    forAll(translationalConstraints(), tcI)
+    {
+        // Note: get end time for this ODE step from mesh, assuming that the top
+        // level solves this ode from [t - deltaT, t], yielding solution at
+        // t. Done this way to preserve the interface of ODE class.
+        // VV, 10/Mar/2017.
+        const scalar t = dict().time().value();
+
+        translationalConstraints()[tcI].stabilise(t, Xval, Uval);
+    }
+
+    // Update (possibly constrained) displacement
+    coeffs_[0] = Xval.x();
+    coeffs_[1] = Xval.y();
+    coeffs_[2] = Xval.z();
+
+    // Update (possibly constrained) velocity
     coeffs_[3] = Uval.x();
     coeffs_[4] = Uval.y();
     coeffs_[5] = Uval.z();
 
+    // Update average velocity
+    Uaverage_.value() = (Xval - Xold)/delta;
+
+
     // Rotation
 
-    // Update omega
+    // Get angular velocity
     const vector omegaOld = omega_.value();
 
     vector& omegaVal = omega_.value();
-
     omegaVal.x() = coeffs_[6];
     omegaVal.y() = coeffs_[7];
     omegaVal.z() = coeffs_[8];
@@ -575,11 +592,29 @@ void Foam::geometricSixDOF::update(const scalar delta)
     // Update rotational tensor
     rotation_ = (rotation_ & rotIncrement_);
 
+    // Stabilise rotational constraints if necessary
+    forAll(rotationalConstraints(), rcI)
+    {
+        // Note: get end time for this ODE step from mesh, assuming that the top
+        // level solves this ode from [t - deltaT, t], yielding solution at
+        // t. Done this way to preserve the interface of ODE class.
+        // VV, 10/Mar/2017.
+        const scalar t = dict().time().value();
+
+        rotationalConstraints()[rcI].stabilise(t, omegaVal);
+    }
+
+    // Update (possibly constrained) omega
+    coeffs_[6] = omegaVal.x();
+    coeffs_[7] = omegaVal.y();
+    coeffs_[8] = omegaVal.z();
+
     // Reset increment vector in coefficients for the next step
     coeffs_[9] = 0;
     coeffs_[10] = 0;
     coeffs_[11] = 0;
 
+    // Update average omega
     omegaAverage_.value() = 0.5*(omegaVal + omegaOld);
 }
 

src/ODE/sixDOF/quaternionSixDOF/quaternionSixDOF.C

@@ -447,40 +447,54 @@ void Foam::quaternionSixDOF::update(const scalar delta)
 {
     // Translation
 
-    // Update displacement
-    vector Xold = Xrel_.value();
+    // Get displacement
+    const vector Xold = Xrel_.value();
 
     vector& Xval = Xrel_.value();
-
     Xval.x() = coeffs_[0];
     Xval.y() = coeffs_[1];
     Xval.z() = coeffs_[2];
 
-    // Update velocity
-    Uaverage_.value() = (Xval - Xold)/delta;
-
+    // Get velocity
     vector& Uval = U_.value();
-
     Uval.x() = coeffs_[3];
     Uval.y() = coeffs_[4];
     Uval.z() = coeffs_[5];
 
+    // Stabilise translational constraints if necessary
+    forAll(translationalConstraints(), tcI)
+    {
+        // Note: get end time for this ODE step from mesh, assuming that the top
+        // level solves this ode from [t - deltaT, t], yielding solution at
+        // t. Done this way to preserve the interface of ODE class.
+        // VV, 10/Mar/2017.
+        const scalar t = dict().time().value();
+
+        translationalConstraints()[tcI].stabilise(t, Xval, Uval);
+    }
+
+    // Update (possibly constrained) displacement
+    coeffs_[0] = Xval.x();
+    coeffs_[1] = Xval.y();
+    coeffs_[2] = Xval.z();
+
+    // Update (possibly constrained) velocity
     coeffs_[3] = Uval.x();
     coeffs_[4] = Uval.y();
     coeffs_[5] = Uval.z();
 
+    // Update average velocity
+    Uaverage_.value() = (Xval - Xold)/delta;
+
+
     // Rotation
 
-    // Update omega
+    // Get angular velocity
     vector& omegaVal = omega_.value();
-
     omegaVal.x() = coeffs_[6];
     omegaVal.y() = coeffs_[7];
     omegaVal.z() = coeffs_[8];
 
-    coeffs_[6] = omegaVal.x();
-    coeffs_[7] = omegaVal.y();
-    coeffs_[8] = omegaVal.z();
 
     rotation_.updateRotation
     (
@@ -493,6 +507,24 @@ void Foam::quaternionSixDOF::update(const scalar delta)
         )
     );
 
+    // Stabilise rotational constraints if necessary
+    forAll(rotationalConstraints(), rcI)
+    {
+        // Note: get end time for this ODE step from mesh, assuming that the top
+        // level solves this ode from [t - deltaT, t], yielding solution at
+        // t. Done this way to preserve the interface of ODE class.
+        // VV, 10/Mar/2017.
+        const scalar t = dict().time().value();
+
+        rotationalConstraints()[rcI].stabilise(t, omegaVal);
+    }
+
+    // Update (possibly constrained) omega
+    coeffs_[6] = omegaVal.x();
+    coeffs_[7] = omegaVal.y();
+    coeffs_[8] = omegaVal.z();
+
+    // Update average omega
     omegaAverage_.value() = rotation_.omegaAverage(delta);
 }
 

src/ODE/sixDOF/sixDOFODE/constraints/rotationalConstraints/constantAngularAcceleration/constantAngularAcceleration.H

@@ -113,6 +113,16 @@ public:
                 const vector&
             ) const;
 
+            //- Stabilise the constraint
+            virtual void stabilise
+            (
+                const scalar,
+                vector&
+            ) const
+            {
+                // Does nothing: no need to stabilise this constraint
+            }
+
 
     // I-O Functions
 

src/ODE/sixDOF/sixDOFODE/constraints/rotationalConstraints/rotationalConstraint/rotationalConstraint.H

@@ -41,8 +41,12 @@ Description
     2. sourceContribution() - corresponding to a(t), to be inserted into right
                               hand side vector.
 
-    Note: constraints are usually used alongside appropriate initial
-    conditions (rotational rate in a given direction).
+    Notes:
+    1. Constraints are usually used alongside appropriate initial
+       conditions (rotational rate in a given direction),
+    2. According to DAE theory, a stabilisation on orientation/angular velocity
+       is necessary based on "differentiation index" used to obtain the final
+       form of the constraint.
 
 Author
     Viktor Pandza, FSB Zagreb.  All rights reserved.
@@ -167,6 +171,14 @@ public:
                 const vector& omega
             ) const = 0;
 
+            //- Stabilise the constraint (necessary for constraints with
+            //  differentiation index higher than zero)
+            virtual void stabilise
+            (
+                const scalar t,
+                vector& omega
+            ) const = 0;
+
 
     // I-O Functions and Operators
 

src/ODE/sixDOF/sixDOFODE/constraints/translationalConstraints/constantTranslationalAcceleration/constantTranslationalAcceleration.H

@@ -111,6 +111,16 @@ public:
                 const vector& 
             ) const;
 
+            //- Stabilise the constraint
+            virtual void stabilise
+            (
+                const scalar,
+                vector&,
+                vector&
+            ) const
+            {
+                // Does nothing: no need to stabilise this constraint
+            };
 
 
     // I-O Functions

src/ODE/sixDOF/sixDOFODE/constraints/translationalConstraints/periodicOscillation/periodicOscillation.C

@@ -117,6 +117,32 @@ Foam::scalar Foam::periodicOscillation::sourceContribution
 }
 
 
+void Foam::periodicOscillation::stabilise
+(
+    const scalar t,
+    vector& x,
+    vector& u
+) const
+{
+    // Set the displacement according to periodic oscillation
+
+    // First subtract calculated displacement...
+    x -= (x & dir_)*dir_;
+
+    // ... then add the correct displacement
+    x += dir_*sin(omega_*t + phi_);
+
+
+    // Set the velocity according to periodic oscillation
+
+    // First subract calculated velocity...
+    u -= (u & dir_)*dir_;
+
+    // ... then add the correct velocity
+    u += dir_*cos(omega_*t + phi_);
+}
+
+
 void Foam::periodicOscillation::write(Ostream& os) const
 {
     os.writeKeyword("type") << tab << type()

src/ODE/sixDOF/sixDOFODE/constraints/translationalConstraints/periodicOscillation/periodicOscillation.H

@@ -32,6 +32,10 @@ Description
     where a is the amplitude of oscillation, omega radial frequency and phi is
     the phase shift.
 
+    Note: since this constraint is basically algebraic (dependent on
+    displacement), the differentiation index is two so the stabilisation on
+    displacement and velocity is necessary.
+
 Author
     Viktor Pandza, FSB Zagreb.  All rights reserved.
     Vuko Vukcevic, FSB Zagreb.  All rights reserved.
@@ -124,6 +128,13 @@ public:
                 const vector& 
             ) const;
 
+            //- Stabilise the constraint
+            virtual void stabilise
+            (
+                const scalar t,
+                vector& x,
+                vector& u
+            ) const;
 
 
     // I-O Functions

src/ODE/sixDOF/sixDOFODE/constraints/translationalConstraints/translationalConstraint/translationalConstraint.H

@@ -41,8 +41,12 @@ Description
     2. sourceContribution() - corresponding to a(t), to be inserted into right
                               hand side vector.
 
-    Note: constraints are usually used alongside appropriate initial
-    conditions (velocity in a given direction).
+    Notes:
+    1. Constraints are usually used alongside appropriate initial
+       conditions (velocity in a given direction),
+    2. According to DAE theory, a stabilisation on displacement/velocity is
+       necessary based on "differentiation index" used to obtain the final form
+       of the constraint.
 
 Author
     Viktor Pandza, FSB Zagreb.  All rights reserved.
@@ -169,6 +173,15 @@ public:
                 const vector& u 
             ) const = 0;
 
+            //- Stabilise the constraint (necessary for constraints with
+            //  differentiation index higher than zero)
+            virtual void stabilise
+            (
+                const scalar t,
+                vector& x,
+                vector& u
+            ) const = 0;
+
 
     // I-O Functions and Operators