metaforce/Runtime/World/CPhysicsActor.cpp

#include "CPhysicsActor.hpp"

namespace urde
{

CPhysicsActor::CPhysicsActor(TUniqueId uid, bool active, const std::string& name, const CEntityInfo& info,
                             const zeus::CTransform& xf, CModelData&& mData, const CMaterialList& matList,
                             const zeus::CAABox& box, const SMoverData& moverData, const CActorParameters& actorParms,
                             float stepUp, float stepDown)
    : CActor(uid, active, name, info, xf, std::move(mData), matList, actorParms, kInvalidUniqueId)
    , xe8_mass(moverData.x30_mass)
    , xec_massRecip(moverData.x30_mass <= 0.f ? 1.f : 1.f / moverData.x30_mass)
    , xf8_24_(true)
    , x150_momentum(moverData.x18_)
    , x1c0_collisionPrimitive(box, matList)
    , x1f4_translation(xf.origin)
    , x200_orientation(xf.buildMatrix3f())
    , x23c_stepUpHeight(stepUp)
    , x240_stepDownHeight(stepDown)
{
    SetMass(moverData.x30_mass);
    MoveCollisionPrimitive(zeus::CVector3f::skZero);
    SetVelocityOR(moverData.x0_velocity);
    SetAngularVelocityOR(moverData.xc_angularVelocity);
    ComputeDerivedQuantities();
}

void CPhysicsActor::Render(const CStateManager &mgr) const
{
    CActor::Render(mgr);
}

zeus::CVector3f CPhysicsActor::GetOrbitPosition(const CStateManager &) const
{
    return GetBoundingBox().center();
}

zeus::CVector3f CPhysicsActor::GetAimPosition(const CStateManager&, float dt) const
{
    if (dt <= 0.0)
        return GetBoundingBox().center();
    zeus::CVector3f trans = PredictMotion(dt).x0_translation;
    return GetBoundingBox().center() + trans;
}

void CPhysicsActor::CollidedWith(const TUniqueId &, const CCollisionInfoList &, CStateManager &)
{
}

const CCollisionPrimitive& CPhysicsActor::GetCollisionPrimitive() const
{
    return x1c0_collisionPrimitive;
}

zeus::CTransform CPhysicsActor::GetPrimitiveTransform() const
{
    return zeus::CTransform::Translate(x34_transform.origin + x1e8_primitiveOffset);
}

float CPhysicsActor::GetStepUpHeight() const
{
    return x23c_stepUpHeight;
}

float CPhysicsActor::GetStepDownHeight() const
{
    return x240_stepDownHeight;
}

float CPhysicsActor::GetWeight() const
{
    return 24.525002f * xe8_mass;
}

void CPhysicsActor::sub_8011A4C(float f)
{
    x238_ = f;
}

float CPhysicsActor::sub_8011A4B8() const
{
    return x238_;
}

void CPhysicsActor::SetPrimitiveOffset(const zeus::CVector2f &offset)
{
    x1e8_primitiveOffset = offset;
}

zeus::CVector3f CPhysicsActor::GetPrimitiveOffset()
{
    return x1e8_primitiveOffset;
}

void CPhysicsActor::MoveCollisionPrimitive(const zeus::CVector3f &offset)
{
    x1e8_primitiveOffset = offset;
}

void CPhysicsActor::SetBoundingBox(const zeus::CAABox &box)
{
    x1a4_baseBoundingBox = box;
    MoveCollisionPrimitive(zeus::CVector3f::skZero);
}

zeus::CAABox CPhysicsActor::GetMotionVolume(float dt) const
{
    zeus::CAABox aabox = GetCollisionPrimitive().CalculateAABox(GetPrimitiveTransform());
    zeus::CVector3f velocity = CalculateNewVelocityWR_UsingImpulses();

    const zeus::CVector3f dv = (dt * velocity);
    aabox.accumulateBounds(aabox.max + dv);
    aabox.accumulateBounds(aabox.min + dv);

    float up = GetStepUpHeight();
    up = zeus::max(up, 0.f);
    aabox.accumulateBounds(aabox.max + up);

    float down = GetStepDownHeight();
    down = zeus::max(down, 0.f);
    aabox.accumulateBounds(aabox.min + down);
    return aabox;
}

zeus::CVector3f CPhysicsActor::CalculateNewVelocityWR_UsingImpulses() const
{
    return x138_velocity + (xec_massRecip * (x168_impulse + x18c_));
}

zeus::CAABox CPhysicsActor::GetBoundingBox() const
{
    return { x1a4_baseBoundingBox.min + x1e8_primitiveOffset + x34_transform.origin,
                x1a4_baseBoundingBox.max + x1e8_primitiveOffset + x34_transform.origin };
}

const zeus::CAABox& CPhysicsActor::GetBaseBoundingBox() const
{
    return x1a4_baseBoundingBox;
}

void CPhysicsActor::AddMotionState(const CMotionState& mst)
{
    zeus::CNUQuaternion q{x34_transform.buildMatrix3f()};
    // TODO: Jack please verify this assignment: 8011B514
    x34_transform = zeus::CTransform::Translate(x34_transform.origin) * zeus::CMatrix3f(q);

    xe4_27_ = true;
    xe4_28_ = true;
    xe4_29_ = true;

    SetTranslation(x34_transform.origin + mst.x0_translation);

    xfc_constantForce += mst.x1c_velocity;

    ComputeDerivedQuantities();
}

CMotionState CPhysicsActor::GetMotionState() const
{
    return {x34_transform.origin, {x34_transform.buildMatrix3f()}, xfc_constantForce, x108_angularMomentum};
}

void CPhysicsActor::SetMotionState(const CMotionState &mst)
{
    x34_transform = zeus::CTransform::Translate(x34_transform.origin) * zeus::CMatrix3f(mst.xc_orientation);

    xe4_27_ = true;
    xe4_28_ = true;
    xe4_29_ = true;
    SetTranslation(mst.x0_translation);

    xfc_constantForce = mst.x1c_velocity;
    x108_angularMomentum = mst.x28_angularMomentum;
    ComputeDerivedQuantities();
}

void CPhysicsActor::SetInertiaTensorScalar(float tensor)
{
    if (tensor <= 0.0f)
        tensor = 1.0f;
    xf0_inertiaTensor = tensor;
    xf4_inertiaTensorRecip = 1.0f / tensor;
}

void CPhysicsActor::SetMass(float mass)
{
    xe8_mass = mass;
    float tensor = 1.0f;
    if (mass > 0.0f)
        tensor = 1.0f / mass;

    xec_massRecip = tensor;
    SetInertiaTensorScalar(mass * tensor);
}

void CPhysicsActor::SetAngularVelocityOR(const zeus::CAxisAngle& angVel)
{
    x144_angularVelocity = x34_transform.rotate(angVel);
    x108_angularMomentum = xf0_inertiaTensor * x144_angularVelocity;
}

zeus::CAxisAngle CPhysicsActor::GetAngularVelocityOR() const
{
    return x34_transform.transposeRotate(x144_angularVelocity);
}

void CPhysicsActor::SetAngularVelocityWR(const zeus::CAxisAngle& angVel)
{
    x144_angularVelocity = angVel;
    x108_angularMomentum = xf0_inertiaTensor * x144_angularVelocity;
}

void CPhysicsActor::SetVelocityWR(const zeus::CVector3f &vel)
{
    x138_velocity = vel;
    xfc_constantForce = xe8_mass * x138_velocity;
}

void CPhysicsActor::SetVelocityOR(const zeus::CVector3f& vel)
{
    SetVelocityWR(x34_transform.rotate(vel));
}

zeus::CVector3f CPhysicsActor::GetTotalForcesWR() const
{
    return x15c_force + x150_momentum;
}

void CPhysicsActor::RotateInOneFrameOR(const zeus::CQuaternion &q, float d)
{
    x198_ += GetRotateToORAngularMomentumWR(q, d);
}

void CPhysicsActor::MoveInOneFrameOR(const zeus::CVector3f &trans, float d)
{
    x18c_ += GetMoveToORImpulseWR(trans, d);
}

void CPhysicsActor::RotateToOR(const zeus::CQuaternion& q, float d)
{
    x108_angularMomentum = GetRotateToORAngularMomentumWR(q, d);
    ComputeDerivedQuantities();
}

void CPhysicsActor::MoveToOR(const zeus::CVector3f& trans, float d)
{
    xfc_constantForce = GetMoveToORImpulseWR(trans, d);
    ComputeDerivedQuantities();
}

void CPhysicsActor::sub_8011B098(const zeus::CVector3f& trans, float d)
{
    x18c_ +=  xe8_mass * (trans - x34_transform.origin) * (1.f / d);
}

void CPhysicsActor::MoveToWR(const zeus::CVector3f& trans, float d)
{
    xfc_constantForce = xe8_mass * (trans - x34_transform.origin) * (1.f / d);
    ComputeDerivedQuantities();
}

zeus::CAxisAngle CPhysicsActor::GetRotateToORAngularMomentumWR(const zeus::CQuaternion& q, float d) const
{
    if (q.w > 0.99999976)
        return zeus::CAxisAngle::skZero;
    return (xf0_inertiaTensor * (((2.f * std::acos(q.w)) * (1.f / d)) *
                                 x34_transform.rotate({q.x, q.y, q.z}).normalized()));
}

zeus::CVector3f CPhysicsActor::GetMoveToORImpulseWR(const zeus::CVector3f &trans, float d) const
{
    return (xe8_mass * x34_transform.rotate(trans)) * (1.f / d);
}

void CPhysicsActor::ClearImpulses()
{
    x18c_ = x168_impulse = zeus::CVector3f::skZero;
    x198_ = x180_angularImpulse = zeus::CAxisAngle::skZero;
}

void CPhysicsActor::ClearForcesAndTorques()
{
    x18c_ = x168_impulse = x15c_force = zeus::CVector3f::skZero;
    x198_ = x180_angularImpulse = x174_torque = zeus::CAxisAngle::skZero;
}

void CPhysicsActor::Stop()
{
    ClearForcesAndTorques();
    xfc_constantForce = zeus::CVector3f::skZero;
    x108_angularMomentum = zeus::CAxisAngle::skZero;
    ComputeDerivedQuantities();
}

void CPhysicsActor::ComputeDerivedQuantities()
{
    x138_velocity = xec_massRecip * xfc_constantForce;
    x114_ = x34_transform.buildMatrix3f();
    x144_angularVelocity = xf0_inertiaTensor * x108_angularMomentum;
}

bool CPhysicsActor::WillMove(const CStateManager&)
{
    if (!zeus::close_enough(zeus::CVector3f::skZero, x138_velocity) ||
            !zeus::close_enough(zeus::CVector3f::skZero, x168_impulse) ||
            !zeus::close_enough(zeus::CVector3f::skZero, x174_torque) ||
            !zeus::close_enough(zeus::CVector3f::skZero, x18c_) ||
            !zeus::close_enough(zeus::CVector3f::skZero, x144_angularVelocity) ||
            !zeus::close_enough(zeus::CVector3f::skZero, x180_angularImpulse) ||
            !zeus::close_enough(zeus::CVector3f::skZero, x198_) ||
            !zeus::close_enough(zeus::CVector3f::skZero, GetTotalForcesWR()))
        return true;

    return false;
}

void CPhysicsActor::SetPhysicsState(const CPhysicsState &state)
{
    SetTranslation(state.GetTranslation());
    x34_transform = zeus::CTransform::Translate(x34_transform.origin) * zeus::CMatrix3f(state.GetOrientation());
    xe4_27_ = true;
    xe4_28_ = true;
    xe4_29_ = true;

    xfc_constantForce = state.GetConstantForceWR();
    x108_angularMomentum = state.GetAngularMomentumWR();;
    x150_momentum = state.GetMomentumWR();
    x15c_force = state.GetForceWR();
    x168_impulse = state.GetImpulseWR();
    x174_torque = state.GetTorque();
    x180_angularImpulse = state.GetAngularImpulseWR();
    ComputeDerivedQuantities();
}

CPhysicsState CPhysicsActor::GetPhysicsState() const
{
    return {x34_transform.origin, {x34_transform.buildMatrix3f()}, xfc_constantForce, x108_angularMomentum,
            x150_momentum, x15c_force, x168_impulse, x174_torque, x180_angularImpulse};
}

CMotionState CPhysicsActor::PredictMotion_Internal(float dt) const
{
    if (xf8_25_)
        return PredictMotion(dt);

    CMotionState msl = PredictLinearMotion(dt);
    CMotionState msa = PredictAngularMotion(dt);
    return {msl.x0_translation, msa.xc_orientation, msl.x1c_velocity, msa.x28_angularMomentum};
}

CMotionState CPhysicsActor::PredictMotion(float dt) const
{
    CMotionState msl = PredictLinearMotion(dt);
    CMotionState msa = PredictAngularMotion(dt);
    return {msl.x0_translation, msa.xc_orientation, msl.x1c_velocity, msa.x28_angularMomentum};
}

CMotionState CPhysicsActor::PredictLinearMotion(float dt) const
{
    zeus::CVector3f velocity = CalculateNewVelocityWR_UsingImpulses();
    return {velocity * dt, zeus::CNUQuaternion::skNoRotation,
                ((x15c_force + x150_momentum) * dt) + x168_impulse,
                zeus::CAxisAngle::skZero};
}

CMotionState CPhysicsActor::PredictAngularMotion(float dt) const
{
    const zeus::CVector3f v1 = xf4_inertiaTensorRecip * (x180_angularImpulse + x198_);
    zeus::CNUQuaternion q = 0.5f * zeus::CNUQuaternion({0.f, x144_angularVelocity + v1});
    return {zeus::CVector3f::skZero,
                q * zeus::CNUQuaternion(x34_transform.buildMatrix3f()) * dt,
                zeus::CVector3f::skZero,
                (x174_torque * dt) + x180_angularImpulse};
}

void CPhysicsActor::ApplyForceOR(const zeus::CVector3f& force, const zeus::CAxisAngle& torque)
{
    x15c_force += x34_transform.rotate(force);
    x174_torque += x34_transform.rotate(torque);
}

void CPhysicsActor::ApplyForceWR(const zeus::CVector3f &force, const zeus::CAxisAngle& torque)
{
    x15c_force += force;
    x174_torque += torque;
}

void CPhysicsActor::ApplyImpulseOR(const zeus::CVector3f &impulse, const zeus::CAxisAngle &angle)
{
    x168_impulse += x34_transform.rotate(impulse);
    x180_angularImpulse += x34_transform.rotate(angle);
}

void CPhysicsActor::ApplyImpulseWR(const zeus::CVector3f &impulse, const zeus::CAxisAngle& angleImp)
{
    x168_impulse += impulse;
    x180_angularImpulse += angleImp;
}

void CPhysicsActor::ApplyTorqueWR(const zeus::CVector3f& torque)
{
    x174_torque += torque;
}

void CPhysicsActor::UseCollisionImpulses()
{
    xfc_constantForce += x168_impulse;
    x108_angularMomentum += x180_angularImpulse;

    x168_impulse = zeus::CVector3f::skZero;
    x180_angularImpulse = zeus::CAxisAngle::skZero;
    ComputeDerivedQuantities();
}

CPhysicsState::CPhysicsState(const zeus::CVector3f& translation, const zeus::CQuaternion& orient,
                             const zeus::CVector3f& v2, const zeus::CAxisAngle& a1,
                             const zeus::CVector3f& v3, const zeus::CVector3f& v4,
                             const zeus::CVector3f& v5, const zeus::CAxisAngle& a2, const zeus::CAxisAngle& a3)
    : x0_translation(translation)
    , xc_orientation(orient)
    , x1c_constantForce(v2)
    , x28_angularMomentum(a1)
    , x34_momentum(v3)
    , x40_force(v4)
    , x4c_impulse(v5)
    , x58_torque(a2)
    , x64_angularImpulse(a3)
{
}

void CPhysicsState::SetTranslation(const zeus::CVector3f& tr)
{
    x0_translation = tr;
}

void CPhysicsState::SetOrientation(const zeus::CQuaternion& orient)
{
    xc_orientation = orient;
}

const zeus::CQuaternion& CPhysicsState::GetOrientation() const
{
    return xc_orientation;
}

const zeus::CVector3f& CPhysicsState::GetTranslation() const
{
    return x0_translation;
}

const zeus::CVector3f& CPhysicsState::GetConstantForceWR() const
{
    return x1c_constantForce;
}

const zeus::CAxisAngle& CPhysicsState::GetAngularMomentumWR() const
{
    return x28_angularMomentum;
}

const zeus::CVector3f& CPhysicsState::GetMomentumWR() const
{
    return x34_momentum;
}

const zeus::CVector3f& CPhysicsState::GetForceWR() const
{
    return x40_force;
}

const zeus::CVector3f& CPhysicsState::GetImpulseWR() const
{
    return x4c_impulse;
}

const zeus::CAxisAngle& CPhysicsState::GetTorque() const
{
    return x58_torque;
}

const zeus::CAxisAngle& CPhysicsState::GetAngularImpulseWR() const
{
    return x64_angularImpulse;
}

}