sixDoFRigidBody: adding constraints.

Added force to fixedPoint constraint.

Moved application of rotation matrices to function.

Implemented predictedPosition function.

Added maximum iteration check and under-relaxation variable.

src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotion.C

@@ -71,13 +71,26 @@ void Foam::sixDoFRigidBodyMotion::applyConstraints(scalar deltaT)
 
         do
         {
-            converged = true;
+            iter++;
 
-            Info<< "Iteration " << iter << endl;
+            if (iter > maxConstraintIters_)
+            {
+                FatalErrorIn
+                (
+                    "Foam::sixDoFRigidBodyMotion::applyConstraints"
+                    "(scalar deltaT)"
+                )
+                    << nl << "Maximum number of constraint iterations ("
+                    << maxConstraintIters_ << ") exceeded." << nl
+                    << exit(FatalError);
+
+            }
+
+            converged = true;
 
             forAll(constraints_, cI)
             {
-                Info<< "Constraint " << cI << endl;
+                // Info<< "Constraint " << cI << endl;
 
                 // constraint position
                 point cP = vector::zero;
@@ -98,11 +111,27 @@ void Foam::sixDoFRigidBodyMotion::applyConstraints(scalar deltaT)
                     cF,
                     cM
                 );
+
+                // Accumulate constraint force
+                cFA += cF;
+
+                // Accumulate constraint moment
+                cMA += cM + ((cP - centreOfMass()) ^ cF);
             }
 
-            iter++;
-
         } while(!converged);
+
+        Info<< "Constraints converged in " << iter << " iterations" << nl
+            << "Constraint force: " << cFA << nl
+            << "Constraint moment: " << cMA
+            << endl;
+
+        // Add the constrain forces and moments to the motion state variables
+        a() += cFA/mass_;
+
+        // The moment of constraint forces has already been added
+        // during accumulation
+        tau() += Q().T() & cMA;
     }
 }
 
@@ -114,6 +143,7 @@ Foam::sixDoFRigidBodyMotion::sixDoFRigidBodyMotion()
     motionState_(),
     restraints_(),
     constraints_(),
+    maxConstraintIters_(0),
     refCentreOfMass_(vector::zero),
     momentOfInertia_(diagTensor::one*VSMALL),
     mass_(VSMALL)
@@ -144,6 +174,7 @@ Foam::sixDoFRigidBodyMotion::sixDoFRigidBodyMotion
     ),
     restraints_(),
     constraints_(),
+    maxConstraintIters_(0),
     refCentreOfMass_(refCentreOfMass),
     momentOfInertia_(momentOfInertia),
     mass_(mass)
@@ -155,6 +186,7 @@ Foam::sixDoFRigidBodyMotion::sixDoFRigidBodyMotion(const dictionary& dict)
     motionState_(dict),
     restraints_(),
     constraints_(),
+    maxConstraintIters_(0),
     refCentreOfMass_(dict.lookupOrDefault("refCentreOfMass", centreOfMass())),
     momentOfInertia_(dict.lookup("momentOfInertia")),
     mass_(readScalar(dict.lookup("mass")))
@@ -173,6 +205,7 @@ Foam::sixDoFRigidBodyMotion::sixDoFRigidBodyMotion
     motionState_(sDoFRBM.motionState()),
     restraints_(sDoFRBM.restraints()),
     constraints_(sDoFRBM.constraints()),
+    maxConstraintIters_(sDoFRBM.maxConstraintIters()),
     refCentreOfMass_(sDoFRBM.refCentreOfMass()),
     momentOfInertia_(sDoFRBM.momentOfInertia()),
     mass_(sDoFRBM.mass())
@@ -242,15 +275,21 @@ void Foam::sixDoFRigidBodyMotion::addConstraints
 
                 constraints_.set
                 (
-                    i,
+                    i++,
                     sixDoFRigidBodyMotionConstraint::New(iter().dict())
                 );
             }
-
-            i++;
         }
 
         constraints_.setSize(i);
+
+        if (constraints_.size())
+        {
+            maxConstraintIters_ = readLabel
+            (
+                constraintDict.lookup("maxIterations")
+            );
+        }
     }
 }
 
@@ -274,27 +313,7 @@ void Foam::sixDoFRigidBodyMotion::updatePosition
 
         // Leapfrog orientation adjustment
 
