void Foam::sixDoFRigidBodyMotion::updateAcceleration
(
    const vector& fGlobal,
    const vector& tauGlobal
)
{
    static bool first = false;

    // Save the previous iteration accelerations for relaxation
    vector aPrevIter = a();
    vector tauPrevIter = tau();

    // Calculate new accelerations
    a() = fGlobal/mass_;
    tau() = (Q().T() & tauGlobal);
    applyRestraints();

    // Relax accelerations on all but first iteration
    if (!first)
    {
        a() = aRelax_*a() + (1 - aRelax_)*aPrevIter;
        tau() = aRelax_*tau() + (1 - aRelax_)*tauPrevIter;
    }

    first = false;
}
void Foam::sixDoFRigidBodyMotion::updateForce
(
    const vector& fGlobal,
    const vector& tauGlobal,
    scalar deltaT
)
{
    // Second leapfrog velocity adjust part, required after motion and
    // force calculation

    if (Pstream::master())
    {
        a() = fGlobal/mass_;

        tau() = (Q().T() & tauGlobal);

        applyRestraints();

        applyConstraints(deltaT);

        vector aClip = a();
        scalar aMag = mag(aClip);

        if (aMag > SMALL)
        {
            aClip /= aMag;
        }

        if (aMag > aLim_)
        {
            WarningIn
            (
                "void Foam::sixDoFRigidBodyMotion::updateForce"
                "("
                "const vector& fGlobal, "
                "const vector& tauGlobal, "
                "scalar deltaT"
                ")"
            )
                    << "Limited acceleration " << a()
                    << " to " << aClip*aLim_
                    << endl;
        }

        v() += 0.5*(1 - cDamp_)*deltaT*aClip*min(aMag, aLim_);

        pi() += 0.5*(1 - cDamp_)*deltaT*tau();

        if(report_)
        {
            status();
        }
    }

    Pstream::scatter(motionState_);
}
void Foam::sixDoFRigidBodyMotion::updateAcceleration
(
    bool firstIter,
    const vector& fGlobal,
    const vector& tauGlobal,
    scalar deltaT,
    scalar deltaT0
)
{
    static bool first = false;

    if (Pstream::master())
    {
        // Save the previous iteration accelerations for relaxation
        vector aPrevIter = a();
        vector tauPrevIter = tau();

        // Calculate new accelerations
        a() = fGlobal/mass_;
        tau() = (Q().T() & tauGlobal);
        applyRestraints();

        // Relax accelerations on all but first iteration
        if (!first)
        {
            a() = aRelax_*a() + (1 - aRelax_)*aPrevIter;
            tau() = aRelax_*tau() + (1 - aRelax_)*tauPrevIter;
        }
        first = false;

        if (firstIter)
        {
            // Second simplectic step:
            //     Complete update of linear and angular velocities

            v() += tConstraints_ & aDamp_*0.5*deltaT*a();
            pi() += rConstraints_ & aDamp_*0.5*deltaT*tau();
        }
        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()));
        }

        if (report_)
        {
            status();
        }
    }

    Pstream::scatter(motionState_);
}
void Foam::sixDoFRigidBodyMotion::updateAcceleration
(
    const vector& fGlobal,
    const vector& tauGlobal,
    scalar deltaT
)
{
    static bool first = false;

    // Second leapfrog velocity adjust part, required after motion and
    // acceleration calculation

    if (Pstream::master())
    {
        // Save the previous iteration accelerations for relaxation
        vector aPrevIter = a();
        vector tauPrevIter = tau();

        // Calculate new accelerations
        a() = fGlobal/mass_;
        tau() = (Q().T() & tauGlobal);
        applyRestraints();

        // Relax accelerations on all but first iteration
        if (!first)
        {
            a() = aRelax_*a() + (1 - aRelax_)*aPrevIter;
            tau() = aRelax_*tau() + (1 - aRelax_)*tauPrevIter;
        }
        first = false;

        // Correct velocities
        v() += tConstraints_ & aDamp_*0.5*deltaT*a();
        pi() += rConstraints_ & aDamp_*0.5*deltaT*tau();

        if (report_)
        {
            status();
        }
    }

    Pstream::scatter(motionState_);
}