Commit f026a5af authored by graham's avatar graham
Browse files

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.
parent a0522a2b
......@@ -71,13 +71,26 @@ void Foam::sixDoFRigidBodyMotion::applyConstraints(scalar deltaT)
do
{
converged = true;
iter++;
if (iter > maxConstraintIters_)
{
FatalErrorIn
(
"Foam::sixDoFRigidBodyMotion::applyConstraints"
"(scalar deltaT)"
)
<< nl << "Maximum number of constraint iterations ("
<< maxConstraintIters_ << ") exceeded." << nl
<< exit(FatalError);
}
Info<< "Iteration " << iter << endl;
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
);
}
iter++;
// Accumulate constraint force
cFA += cF;
// Accumulate constraint moment
cMA += cM + ((cP - centreOfMass()) ^ cF);
}
} 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_)));
}
......
......@@ -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
......
......@@ -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_);
}
......
......@@ -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_;
......
......@@ -47,7 +47,11 @@ Foam::sixDoFRigidBodyMotionConstraint::sixDoFRigidBodyMotionConstraint
+ "Coeffs"
)
),
tolerance_(readScalar(sDoFRBMCDict.lookup("tolerance")))
tolerance_(readScalar(sDoFRBMCDict.lookup("tolerance"))),
relaxationFactor_
(
sDoFRBMCDict.lookupOrDefault<scalar>("relaxationFactor", 1)
)
{}
......
......@@ -77,6 +77,9 @@ protected:
// absolute distance or angle.
scalar tolerance_;
//- Relaxation factor for solution, default to one
scalar relaxationFactor_;
public:
......
......@@ -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_)));
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment