diff --git a/src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotion.C b/src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotion.C index 0eef0872e8..cf5f1f95af 100644 --- a/src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotion.C +++ b/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_))); } diff --git a/src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotion.H b/src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotion.H index 4b1c724414..fd3078ed9e 100644 --- a/src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotion.H +++ b/src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotion.H @@ -94,6 +94,10 @@ class sixDoFRigidBodyMotion //- Constaints on the motion PtrList 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& 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 currentPosition(const pointField& pts) const; + inline tmp 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 diff --git a/src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionConstraint/fixedPoint/fixedPoint.C b/src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionConstraint/fixedPoint/fixedPoint.C index 7ff526fe07..cfe8b5421d 100644 --- a/src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionConstraint/fixedPoint/fixedPoint.C +++ b/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_); } diff --git a/src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionConstraint/fixedPoint/fixedPoint.H b/src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionConstraint/fixedPoint/fixedPoint.H index 9619d8e72f..59b5c35c43 100644 --- a/src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionConstraint/fixedPoint/fixedPoint.H +++ b/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_; diff --git a/src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionConstraint/sixDoFRigidBodyMotionConstraint/sixDoFRigidBodyMotionConstraint.C b/src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionConstraint/sixDoFRigidBodyMotionConstraint/sixDoFRigidBodyMotionConstraint.C index bb8c50b3a5..7c22078878 100644 --- a/src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionConstraint/sixDoFRigidBodyMotionConstraint/sixDoFRigidBodyMotionConstraint.C +++ b/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("relaxationFactor", 1) + ) {} diff --git a/src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionConstraint/sixDoFRigidBodyMotionConstraint/sixDoFRigidBodyMotionConstraint.H b/src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionConstraint/sixDoFRigidBodyMotionConstraint/sixDoFRigidBodyMotionConstraint.H index be240dce2b..0b5fa0bc79 100644 --- a/src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionConstraint/sixDoFRigidBodyMotionConstraint/sixDoFRigidBodyMotionConstraint.H +++ b/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: diff --git a/src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionI.H b/src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionI.H index 5b0d607ef4..146282749e 100644 --- a/src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionI.H +++ b/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::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_))); }