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::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_);
}
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();
}