-        tensor R;
-
-        R = rotationTensorX(0.5*deltaT*pi().x()/momentOfInertia_.xx());
-        pi() = pi() & R;
-        Q() = Q() & R;
-
-        R = rotationTensorY(0.5*deltaT*pi().y()/momentOfInertia_.yy());
-        pi() = pi() & R;
-        Q() = Q() & R;
-
-        R = rotationTensorZ(deltaT*pi().z()/momentOfInertia_.zz());
-        pi() = pi() & R;
-        Q() = Q() & R;
-
-        R = rotationTensorY(0.5*deltaT*pi().y()/momentOfInertia_.yy());
-        pi() = pi() & R;
-        Q() = Q() & R;
-
-        R = rotationTensorX(0.5*deltaT*pi().x()/momentOfInertia_.xx());
-        pi() = pi() & R;
-        Q() = Q() & R;
+        rotate(Q(), pi(), deltaT);
     }
 
     Pstream::scatter(motionState_);
@@ -365,8 +384,17 @@ Foam::point Foam::sixDoFRigidBodyMotion::predictedPosition
     scalar deltaT
 ) const
 {
-    Info<< "predictedPosition NOT IMPLEMENTED" << endl;
-    return pt;
+    vector vTemp = v() + deltaT*(a() + deltaForce/mass_);
+
+    vector piTemp = pi() + deltaT*(tau() + (Q().T() & deltaMoment));
+
+    point centreOfMassTemp = centreOfMass() + deltaT*vTemp;
+
+    tensor QTemp = Q();
+
+    rotate(QTemp, piTemp, deltaT);
+
+    return (centreOfMassTemp + (QTemp & (pt - refCentreOfMass_)));
 }
 
 

src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotion.H

@@ -94,6 +94,10 @@ class sixDoFRigidBodyMotion
         //- Constaints on the motion
         PtrList<sixDoFRigidBodyMotionConstraint> constraints_;
 
+        //- Maximum number of iterations allowed to attempt to obey
+        //  constraints
+        label maxConstraintIters_;
+
         //- Centre of mass of reference state
         point refCentreOfMass_;
 
@@ -118,6 +122,9 @@ class sixDoFRigidBodyMotion
         //  frame z-axis by the given angle
         inline tensor rotationTensorZ(scalar deltaT) const;
 
+        //- Apply rotation tensors to Q for the given torque (pi) and deltaT
+        inline void rotate(tensor& Q, vector& pi, scalar deltaT) const;
+
         //- Apply the restraints to the object
         void applyRestraints();
 
@@ -140,6 +147,10 @@ class sixDoFRigidBodyMotion
             inline const PtrList<sixDoFRigidBodyMotionConstraint>&
                 constraints() const;
 
+            //- Return access to the maximum allowed number of
+            //  constraint iterations
+            inline label maxConstraintIters() const;
+
             //- Return access to the centre of mass
             inline const point& refCentreOfMass() const;
 
@@ -255,11 +266,11 @@ public:
 
         //- Transform the given reference state pointField by the
         //  current motion state
-        inline tmp<pointField> currentPosition(const pointField& pts) const;
+        inline tmp<pointField> currentPosition(const pointField& refPts) const;
 
         //- Transform the given reference state point by the current
         //  motion state
-        inline point currentPosition(const point& pt) const;
+        inline point currentPosition(const point& refPt) const;
 
         //- Predict the position of the supplied point after deltaT
         //  given the current motion state and the additional supplied

src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionConstraint/fixedPoint/fixedPoint.C

@@ -86,11 +86,35 @@ bool Foam::sixDoFRigidBodyMotionConstraints::fixedPoint::constrain
         deltaT
     );
 
+    constraintPosition = motion.currentPosition(fixedPoint_);
+
+    // Info<< "current position " << constraintPosition << nl
+    //     << "next predictedPosition " << predictedPosition
+    //     << endl;
+
     vector error = predictedPosition - fixedPoint_;
 
-    Info<< error << endl;
+    // Info<< "error " << error << endl;
 
-    return true;
+    // Correction force derived from Lagrange multiplier:
+    //     G = -lambda*grad(sigma)
+    // where
+    //     sigma = mag(error) = 0
+    // so
+    //     grad(sigma) = error/mag(error)
+    // Solving for lambda using the SHAKE methodology gives
+    //     lambda = mass*mag(error)/sqr(deltaT)
+    // This is only strictly applicable (i.e. will converge in one
+    // iteration) to constraints at the centre of mass.  Everything
+    // else will need to iterate, and may need under-relaxed to be
+    // stable.
+
+    constraintForceIncrement =
+        -relaxationFactor_*error*motion.mass()/sqr(deltaT);
+
+    constraintMomentIncrement = vector::zero;
+
+    return (mag(error) < tolerance_);
 }
 
 

