mirror of
https://github.com/PrimeDecomp/prime.git
synced 2025-12-14 18:06:08 +00:00
402 lines
14 KiB
C++
402 lines
14 KiB
C++
#include "MetroidPrime/CPhysicsActor.hpp"
|
|
|
|
#include "Kyoto/Math/CloseEnough.hpp"
|
|
|
|
#include "rstl/math.hpp"
|
|
|
|
const float CPhysicsActor::kGravityAccel = 9.81f * 2.5f;
|
|
|
|
CPhysicsActor::CPhysicsActor(TUniqueId uid, bool active, const rstl::string& name,
|
|
const CEntityInfo& info, const CTransform4f& xf,
|
|
const CModelData& mData, const CMaterialList& matList,
|
|
const CAABox& aabb, const SMoverData& moverData,
|
|
const CActorParameters& actParams, float stepUp, float stepDown)
|
|
: CActor(uid, active, name, info, xf, mData, matList, actParams, kInvalidUniqueId)
|
|
, xe8_mass(moverData.x30_mass)
|
|
, xec_massRecip(moverData.x30_mass > 0.f ? 1.f / moverData.x30_mass : 1.f)
|
|
, xf0_inertiaTensor(0.f)
|
|
, xf4_inertiaTensorRecip(0.f)
|
|
, xf8_24_movable(true)
|
|
, xf8_25_angularEnabled(false)
|
|
, xf9_standardCollider(false)
|
|
, xfc_constantForce(CVector3f(0.f, 0.f, 0.f))
|
|
, x108_angularMomentum(CAxisAngle::Identity())
|
|
, x114_(CMatrix3f::Identity())
|
|
, x138_velocity(CVector3f(0.f, 0.f, 0.f))
|
|
, x144_angularVelocity(CAxisAngle::Identity())
|
|
, x150_momentum(moverData.x18_momentum)
|
|
, x15c_force(CVector3f(0.f, 0.f, 0.f))
|
|
, x168_impulse(CVector3f(0.f, 0.f, 0.f))
|
|
, x174_torque(CAxisAngle::Identity())
|
|
, x180_angularImpulse(CAxisAngle::Identity())
|
|
, x18c_moveImpulse(CVector3f(0.f, 0.f, 0.f))
|
|
, x198_moveAngularImpulse(CAxisAngle::Identity())
|
|
, x1a4_baseBoundingBox(aabb)
|
|
, x1c0_collisionPrimitive(aabb, matList)
|
|
, x1e8_primitiveOffset(xf.GetTranslation())
|
|
, x1f4_lastNonCollidingState(xf.GetTranslation(),
|
|
CNUQuaternion::BuildFromMatrix3f(xf.BuildMatrix3f()),
|
|
CVector3f::Zero(), CAxisAngle::Identity())
|
|
, x238_maximumCollisionVelocity(1000000.0)
|
|
, x23c_stepUpHeight(stepUp)
|
|
, x240_stepDownHeight(stepDown)
|
|
, x244_restitutionCoefModifier(0.f)
|
|
, x248_collisionAccuracyModifier(1.f)
|
|
, x24c_numTicksStuck(0)
|
|
, x250_numTicksPartialUpdate(0) {
|
|
SetMass(moverData.x30_mass);
|
|
MoveCollisionPrimitive(CVector3f::Zero());
|
|
SetVelocityOR(moverData.x0_velocity);
|
|
SetAngularVelocityOR(moverData.xc_angularVelocity);
|
|
ComputeDerivedQuantities();
|
|
}
|
|
|
|
CPhysicsActor::~CPhysicsActor() {}
|
|
|
|
void CPhysicsActor::ApplyImpulseWR(const CVector3f& impulse, const CAxisAngle& angularImpulse) {
|
|
x168_impulse = x168_impulse + impulse;
|
|
x180_angularImpulse = x180_angularImpulse + angularImpulse;
|
|
}
|
|
|
|
void CPhysicsActor::ApplyTorqueWR(const CVector3f& torque) {
|
|
x174_torque = x174_torque + CAxisAngle(torque);
|
|
}
|
|
|
|
void CPhysicsActor::ApplyForceWR(const CVector3f& force, const CAxisAngle& torque) {
|
|
x15c_force = x15c_force + force;
|
|
x174_torque = x174_torque + torque;
|
|
}
|
|
|
|
void CPhysicsActor::ApplyImpulseOR(const CVector3f& impulse, const CAxisAngle& angle) {
|
|
x168_impulse = x168_impulse + GetTransform().Rotate(impulse);
|
|
CAxisAngle rotatedAngle(GetTransform().Rotate(angle.GetVector()));
|
|
x180_angularImpulse = x180_angularImpulse + rotatedAngle;
|
|
}
|
|
|
|
void CPhysicsActor::ApplyForceOR(const CVector3f& force, const CAxisAngle& torque) {
|
|
x15c_force = x15c_force + GetTransform().Rotate(force);
|
|
CAxisAngle rotatedTorque(GetTransform().Rotate(torque.GetVector()));
|
|
x174_torque = x174_torque + rotatedTorque;
|
|
}
|
|
|
|
void CPhysicsActor::ComputeDerivedQuantities() {
|
|
x138_velocity = xfc_constantForce * xec_massRecip;
|
|
x114_ = GetTransform().BuildMatrix3f();
|
|
x144_angularVelocity = CAxisAngle(x108_angularMomentum.GetVector() * xf4_inertiaTensorRecip);
|
|
}
|
|
|
|
CPhysicsState CPhysicsActor::GetPhysicsState() const {
|
|
return CPhysicsState(GetTranslation(), GetRotation(), GetConstantForceWR(),
|
|
GetAngularMomentumWR(), GetMomentumWR(), GetForceWR(), GetImpulseWR(),
|
|
GetTorqueWR(), GetAngularImpulseWR());
|
|
}
|
|
|
|
void CPhysicsActor::SetPhysicsState(const CPhysicsState& state) {
|
|
SetTranslation(state.GetTranslation());
|
|
const CQuaternion& quat = state.GetOrientationWR();
|
|
const CVector3f& translation = GetTranslation();
|
|
SetTransform(quat.BuildTransform4f(translation));
|
|
SetConstantForceWR(state.GetConstantForceWR());
|
|
SetAngularMomentumWR(state.GetAngularMomentumWR());
|
|
SetMomentumWR(state.GetMomentumWR());
|
|
SetForceWR(state.GetForceWR());
|
|
SetImpulseWR(state.GetImpulseWR());
|
|
SetTorqueWR(state.GetTorque());
|
|
SetAngularImpulseWR(state.GetAngularImpulseWR());
|
|
ComputeDerivedQuantities();
|
|
}
|
|
|
|
CVector3f CPhysicsActor::CalculateNewVelocityWR_UsingImpulses() const {
|
|
return x138_velocity + xec_massRecip * (x168_impulse + x18c_moveImpulse);
|
|
}
|
|
|
|
CMotionState CPhysicsActor::PredictMotion(float dt) const {
|
|
const CMotionState& msl = PredictLinearMotion(dt);
|
|
CVector3f translation = msl.GetTranslation();
|
|
CVector3f velocity = msl.GetVelocity();
|
|
|
|
const CMotionState& msa = PredictAngularMotion(dt);
|
|
CNUQuaternion orientation = msa.GetOrientation();
|
|
CAxisAngle angularMomentum = msa.GetAngularMomentum();
|
|
|
|
return CMotionState(translation, orientation, velocity, angularMomentum);
|
|
}
|
|
|
|
CMotionState CPhysicsActor::PredictAngularMotion(float dt) const {
|
|
CVector3f v1 = (x180_angularImpulse.GetVector() + x198_moveAngularImpulse.GetVector()) *
|
|
xf4_inertiaTensorRecip;
|
|
CVector3f v2 = x144_angularVelocity.GetVector() + v1;
|
|
|
|
CNUQuaternion q3 = (0.5f * CNUQuaternion(0.f, v2)) *
|
|
CNUQuaternion::BuildFromQuaternion(CQuaternion::FromMatrix(GetTransform()));
|
|
CAxisAngle torque = x174_torque;
|
|
|
|
return CMotionState(CVector3f::Zero(), q3 * dt, CVector3f::Zero(),
|
|
(torque * dt) + x180_angularImpulse);
|
|
}
|
|
|
|
CMotionState CPhysicsActor::PredictLinearMotion(float dt) const {
|
|
CVector3f velocity = CalculateNewVelocityWR_UsingImpulses();
|
|
CVector3f sum = x15c_force + x150_momentum;
|
|
|
|
return CMotionState(dt * velocity, CNUQuaternion(0.0f, CVector3f::Zero()),
|
|
dt * sum + x168_impulse, CAxisAngle::Identity());
|
|
}
|
|
|
|
CMotionState CPhysicsActor::PredictMotion_Internal(float dt) const {
|
|
if (!xf8_25_angularEnabled) {
|
|
|
|
const CMotionState& msl = PredictLinearMotion(dt);
|
|
CVector3f translation = msl.GetTranslation();
|
|
CVector3f velocity = msl.GetVelocity();
|
|
|
|
const CMotionState& msa = PredictAngularMotion(dt);
|
|
CNUQuaternion orientation = msa.GetOrientation();
|
|
CAxisAngle angularMomentum = msa.GetAngularMomentum();
|
|
|
|
return CMotionState(translation, orientation, velocity, angularMomentum);
|
|
} else {
|
|
return PredictLinearMotion(dt);
|
|
}
|
|
}
|
|
|
|
void CPhysicsActor::SetMotionState(const CMotionState& state) {
|
|
const CQuaternion& q = CQuaternion::FromNUQuaternion(state.GetOrientation());
|
|
SetTransform(q.BuildTransform4f(GetTransform().GetTranslation()));
|
|
SetTranslation(state.GetTranslation());
|
|
|
|
xfc_constantForce = state.GetVelocity();
|
|
x108_angularMomentum = state.GetAngularMomentum();
|
|
ComputeDerivedQuantities();
|
|
}
|
|
|
|
CMotionState CPhysicsActor::GetMotionState() const {
|
|
const CNUQuaternion& nquat = CNUQuaternion::BuildFromQuaternion(GetRotation());
|
|
return CMotionState(GetTranslation(), nquat, GetConstantForceWR(), GetAngularMomentumWR());
|
|
}
|
|
|
|
void CPhysicsActor::AddMotionState(const CMotionState& state) {
|
|
CNUQuaternion q(CNUQuaternion::BuildFromMatrix3f(GetTransform().BuildMatrix3f()));
|
|
q += state.GetOrientation();
|
|
const CQuaternion& quat = CQuaternion::FromNUQuaternion(q);
|
|
|
|
CVector3f transPos = GetTransform().GetTranslation();
|
|
SetTransform(quat.BuildTransform4f(transPos));
|
|
|
|
transPos += state.GetTranslation();
|
|
SetTranslation(transPos);
|
|
|
|
xfc_constantForce += state.GetVelocity();
|
|
x108_angularMomentum += state.GetAngularMomentum();
|
|
|
|
ComputeDerivedQuantities();
|
|
|
|
}
|
|
|
|
bool CPhysicsActor::WillMove(const CStateManager& mgr) {
|
|
if (close_enough(x138_velocity, CVector3f::Zero()) &&
|
|
close_enough(x168_impulse, CVector3f::Zero()) &&
|
|
close_enough(x174_torque.GetVector(), CVector3f::Zero()) &&
|
|
close_enough(x18c_moveImpulse, CVector3f::Zero()) &&
|
|
close_enough(x144_angularVelocity.GetVector(), CVector3f::Zero()) &&
|
|
close_enough(x180_angularImpulse.GetVector(), CVector3f::Zero()) &&
|
|
close_enough(x198_moveAngularImpulse.GetVector(), CVector3f::Zero()) &&
|
|
close_enough(GetTotalForceWR(), CVector3f::Zero())) {
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void CPhysicsActor::Stop() {
|
|
ClearForcesAndTorques();
|
|
xfc_constantForce = CVector3f::Zero();
|
|
x108_angularMomentum = CAxisAngle::Identity();
|
|
ComputeDerivedQuantities();
|
|
}
|
|
|
|
void CPhysicsActor::ClearForcesAndTorques() {
|
|
x15c_force = x168_impulse = x18c_moveImpulse = CVector3f::Zero();
|
|
x174_torque = x180_angularImpulse = x198_moveAngularImpulse = CAxisAngle::Identity();
|
|
}
|
|
|
|
void CPhysicsActor::ClearImpulses() {
|
|
x168_impulse = x18c_moveImpulse = CVector3f::Zero();
|
|
x180_angularImpulse = x198_moveAngularImpulse = CAxisAngle::Identity();
|
|
}
|
|
|
|
void CPhysicsActor::UseCollisionImpulses() {
|
|
xfc_constantForce += x168_impulse;
|
|
x108_angularMomentum += x180_angularImpulse;
|
|
x168_impulse = CVector3f::Zero();
|
|
x180_angularImpulse = CAxisAngle::Identity();
|
|
ComputeDerivedQuantities();
|
|
}
|
|
|
|
void CPhysicsActor::MoveToWR(const CVector3f& trans, float d) {
|
|
xfc_constantForce = (trans - GetTransform().GetTranslation()) * GetMass() / d;
|
|
ComputeDerivedQuantities();
|
|
}
|
|
|
|
void CPhysicsActor::MoveToInOneFrameWR(const CVector3f& trans, float d) {
|
|
x18c_moveImpulse += (trans - GetTranslation()) * GetMass() / d;
|
|
}
|
|
|
|
CVector3f CPhysicsActor::GetMoveToORImpulseWR(const CVector3f& trans, float d) const {
|
|
CVector3f impulse = GetTransform().Rotate(trans);
|
|
return (GetMass() * impulse) / d;
|
|
}
|
|
|
|
CVector3f CPhysicsActor::GetRotateToORAngularMomentumWR(const CQuaternion& q, float d) const {
|
|
if (q.GetW() > 0.99999976f) {
|
|
return CVector3f::Zero();
|
|
} else {
|
|
const CVector3f rotated = GetTransform().Rotate(q.GetImaginary());
|
|
|
|
float ac = acos(q.GetW());
|
|
return rotated.AsNormalized() * ((ac * 2.0f) * (1.0f / d)) * xf0_inertiaTensor;
|
|
}
|
|
}
|
|
|
|
void CPhysicsActor::MoveToOR(const CVector3f& trans, float d) {
|
|
xfc_constantForce = GetMoveToORImpulseWR(trans, d);
|
|
ComputeDerivedQuantities();
|
|
}
|
|
|
|
void CPhysicsActor::RotateToOR(const CQuaternion& q, float d) {
|
|
const CVector3f& vec = GetRotateToORAngularMomentumWR(q, d);
|
|
x108_angularMomentum = CAxisAngle(vec);
|
|
ComputeDerivedQuantities();
|
|
}
|
|
|
|
void CPhysicsActor::MoveInOneFrameOR(const CVector3f& trans, float d) {
|
|
x18c_moveImpulse += GetMoveToORImpulseWR(trans, d);
|
|
}
|
|
|
|
void CPhysicsActor::RotateInOneFrameOR(const CQuaternion& q, float d) {
|
|
const CVector3f& vec = GetRotateToORAngularMomentumWR(q, d);
|
|
x198_moveAngularImpulse += CAxisAngle(vec);
|
|
}
|
|
|
|
void CPhysicsActor::SetVelocityOR(const CVector3f& vel) {
|
|
SetVelocityWR(GetTransform().Rotate(vel));
|
|
}
|
|
|
|
CVector3f CPhysicsActor::GetTotalForceWR() const { return x15c_force + x150_momentum; }
|
|
|
|
void CPhysicsActor::SetVelocityWR(const CVector3f& vel) {
|
|
x138_velocity = vel;
|
|
xfc_constantForce = xe8_mass * x138_velocity;
|
|
}
|
|
|
|
void CPhysicsActor::SetAngularVelocityWR(const CAxisAngle& angVel) {
|
|
x144_angularVelocity = angVel;
|
|
x108_angularMomentum = CAxisAngle(x144_angularVelocity.GetVector() * xf0_inertiaTensor);
|
|
}
|
|
|
|
CAxisAngle CPhysicsActor::GetAngularVelocityOR() const {
|
|
return CAxisAngle(GetTransform().TransposeRotate(x144_angularVelocity.GetVector()));
|
|
}
|
|
|
|
void CPhysicsActor::SetAngularVelocityOR(const CAxisAngle& angVel) {
|
|
x144_angularVelocity = CAxisAngle(GetTransform().Rotate(angVel.GetVector()));
|
|
x108_angularMomentum = CAxisAngle(x144_angularVelocity.GetVector() * xf0_inertiaTensor);
|
|
}
|
|
|
|
void CPhysicsActor::SetMass(float mass) {
|
|
xe8_mass = mass;
|
|
xec_massRecip = (xe8_mass > 0.0f) ? (1.0f / xe8_mass) : 1.0f;
|
|
SetInertiaTensorScalar(0.16666667f * xe8_mass);
|
|
}
|
|
|
|
void CPhysicsActor::SetInertiaTensorScalar(float tensor) {
|
|
xf0_inertiaTensor = (tensor > 0.0f) ? tensor : 1.0f;
|
|
xf4_inertiaTensorRecip = 1.0f / xf0_inertiaTensor;
|
|
}
|
|
|
|
const CCollisionPrimitive* CPhysicsActor::GetCollisionPrimitive() const {
|
|
return &x1c0_collisionPrimitive;
|
|
}
|
|
|
|
void CPhysicsActor::MoveCollisionPrimitive(const CVector3f& offset) {
|
|
x1e8_primitiveOffset = offset;
|
|
}
|
|
|
|
CTransform4f CPhysicsActor::GetPrimitiveTransform() const {
|
|
return CTransform4f::Translate(GetTransform().GetTranslation() + x1e8_primitiveOffset);
|
|
}
|
|
|
|
void CPhysicsActor::CollidedWith(const TUniqueId& id, const CCollisionInfoList& list,
|
|
CStateManager& mgr) {}
|
|
|
|
const CAABox& CPhysicsActor::GetBaseBoundingBox() const { return x1a4_baseBoundingBox; }
|
|
|
|
CAABox CPhysicsActor::GetBoundingBox() const {
|
|
CVector3f off = x1e8_primitiveOffset + GetTransform().GetTranslation();
|
|
return CAABox(x1a4_baseBoundingBox.GetMinPoint() + off, x1a4_baseBoundingBox.GetMaxPoint() + off);
|
|
}
|
|
|
|
CAABox CPhysicsActor::GetMotionVolume(float dt) const {
|
|
CAABox aabox = GetCollisionPrimitive()->CalculateAABox(GetPrimitiveTransform());
|
|
CVector3f velocity = CalculateNewVelocityWR_UsingImpulses();
|
|
|
|
const CVector3f dv = (dt * velocity);
|
|
aabox.AccumulateBounds(aabox.GetMaxPoint() + dv);
|
|
aabox.AccumulateBounds(aabox.GetMinPoint() + dv);
|
|
|
|
float up = rstl::max_val(GetStepUpHeight(), 0.f);
|
|
aabox.AccumulateBounds(aabox.GetMaxPoint() + CVector3f(0.5f, 0.5f, up + 1.f));
|
|
|
|
float down = rstl::max_val(GetStepDownHeight(), 0.f);
|
|
aabox.AccumulateBounds(aabox.GetMinPoint() - CVector3f(0.5f, 0.5f, down + 1.5f));
|
|
return aabox;
|
|
}
|
|
|
|
void CPhysicsActor::SetBoundingBox(const CAABox& box) {
|
|
x1a4_baseBoundingBox = box;
|
|
MoveCollisionPrimitive(CVector3f::Zero());
|
|
}
|
|
|
|
float CPhysicsActor::GetWeight() const { return CPhysicsActor::GravityConstant() * GetMass(); }
|
|
|
|
CVector3f CPhysicsActor::GetPrimitiveOffset() const { return x1e8_primitiveOffset; }
|
|
|
|
float CPhysicsActor::GetStepDownHeight() const { return x240_stepDownHeight; }
|
|
|
|
float CPhysicsActor::GetStepUpHeight() const { return x23c_stepUpHeight; }
|
|
|
|
CVector3f CPhysicsActor::GetOrbitPosition(const CStateManager&) const {
|
|
return GetBoundingBox().GetCenterPoint();
|
|
}
|
|
|
|
CVector3f CPhysicsActor::GetAimPosition(const CStateManager&, float dt) const {
|
|
if (dt > 0.0f) {
|
|
CVector3f trans = PredictMotion(dt).GetTranslation();
|
|
return GetBoundingBox().GetCenterPoint() + trans;
|
|
} else {
|
|
return GetBoundingBox().GetCenterPoint();
|
|
}
|
|
}
|
|
|
|
void CPhysicsActor::Render(const CStateManager& mgr) const { CActor::Render(mgr); }
|
|
|
|
void CPhysicsActor::SetCoefficientOfRestitutionModifier(float modifier) {
|
|
x244_restitutionCoefModifier = modifier;
|
|
}
|
|
|
|
float CPhysicsActor::GetCoefficientOfRestitutionModifier() const {
|
|
return x244_restitutionCoefModifier;
|
|
}
|
|
|
|
float CPhysicsActor::GetCollisionAccuracyModifier() const { return x248_collisionAccuracyModifier; }
|
|
|
|
void CPhysicsActor::SetCollisionAccuracyModifier(float modifier) {
|
|
x248_collisionAccuracyModifier = modifier;
|
|
}
|
|
|
|
float CPhysicsActor::GetMaximumCollisionVelocity() const { return x238_maximumCollisionVelocity; }
|
|
|
|
void CPhysicsActor::SetMaxVelocityAfterCollision(float velocity) {
|
|
x238_maximumCollisionVelocity = velocity;
|
|
}
|