void Foam::sixDoFRigidBodyMotion::updatePosition
(
scalar deltaT,
scalar deltaT0
)
{
// First leapfrog velocity adjust and motion part, required before
// force calculation
if (Pstream::master())
{
v() = tConstraints_ & aDamp_*(v0() + 0.5*deltaT0*a());
pi() = rConstraints_ & aDamp_*(pi0() + 0.5*deltaT0*tau());
// Leapfrog move part
centreOfRotation() = centreOfRotation0() + deltaT*v();
// Leapfrog orientation adjustment
Tuple2<tensor, vector> Qpi = rotate(Q0(), pi(), deltaT);
Q() = Qpi.first();
pi() = rConstraints_ & Qpi.second();
}
Pstream::scatter(motionState_);
}
void Foam::sixDoFSolvers::CrankNicolson::solve
(
bool firstIter,
const vector& fGlobal,
const vector& tauGlobal,
scalar deltaT,
scalar deltaT0
)
{
// Update the linear acceleration and torque
updateAcceleration(fGlobal, tauGlobal);
// Correct linear velocity
v() = tConstraints()
& (v0() + aDamp()*deltaT*(aoc_*a() + (1 - aoc_)*a0()));
// Correct angular momentum
pi() = rConstraints()
& (pi0() + aDamp()*deltaT*(aoc_*tau() + (1 - aoc_)*tau0()));
// Correct position
centreOfRotation() =
centreOfRotation0() + deltaT*(voc_*v() + (1 - voc_)*v0());
// Correct orientation
Tuple2<tensor, vector> Qpi =
rotate(Q0(), (voc_*pi() + (1 - voc_)*pi0()), deltaT);
Q() = Qpi.first();
}
void Foam::sixDoFRigidBodyMotion::status() const
{
Info<< "Centre of rotation: " << centreOfRotation() << nl
<< "Centre of mass: " << centreOfMass() << nl
<< "Linear velocity: " << v() << nl
<< "Angular velocity: " << omega()
<< endl;
}
Foam::tmp<Foam::pointField> Foam::sixDoFRigidBodyMotion::transform
(
const pointField& initialPoints
) const
{
return
(
centreOfRotation()
+ (Q() & initialQ_.T() & (initialPoints - initialCentreOfRotation_))
);
}
void Foam::sixDoFRigidBodyMotion::updatePosition
(
bool firstIter,
scalar deltaT,
scalar deltaT0
)
{
if (Pstream::master())
{
if (firstIter)
{
// First simplectic step:
// Half-step for linear and angular velocities
// Update position and orientation
v() = tConstraints_ & (v0() + aDamp_*0.5*deltaT0*a());
pi() = rConstraints_ & (pi0() + aDamp_*0.5*deltaT0*tau());
centreOfRotation() = centreOfRotation0() + deltaT*v();
}
else
{
// For subsequent iterations use Crank-Nicolson
v() = tConstraints_
& (v0() + aDamp_*0.5*deltaT*(a() + motionState0_.a()));
pi() = rConstraints_
& (pi0() + aDamp_*0.5*deltaT*(tau() + motionState0_.tau()));
centreOfRotation() =
centreOfRotation0() + 0.5*deltaT*(v() + motionState0_.v());
}
// Correct orientation
Tuple2<tensor, vector> Qpi = rotate(Q0(), pi(), deltaT);
Q() = Qpi.first();
pi() = rConstraints_ & Qpi.second();
}
Pstream::scatter(motionState_);
}
Foam::tmp<Foam::pointField> Foam::sixDoFRigidBodyMotion::transform
(
const pointField& initialPoints,
const scalarField& scale
) const
{
// Calculate the transformation septerion from the initial state
septernion s
(
centreOfRotation() - initialCentreOfRotation(),
quaternion(Q().T() & initialQ())
);
tmp<pointField> tpoints(new pointField(initialPoints));
pointField& points = tpoints.ref();
forAll(points, pointi)
{
// Move non-stationary points
if (scale[pointi] > SMALL)
{
// Use solid-body motion where scale = 1
if (scale[pointi] > 1 - SMALL)
{
points[pointi] = transform(initialPoints[pointi]);
}
// Slerp septernion interpolation
else
{
septernion ss(slerp(septernion::I, s, scale[pointi]));
points[pointi] =
initialCentreOfRotation()
+ ss.invTransformPoint
(
initialPoints[pointi]
- initialCentreOfRotation()
);
}
}
}
return tpoints;
}
void Foam::sixDoFSolvers::Newmark::solve
(
bool firstIter,
const vector& fGlobal,
const vector& tauGlobal,
scalar deltaT,
scalar deltaT0
)
{
// Update the linear acceleration and torque
updateAcceleration(fGlobal, tauGlobal);
// Correct linear velocity
v() =
tConstraints()
& (v0() + aDamp()*deltaT*(gamma_*a() + (1 - gamma_)*a0()));
// Correct angular momentum
pi() =
rConstraints()
& (pi0() + aDamp()*deltaT*(gamma_*tau() + (1 - gamma_)*tau0()));
// Correct position
centreOfRotation() =
centreOfRotation0()
+ (
tConstraints()
& (
deltaT*v0()
+ aDamp()*sqr(deltaT)*(beta_*a() + (0.5 - beta_)*a0())
)
);
// Correct orientation
vector piDeltaT =
rConstraints()
& (
deltaT*pi0()
+ aDamp()*sqr(deltaT)*(beta_*tau() + (0.5 - beta_)*tau0())
);
Tuple2<tensor, vector> Qpi = rotate(Q0(), piDeltaT, 1);
Q() = Qpi.first();
}
void Foam::sixDoFRigidBodyMotion::applyRestraints()
{
if (restraints_.empty())
{
return;
}
if (Pstream::master())
{
forAll(restraints_, rI)
{
if (report_)
{
Info<< "Restraint " << restraints_[rI].name() << ": ";
}
// Restraint position
point rP = Zero;
// Restraint force
vector rF = Zero;
// Restraint moment
vector rM = Zero;
// Accumulate the restraints
restraints_[rI].restrain(*this, rP, rF, rM);
// Update the acceleration
a() += rF/mass_;
// Moments are returned in global axes, transforming to
// body local to add to torque.
tau() += Q().T() & (rM + ((rP - centreOfRotation()) ^ rF));
}
}
}