src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionConstraint/fixedPoint/fixedPoint.H

@@ -58,7 +58,9 @@ class fixedPoint
     // Private data
 
         //- Point which is rigidly attached to the body and constrains
-        //  it so that this point remains fixed.
+        //  it so that this point remains fixed.  This serves as the
+        //  reference point for displacements as well as the target
+        //  position.
         point fixedPoint_;
 
 

src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionConstraint/sixDoFRigidBodyMotionConstraint/sixDoFRigidBodyMotionConstraint.C

@@ -47,7 +47,11 @@ Foam::sixDoFRigidBodyMotionConstraint::sixDoFRigidBodyMotionConstraint
           + "Coeffs"
         )
     ),
-    tolerance_(readScalar(sDoFRBMCDict.lookup("tolerance")))
+    tolerance_(readScalar(sDoFRBMCDict.lookup("tolerance"))),
+    relaxationFactor_
+    (
+        sDoFRBMCDict.lookupOrDefault<scalar>("relaxationFactor", 1)
+    )
 {}
 
 

src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionConstraint/sixDoFRigidBodyMotionConstraint/sixDoFRigidBodyMotionConstraint.H

@@ -77,6 +77,9 @@ protected:
         //  absolute distance or angle.
         scalar tolerance_;
 
+        //- Relaxation factor for solution, default to one
+        scalar relaxationFactor_;
+
 
 public:
 

src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionI.H

@@ -62,6 +62,37 @@ Foam::sixDoFRigidBodyMotion::rotationTensorZ(scalar phi) const
 }
 
 
+inline void Foam::sixDoFRigidBodyMotion::rotate
+(
+    tensor& Q,
+    vector& pi,
+    scalar deltaT
+) const
+{
+    tensor R;
+
+    R = rotationTensorX(0.5*deltaT*pi.x()/momentOfInertia_.xx());
+    pi = pi & R;
+    Q = Q & R;
+
+    R = rotationTensorY(0.5*deltaT*pi.y()/momentOfInertia_.yy());
+    pi = pi & R;
+    Q = Q & R;
+
+    R = rotationTensorZ(deltaT*pi.z()/momentOfInertia_.zz());
+    pi = pi & R;
+    Q = Q & R;
+
+    R = rotationTensorY(0.5*deltaT*pi.y()/momentOfInertia_.yy());
+    pi = pi & R;
+    Q = Q & R;
+
+    R = rotationTensorX(0.5*deltaT*pi.x()/momentOfInertia_.xx());
+    pi = pi & R;
+    Q = Q & R;
+}
+
+
 inline const Foam::sixDoFRigidBodyMotionState&
 Foam::sixDoFRigidBodyMotion::motionState() const
 {
@@ -83,6 +114,12 @@ Foam::sixDoFRigidBodyMotion::constraints() const
 }
 
 
+inline Foam::label Foam::sixDoFRigidBodyMotion::maxConstraintIters() const
+{
+    return maxConstraintIters_;
+}
+
+
 inline const Foam::point& Foam::sixDoFRigidBodyMotion::refCentreOfMass() const
 {
     return refCentreOfMass_;
@@ -171,18 +208,18 @@ inline Foam::vector& Foam::sixDoFRigidBodyMotion::tau()
 // * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
 
 inline Foam::tmp<Foam::pointField>
-Foam::sixDoFRigidBodyMotion::currentPosition(const pointField& pts) const
+Foam::sixDoFRigidBodyMotion::currentPosition(const pointField& refPts) const
 {
-    return (centreOfMass() + (Q() & (pts - refCentreOfMass_)));
+    return (centreOfMass() + (Q() & (refPts - refCentreOfMass_)));
 }
 
 
 inline Foam::point Foam::sixDoFRigidBodyMotion::currentPosition
 (
-    const point& pt
+    const point& refPt
 ) const
 {
-    return (centreOfMass() + (Q() & (pt - refCentreOfMass_)));
+    return (centreOfMass() + (Q() & (refPt - refCentreOfMass_)));
 }