void Foam::sixDoFRigidBodyMotion::status() const
{
Info<< "Centre of mass: " << centreOfMass() << nl
<< "Linear velocity: " << v() << nl
<< "Angular velocity: " << omega()
<< endl;
}
void Foam::sixDoFRigidBodyMotion::applyRestraints()
{
if (restraints_.empty())
{
return;
}
if (Pstream::master())
{
forAll(restraints_, rI)
{
if (report_)
{
Info<< "Restraint " << restraintNames_[rI] << ": ";
}
// restraint position
point rP = vector::zero;
// restraint force
vector rF = vector::zero;
// restraint moment
vector rM = vector::zero;
restraints_[rI].restrain(*this, rP, rF, rM);
a() += rF/mass_;
// Moments are returned in global axes, transforming to
// body local to add to torque.
tau() += Q().T() & (rM + ((rP - centreOfMass()) ^ rF));
}
}
}
Foam::sixDoFRigidBodyMotion::sixDoFRigidBodyMotion(const dictionary& dict)
:
motionState_(dict),
restraints_(),
restraintNames_(),
constraints_(),
constraintNames_(),
maxConstraintIterations_(0),
initialCentreOfMass_
(
dict.lookupOrDefault("initialCentreOfMass", centreOfMass())
),
initialQ_
(
dict.lookupOrDefault("initialOrientation", Q())
),
momentOfInertia_(dict.lookup("momentOfInertia")),
mass_(readScalar(dict.lookup("mass"))),
cDamp_(dict.lookupOrDefault<scalar>("accelerationDampingCoeff", 0.0)),
aLim_(dict.lookupOrDefault<scalar>("accelerationLimit", VGREAT)),
report_(dict.lookupOrDefault<Switch>("report", false))
{
addRestraints(dict);
addConstraints(dict);
}
void Foam::sixDoFRigidBodyMotion::status() const
{
Info<< "6-DoF rigid body motion" << nl
<< " Centre of rotation: " << centreOfRotation() << nl
<< " Centre of mass: " << centreOfMass() << nl
<< " Orientation: " << orientation() << nl
<< " Linear velocity: " << v() << nl
<< " Angular velocity: " << omega()
<< endl;
}
void Foam::sixDoFRigidBodyMotion::updatePosition
(
scalar deltaT,
scalar deltaT0
)
{
// First leapfrog velocity adjust and motion part, required before
// force calculation
if (Pstream::master())
{
vector aClip = a();
scalar aMag = mag(aClip);
if (aMag > SMALL)
{
aClip /= aMag;
}
if (aMag > aLim_)
{
WarningIn
(
"void Foam::sixDoFRigidBodyMotion::updatePosition"
"("
"scalar deltaT, "
"scalar deltaT0"
")"
)
<< "Limited acceleration " << a()
<< " to " << aClip*aLim_
<< endl;
}
v() += 0.5*(1 - cDamp_)*deltaT0*aClip*min(aMag, aLim_);
pi() += 0.5*(1 - cDamp_)*deltaT0*tau();
// Leapfrog move part
centreOfMass() += deltaT*v();
// Leapfrog orientation adjustment
rotate(Q(), pi(), deltaT);
}
Pstream::scatter(motionState_);
}
void Foam::sixDoFRigidBodyMotion::updateForce
(
const pointField& positions,
const vectorField& forces,
scalar deltaT
)
{
vector fGlobal = vector::zero;
vector tauGlobal = vector::zero;
if (Pstream::master())
{
fGlobal = sum(forces);
forAll(positions, i)
{
tauGlobal += (positions[i] - centreOfMass()) ^ forces[i];
}
}
updateForce(fGlobal, tauGlobal, deltaT);
}
Foam::point Foam::sixDoFRigidBodyMotion::predictedPosition
(
const point& pInitial,
const vector& deltaForce,
const vector& deltaMoment,
scalar deltaT
) const
{
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 & initialQ_.T() & (pInitial - initialCentreOfMass_))
);
}
std::vector<float> NelderMead::optimise(OptimisableFunction *target, unsigned num_params,
unsigned max_evals){
std::vector<float> lowest_coord;
float lowest_value = 0.0f;
this->num_params = num_params;
simplex_vertex = createInitialSimplex(target, num_params);
SVComp comp;
sort(simplex_vertex.begin(), simplex_vertex.end(), comp);
for(unsigned iter = 0; iter < max_evals; iter++){
// Find the centre of mass of the first n-1 vertices
std::vector<float> centre_of_mass = centreOfMass(simplex_vertex, simplex_vertex.size() - 1);
// reflect the worst vertex around the centre of mass of the rest of the vertices.
NelderMead::SimplexVertex reflected_vertex =
reflectedVertex(target, simplex_vertex.back(), centre_of_mass);
// if this reflected vertex is the best vertex, try expanding it even further.
if(reflected_vertex.value < simplex_vertex.front().value){
NelderMead::SimplexVertex extended_vertex =
reflectedVertex(target, simplex_vertex.back(), centre_of_mass, 2.0f);
if(extended_vertex.value < reflected_vertex.value){
simplex_vertex.back() = extended_vertex;
}
else{
simplex_vertex.back() = reflected_vertex;
}
}
// If the reflected vertex is decent then just replace the worst with it.
else if(reflected_vertex.value <= simplex_vertex.at(simplex_vertex.size() - 2).value){
simplex_vertex.back() = reflected_vertex;
}
// if the vertex we reflected is the worst, but still better than the one we started with, try
// contracting the original worst.
else if(reflected_vertex.value < simplex_vertex.back().value){
simplex_vertex.back() = contractedVertex(target, simplex_vertex.back(), centre_of_mass);
}
// otherwise this reflected vertex is REALLY bad, even worse than the original, so contract the
// whole simplex towards the best point.
else{
for(unsigned i = 1; i < simplex_vertex.size(); i++){
simplex_vertex.at(i) = contractedVertex(target, simplex_vertex.at(i),
simplex_vertex.front().coord);
}
}
sort(simplex_vertex.begin(), simplex_vertex.end(), comp);
if(simplex_vertex.front().value < lowest_value || iter == 0){
lowest_coord = simplex_vertex.front().coord;
lowest_value = simplex_vertex.front().value;
}
}
return lowest_coord;
}
void Foam::sixDoFRigidBodyMotion::applyConstraints(scalar deltaT)
{
if (constraints_.empty())
{
return;
}
if (Pstream::master())
{
label iteration = 0;
bool allConverged = true;
// constraint force accumulator
vector cFA = vector::zero;
// constraint moment accumulator
vector cMA = vector::zero;
do
{
allConverged = true;
forAll(constraints_, cI)
{
if (sixDoFRigidBodyMotionConstraint::debug)
{
Info<< "Constraint " << constraintNames_[cI] << ": ";
}
// constraint position
point cP = vector::zero;
// constraint force
vector cF = vector::zero;
// constraint moment
vector cM = vector::zero;
bool constraintConverged = constraints_[cI].constrain
(
*this,
cFA,
cMA,
deltaT,
cP,
cF,
cM
);
allConverged = allConverged && constraintConverged;
// Accumulate constraint force
cFA += cF;
// Accumulate constraint moment
cMA += cM + ((cP - centreOfMass()) ^ cF);
}
} while(++iteration < maxConstraintIterations_ && !allConverged);
if (iteration >= maxConstraintIterations_)
{
FatalErrorIn
(
"Foam::sixDoFRigidBodyMotion::applyConstraints"
"(scalar deltaT)"
)
<< nl << "Maximum number of sixDoFRigidBodyMotion constraint "
<< "iterations ("
<< maxConstraintIterations_
<< ") exceeded." << nl
<< exit(FatalError);
}
Info<< "sixDoFRigidBodyMotion constraints converged in "
<< iteration << " iterations" << endl;
if (report_)
{
Info<< "Constraint force: " << cFA << nl
<< "Constraint moment: " << cMA
<< endl;
}
// Add the constraint forces and moments to the motion state variables
a() += cFA/mass_;
// The moment of constraint forces has already been added
// during accumulation. Moments are returned in global axes,
// transforming to body local
tau() += Q().T() & cMA;
}
}
