ksys/phys: Add a bunch of easy RigidBody functions

data/uking_functions.csv

@@ -2339,7 +2339,7 @@ Address,Quality,Size,Name
 0x000000710007a6fc,U,000012,Motorcycle::x_6
 0x000000710007a708,U,000068,Motorcycle::x_4
 0x000000710007a74c,U,000076,
-0x000000710007a798,U,000180,_ZN3agl3utl20ParameterDirection3f28listenPropertyEventParameterEPN4sead6hostio10ReflexibleEPKNS3_13PropertyEventE
+0x000000710007a798,U,000180,
 0x000000710007a84c,U,000064,Motorcycle::setLeftStickX
 0x000000710007a88c,U,000064,Motorcycle::setAccelMaybe
 0x000000710007a8cc,U,000008,Motorcycle::setLeftStickY
@@ -2352,14 +2352,14 @@ Address,Quality,Size,Name
 0x000000710007a9c8,U,000172,Motorcycle::speedStuff
 0x000000710007aa74,U,000044,Motorcycle::x_8
 0x000000710007aaa0,U,000220,
-0x000000710007ab7c,U,000040,_ZNK3aal11Sound2DPlot33calcRollOffCurveHalfValueDistanceERKNS_12RollOffCurveE
+0x000000710007ab7c,U,000040,
 0x000000710007aba4,U,000032,
 0x000000710007abc4,U,000712,Motorcycle::speedStuff_0
 0x000000710007ae8c,U,000676,Motorcycle::x_21
 0x000000710007b130,U,001172,
 0x000000710007b5c4,U,000200,
 0x000000710007b68c,U,000008,Motorcycle::x_1
-0x000000710007b694,U,000356,_ZN3aal15SoundController10initializeEPN4sead4HeapE
+0x000000710007b694,U,000356,
 0x000000710007b7f8,U,001172,
 0x000000710007bc8c,U,000772,
 0x000000710007bf90,U,000124,Motorcycle::speedStuff_1
@@ -82977,22 +82977,22 @@ Address,Quality,Size,Name
 0x0000007100f8d210,O,000012,_ZNK4ksys4phys9RigidBody16isMotionFlag2SetEv
 0x0000007100f8d21c,O,000236,_ZN4ksys4phys9RigidBody14sub_7100F8D21CEv
 0x0000007100f8d308,U,000888,phys::RigidBody::x_6
-0x0000007100f8d680,U,000140,phys::RigidBody::getMotionAccessorType1
+0x0000007100f8d680,O,000140,_ZNK4ksys4phys9RigidBody17getMotionAccessorEv
-0x0000007100f8d70c,U,000156,phys::RigidBody::getMotionAccessorType2
+0x0000007100f8d70c,O,000156,_ZNK4ksys4phys9RigidBody18getLinkedRigidBodyEv
-0x0000007100f8d7a8,U,000152,phys::RigidBody::motionAccessorType2Stuff2
+0x0000007100f8d7a8,O,000152,_ZNK4ksys4phys9RigidBody20resetLinkedRigidBodyEv
 0x0000007100f8d840,U,001156,phys::RigidBody::x_8
 0x0000007100f8dcc4,O,000056,_ZNK4ksys4phys9RigidBody13getMotionTypeEv
 0x0000007100f8dcfc,U,001044,phys::RigidBody::x_9
 0x0000007100f8e110,U,000748,phys::RigidBody::x_10
 0x0000007100f8e3fc,U,000816,phys::RigidBody::x_11
 0x0000007100f8e72c,U,000136,phys::RigidBody::x_12
-0x0000007100f8e7b4,U,000052,RigidBody::registerContactPoints
+0x0000007100f8e7b4,O,000052,_ZN4ksys4phys9RigidBody16setContactPointsEPNS0_18RigidContactPointsE
 0x0000007100f8e7e8,U,000724,phys::RigidBody::x_13
 0x0000007100f8e8f0,U,000460,phys::RigidBody::x_14
 0x0000007100f8eabc,U,000384,phys::RigidBody::x_15
-0x0000007100f8ec3c,U,000312,RigidBody::setLinearVelocity
+0x0000007100f8ec3c,O,000312,_ZN4ksys4phys9RigidBody17setLinearVelocityERKN4sead7Vector3IfEEf
-0x0000007100f8ed74,U,000196,RigidBody::setAngularVelocity
+0x0000007100f8ed74,O,000196,_ZN4ksys4phys9RigidBody18setAngularVelocityERKN4sead7Vector3IfEEf
-0x0000007100f8ee38,U,000024,phys::RigidBody::x_16
+0x0000007100f8ee38,O,000024,_ZN4ksys4phys9RigidBody16resetFrozenStateEv
 0x0000007100f8ee50,U,000048,phys::RigidBody::x_17
 0x0000007100f8ee80,U,000384,phys::RigidBody::x_2
 0x0000007100f8f000,U,000012,phys::RigidBody::x_18
@@ -83026,25 +83026,25 @@ Address,Quality,Size,Name
 0x0000007100f8fd34,U,000012,phys::RigidBody::x_36
 0x0000007100f8fd40,U,000124,phys::RigidBody::x_37
 0x0000007100f8fdbc,U,000328,phys::RigidBody::x_38
-0x0000007100f8ff04,U,000032,phys::RigidBody::x_39_suspendMaybe
+0x0000007100f8ff04,O,000032,_ZNK4ksys4phys9RigidBody11getPositionEPN4sead7Vector3IfEE
-0x0000007100f8ff24,U,000076,phys::RigidBody::getPosition
+0x0000007100f8ff24,O,000076,_ZNK4ksys4phys9RigidBody11getPositionEv
-0x0000007100f8ff70,U,000032,phys::RigidBody::getRotation
+0x0000007100f8ff70,O,000032,_ZNK4ksys4phys9RigidBody11getRotationEPN4sead4QuatIfEE
-0x0000007100f8ff90,U,000076,phys::RigidBody::getRotation_0
+0x0000007100f8ff90,O,000076,_ZNK4ksys4phys9RigidBody11getRotationEv
-0x0000007100f8ffdc,U,000112,phys::RigidBody::getPositionAndRotation
+0x0000007100f8ffdc,O,000112,_ZNK4ksys4phys9RigidBody22getPositionAndRotationEPN4sead7Vector3IfEEPNS2_4QuatIfEE
-0x0000007100f9004c,U,000032,phys::RigidBody::getTransform
+0x0000007100f9004c,O,000032,_ZNK4ksys4phys9RigidBody12getTransformEPN4sead8Matrix34IfEE
-0x0000007100f9006c,U,000040,phys::RigidBody::getTransform_0
+0x0000007100f9006c,O,000040,_ZNK4ksys4phys9RigidBody12getTransformEv
 0x0000007100f90094,U,000968,phys::RigidBody::x_3
 0x0000007100f9045c,U,001132,phys::RigidBody::x_39
 0x0000007100f908c8,U,001632,phys::RigidBody::x_40
-0x0000007100f90f28,U,000140,phys::RigidBody::getMotionAccessorForProxy
+0x0000007100f90f28,O,000140,_ZNK4ksys4phys9RigidBody25getMotionAccessorForProxyEv
 0x0000007100f90fb4,U,000332,phys::RigidBody::x_41
 0x0000007100f91100,U,000140,phys::RigidBody::x_42
-0x0000007100f9118c,U,000032,phys::RigidBody::getLinearVelocity
+0x0000007100f9118c,O,000032,_ZNK4ksys4phys9RigidBody17getLinearVelocityEPN4sead7Vector3IfEE
-0x0000007100f911ac,U,000076,phys::RigidBody::getLinearVelocity_0
+0x0000007100f911ac,O,000076,_ZNK4ksys4phys9RigidBody17getLinearVelocityEv
-0x0000007100f911f8,U,000032,phys::RigidBody::getAngularVelocity
+0x0000007100f911f8,O,000032,_ZNK4ksys4phys9RigidBody18getAngularVelocityEPN4sead7Vector3IfEE
-0x0000007100f91218,U,000076,phys::RigidBody::getAngularVelocity_0
+0x0000007100f91218,O,000076,_ZNK4ksys4phys9RigidBody18getAngularVelocityEv
-0x0000007100f91264,U,000276,phys::RigidBody::getLinearAndAngularVelocity
+0x0000007100f91264,O,000276,_ZNK4ksys4phys9RigidBody16getPointVelocityEPN4sead7Vector3IfEERKS4_
-0x0000007100f91378,U,000040,phys::RigidBody::x_44
+0x0000007100f91378,O,000040,_ZNK4ksys4phys9RigidBody22getCenterOfMassInWorldEv
 0x0000007100f913a0,U,000992,phys::RigidBody::x_45_moveToHomeMtxMaybe
 0x0000007100f91780,U,000672,phys::RigidBody::x_46
 0x0000007100f91a20,U,000580,phys::RigidBody::x_47
@@ -83055,20 +83055,20 @@ Address,Quality,Size,Name
 0x0000007100f92758,U,000528,phys::RigidBody::x_52
 0x0000007100f92968,U,000524,phys::RigidBody::x_53
 0x0000007100f92b74,U,000140,phys::RigidBody::x_54
-0x0000007100f92c00,U,000128,phys::RigidBody::x_55
+0x0000007100f92c00,O,000128,_ZN4ksys4phys9RigidBody22setCenterOfMassInLocalERKN4sead7Vector3IfEE
-0x0000007100f92c80,U,000016,phys::RigidBody::x_56
+0x0000007100f92c80,O,000016,_ZNK4ksys4phys9RigidBody22getCenterOfMassInLocalEPN4sead7Vector3IfEE
-0x0000007100f92c90,U,000052,phys::RigidBody::x_57
+0x0000007100f92c90,O,000052,_ZNK4ksys4phys9RigidBody22getCenterOfMassInLocalEv
-0x0000007100f92cc4,U,000272,phys::RigidBody::x_58
+0x0000007100f92cc4,O,000272,_ZNK4ksys4phys9RigidBody22getCenterOfMassInWorldEPN4sead7Vector3IfEE
-0x0000007100f92dd4,U,000124,phys::RigidBody::x_59
+0x0000007100f92dd4,O,000124,_ZN4ksys4phys9RigidBody20setMaxLinearVelocityEf
-0x0000007100f92e50,U,000016,phys::RigidBody::x_60
+0x0000007100f92e50,O,000016,_ZNK4ksys4phys9RigidBody20getMaxLinearVelocityEv
-0x0000007100f92e60,U,000124,phys::RigidBody::x_61
+0x0000007100f92e60,O,000124,_ZN4ksys4phys9RigidBody21setMaxAngularVelocityEf
-0x0000007100f92edc,U,000016,phys::RigidBody::x_62
+0x0000007100f92edc,O,000016,_ZNK4ksys4phys9RigidBody21getMaxAngularVelocityEv
-0x0000007100f92eec,U,000348,phys::RigidBody::x_63
+0x0000007100f92eec,O,000348,_ZN4ksys4phys9RigidBody18applyLinearImpulseERKN4sead7Vector3IfEE
-0x0000007100f93048,U,000348,phys::RigidBody::x_64
+0x0000007100f93048,O,000348,_ZN4ksys4phys9RigidBody19applyAngularImpulseERKN4sead7Vector3IfEE
-0x0000007100f931a4,U,000420,phys::RigidBody::x_65
+0x0000007100f931a4,O,000420,_ZN4ksys4phys9RigidBody17applyPointImpulseERKN4sead7Vector3IfEES6_
-0x0000007100f93348,U,000180,phys::RigidBody::x_66_setMass
+0x0000007100f93348,O,000180,_ZN4ksys4phys9RigidBody7setMassEf
-0x0000007100f933fc,U,000156,phys::RigidBody::x_67_getMass
+0x0000007100f933fc,O,000156,_ZNK4ksys4phys9RigidBody7getMassEv
-0x0000007100f93498,U,000156,phys::RigidBody::x_68
+0x0000007100f93498,O,000156,_ZNK4ksys4phys9RigidBody10getMassInvEv
 0x0000007100f93534,U,000168,phys::RigidBody::x_69_setInertiaLocal
 0x0000007100f935dc,U,000176,phys::RigidBody::x_70
 0x0000007100f9368c,U,000196,phys::RigidBody::x_71_getInertia
@@ -83132,8 +83132,8 @@ Address,Quality,Size,Name
 0x0000007100f96700,U,000068,phys::RigidBody::m5
 0x0000007100f96744,U,000176,phys::RigidBody::x_118
 0x0000007100f967f4,U,000428,phys::RigidBody::x_119
-0x0000007100f969a0,U,000072,phys::RigidBody::lock
+0x0000007100f969a0,O,000072,_ZN4ksys4phys9RigidBody4lockEb
-0x0000007100f969e8,U,000088,phys::RigidBody::unlock
+0x0000007100f969e8,O,000088,_ZN4ksys4phys9RigidBody6unlockEb
 0x0000007100f96a40,O,000012,_ZNK4ksys4phys9RigidBody9getMotionEv
 0x0000007100f96a4c,U,000076,phys::RigidBody::x_123
 0x0000007100f96a98,U,000188,phys::RigidBody::x_124
@@ -83474,7 +83474,7 @@ Address,Quality,Size,Name
 0x0000007100fa7ca4,U,001800,phys::RigidBodyRequestMgr::x_12
 0x0000007100fa83ac,U,000560,phys::RigidBodyRequestMgr::x_13
 0x0000007100fa85dc,U,001548,
-0x0000007100fa8be8,U,000072,
+0x0000007100fa8be8,O,000072,_ZN4ksys4phys19RigidBodyRequestMgr6Config23isLinearVelocityTooHighERKN4sead7Vector3IfEE
 0x0000007100fa8c30,U,000004,j_phys::RigidBodyRequestMgr::x_11
 0x0000007100fa8c34,U,000008,
 0x0000007100fa8c3c,U,000008,
@@ -83494,8 +83494,8 @@ Address,Quality,Size,Name
 0x0000007100fa8ea4,U,000180,phys::RigidBodyGroup::x_7
 0x0000007100fa8f58,U,000072,phys::RigidBodyGroup::x_8
 0x0000007100fa8fa0,U,000080,phys::RigidBodyGroup::x_9
-0x0000007100fa8ff0,U,000056,_ZN4gsys15SnapshotTexture14requestDrawAllEv
+0x0000007100fa8ff0,U,000056,
-0x0000007100fa9028,U,000056,_ZNK3agl4lght11LightMapMgr9updateGPUEv
+0x0000007100fa9028,U,000056,
 0x0000007100fa9060,U,000084,phys::RigidBodyGroup::x_3
 0x0000007100fa90b4,U,000328,phys::RigidBodyGroup::x_0
 0x0000007100fa91fc,U,000308,phys::RigidBodyGroup::x_1

lib/sead

@@ -1 +1 @@
-Subproject commit 571ffe5b5968d1b6fee06907390895518404ea3e
+Subproject commit 6cdaef9d2dc1141e49e7fa98c47eea53345038a8

src/KingSystem/ActorSystem/actPhysicsUserTag.h

@@ -23,7 +23,7 @@ public:
     void m4() override;
     void m5() override;
     const sead::SafeString& getName() const override;
-    void m7() override;
+    void m7(phys::RigidBody* rigid_body, int a) override;
     const sead::SafeString& getName2() const override;
 
 private:

src/KingSystem/Physics/RigidBody/physRigidBody.cpp

@@ -3,18 +3,28 @@
 #include <Havok/Physics2012/Dynamics/Entity/hkpRigidBody.h>
 #include <Havok/Physics2012/Dynamics/Motion/Rigid/hkpFixedRigidMotion.h>
 #include <Havok/Physics2012/Dynamics/Motion/Rigid/hkpKeyframedRigidMotion.h>
+#include <cmath>
 #include "KingSystem/Physics/RigidBody/physMotionAccessor.h"
 #include "KingSystem/Physics/RigidBody/physRigidBodyMotion.h"
 #include "KingSystem/Physics/RigidBody/physRigidBodyMotionProxy.h"
 #include "KingSystem/Physics/RigidBody/physRigidBodyParam.h"
 #include "KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h"
 #include "KingSystem/Physics/System/physMemSystem.h"
+#include "KingSystem/Physics/System/physUserTag.h"
 #include "KingSystem/Physics/physConversions.h"
 
 namespace ksys::phys {
 
 constexpr float MinInertia = 0.001;
 
+static bool isVectorInvalid(const sead::Vector3f& vec) {
+    for (int i = 0; i < 3; ++i) {
+        if (std::isnan(vec.e[i]))
+            return true;
+    }
+    return false;
+}
+
 RigidBody::RigidBody(Type type, u32 mass_scaling, hkpRigidBody* hk_body,
                      const sead::SafeString& name, sead::Heap* heap, bool a7)
     : mCS(heap), mHkBody(hk_body), mRigidBodyAccessor(hk_body), mType(type) {
@@ -204,6 +214,36 @@ void RigidBody::sub_7100F8D21C() {
     }
 }
 
+RigidBodyMotion* RigidBody::getMotionAccessor() const {
+    return sead::DynamicCast<RigidBodyMotion>(mMotionAccessor);
+}
+
+RigidBodyMotion* RigidBody::getMotionAccessorForProxy() const {
+    return getMotionAccessor();
+}
+
+RigidBodyMotionProxy* RigidBody::getMotionProxy() const {
+    if (!isMassScaling())
+        return nullptr;
+    if (!mMotionAccessor)
+        return nullptr;
+    return sead::DynamicCast<RigidBodyMotionProxy>(mMotionAccessor);
+}
+
+RigidBody* RigidBody::getLinkedRigidBody() const {
+    auto* proxy = getMotionProxy();
+    if (!proxy)
+        return nullptr;
+    return proxy->getLinkedRigidBody();
+}
+
+void RigidBody::resetLinkedRigidBody() const {
+    auto* proxy = getMotionProxy();
+    if (!proxy)
+        return;
+    proxy->resetLinkedRigidBody();
+}
+
 MotionType RigidBody::getMotionType() const {
     if (mMotionFlags.isOn(MotionFlag::Dynamic))
         return MotionType::Dynamic;
@@ -214,6 +254,17 @@ MotionType RigidBody::getMotionType() const {
     return mRigidBodyAccessor.getMotionType();
 }
 
+void RigidBody::setContactPoints(RigidContactPoints* points) {
+    mContactPoints = points;
+    if (isFlag8Set() && mContactPoints && !mContactPoints->isLinked())
+        MemSystem::instance()->registerContactPoints(points);
+}
+
+void RigidBody::resetFrozenState() {
+    if (mMotionAccessor)
+        mMotionAccessor->resetFrozenState();
+}
+
 void RigidBody::setContactMask(u32 value) {
     mContactMask.setDirect(value);
 }
@@ -226,8 +277,260 @@ void RigidBody::setContactNone() {
     mContactMask.makeAllZero();
 }
 
+void RigidBody::getPosition(sead::Vector3f* position) const {
+    if (mMotionAccessor)
+        mMotionAccessor->getPosition(position);
+    else
+        mRigidBodyAccessor.getPosition(position);
+}
+
+sead::Vector3f RigidBody::getPosition() const {
+    sead::Vector3f position;
+    getPosition(&position);
+    return position;
+}
+
+void RigidBody::getRotation(sead::Quatf* rotation) const {
+    if (mMotionAccessor)
+        mMotionAccessor->getRotation(rotation);
+    else
+        mRigidBodyAccessor.getRotation(rotation);
+}
+
+sead::Quatf RigidBody::getRotation() const {
+    sead::Quatf rotation;
+    getRotation(&rotation);
+    return rotation;
+}
+
+void RigidBody::getPositionAndRotation(sead::Vector3f* position, sead::Quatf* rotation) const {
+    getPosition(position);
+    getRotation(rotation);
+}
+
+void RigidBody::getTransform(sead::Matrix34f* mtx) const {
+    if (mMotionAccessor)
+        mMotionAccessor->getTransform(mtx);
+    else
+        mRigidBodyAccessor.getTransform(mtx);
+}
+
+sead::Matrix34f RigidBody::getTransform() const {
+    sead::Matrix34f transform;
+    getTransform(&transform);
+    return transform;
+}
+
+bool RigidBody::setLinearVelocity(const sead::Vector3f& velocity, float epsilon) {
+    if (isVectorInvalid(velocity)) {
+        onInvalidParameter();
+        return false;
+    }
+
+    if (!isMassScaling() && RigidBodyRequestMgr::Config::isLinearVelocityTooHigh(velocity)) {
+        onInvalidParameter(1);
+        return false;
+    }
+
+    return mMotionAccessor->setLinearVelocity(velocity, epsilon);
+}
+
+void RigidBody::getLinearVelocity(sead::Vector3f* velocity) const {
+    if (mMotionAccessor)
+        mMotionAccessor->getLinearVelocity(velocity);
+    else
+        mRigidBodyAccessor.getLinearVelocity(velocity);
+}
+
+sead::Vector3f RigidBody::getLinearVelocity() const {
+    sead::Vector3f v;
+    getLinearVelocity(&v);
+    return v;
+}
+
+bool RigidBody::setAngularVelocity(const sead::Vector3f& velocity, float epsilon) {
+    if (isVectorInvalid(velocity)) {
+        onInvalidParameter();
+        return false;
+    }
+
+    return mMotionAccessor->setAngularVelocity(velocity, epsilon);
+}
+
+void RigidBody::getAngularVelocity(sead::Vector3f* velocity) const {
+    if (mMotionAccessor)
+        mMotionAccessor->getAngularVelocity(velocity);
+    else
+        mRigidBodyAccessor.getAngularVelocity(velocity);
+}
+
+sead::Vector3f RigidBody::getAngularVelocity() const {
+    sead::Vector3f v;
+    getAngularVelocity(&v);
+    return v;
+}
+
+void RigidBody::getPointVelocity(sead::Vector3f* velocity, const sead::Vector3f& point) const {
+    const auto rel_pos = point - getCenterOfMassInWorld();
+    velocity->setCross(getAngularVelocity(), rel_pos);
+    velocity->add(getLinearVelocity());
+}
+
+void RigidBody::setCenterOfMassInLocal(const sead::Vector3f& center) {
+    sead::Vector3f current_center;
+    mMotionAccessor->getCenterOfMassInLocal(&current_center);
+    if (current_center != center)
+        mMotionAccessor->setCenterOfMassInLocal(center);
+}
+
+void RigidBody::getCenterOfMassInLocal(sead::Vector3f* center) const {
+    mMotionAccessor->getCenterOfMassInLocal(center);
+}
+
+sead::Vector3f RigidBody::getCenterOfMassInLocal() const {
+    sead::Vector3f center;
+    getCenterOfMassInLocal(&center);
+    return center;
+}
+
+void RigidBody::getCenterOfMassInWorld(sead::Vector3f* center) const {
+    if (mMotionFlags.isAnyOn({MotionFlag::DirtyCenterOfMassLocal, MotionFlag::DirtyTransform})) {
+        sead::Vector3f local_center;
+        getCenterOfMassInLocal(&local_center);
+
+        sead::Matrix34f transform;
+        getTransform(&transform);
+
+        center->setMul(transform, local_center);
+    } else {
+        auto hk_center = getMotion()->getCenterOfMassInWorld();
+        storeToVec3(center, hk_center);
+    }
+}
+
+sead::Vector3f RigidBody::getCenterOfMassInWorld() const {
+    sead::Vector3f center;
+    getCenterOfMassInWorld(&center);
+    return center;
+}
+
+void RigidBody::setMaxLinearVelocity(float max) {
+    if (!sead::Mathf::equalsEpsilon(max, getMaxLinearVelocity()))
+        mMotionAccessor->setMaxLinearVelocity(max);
+}
+
+float RigidBody::getMaxLinearVelocity() const {
+    return mMotionAccessor->getMaxLinearVelocity();
+}
+
+void RigidBody::setMaxAngularVelocity(float max) {
+    if (!sead::Mathf::equalsEpsilon(max, getMaxAngularVelocity()))
+        mMotionAccessor->setMaxAngularVelocity(max);
+}
+
+float RigidBody::getMaxAngularVelocity() const {
+    return mMotionAccessor->getMaxAngularVelocity();
+}
+
+void RigidBody::applyLinearImpulse(const sead::Vector3f& impulse) {
+    if (MemSystem::instance()->isPaused())
+        return;
+
+    if (hasFlag(Flag::_400) || hasFlag(Flag::_40))
+        return;
+
+    if (isVectorInvalid(impulse)) {
+        onInvalidParameter();
+        return;
+    }
+
+    if (!isMassScaling())
+        getMotionAccessor()->applyLinearImpulse(impulse);
+}
+
+void RigidBody::applyAngularImpulse(const sead::Vector3f& impulse) {
+    if (MemSystem::instance()->isPaused())
+        return;
+
+    if (hasFlag(Flag::_400) || hasFlag(Flag::_40))
+        return;
+
+    if (isVectorInvalid(impulse)) {
+        onInvalidParameter();
+        return;
+    }
+
+    if (!isMassScaling())
+        getMotionAccessor()->applyAngularImpulse(impulse);
+}
+
+void RigidBody::applyPointImpulse(const sead::Vector3f& impulse, const sead::Vector3f& point) {
+    if (MemSystem::instance()->isPaused())
+        return;
+
+    if (hasFlag(Flag::_400) || hasFlag(Flag::_40))
+        return;
+
+    if (isVectorInvalid(impulse)) {
+        onInvalidParameter();
+        return;
+    }
+
+    if (isVectorInvalid(point)) {
+        onInvalidParameter();
+        return;
+    }
+
+    if (!isMassScaling())
+        getMotionAccessor()->applyPointImpulse(impulse, point);
+}
+
+void RigidBody::setMass(float mass) {
+    if (isMassScaling())
+        return;
+    getMotionAccessor()->setMass(mass);
+}
+
+float RigidBody::getMass() const {
+    if (isMassScaling())
+        return 0.0;
+    return getMotionAccessor()->getMass();
+}
+
+float RigidBody::getMassInv() const {
+    if (isMassScaling())
+        return 0.0;
+    return getMotionAccessor()->getMassInv();
+}
+
+void RigidBody::lock(bool also_lock_world) {
+    if (also_lock_world)
+        MemSystem::instance()->lockWorld(getLayerType());
+    mCS.lock();
+}
+
+void RigidBody::unlock(bool also_unlock_world) {
+    mCS.unlock();
+    if (also_unlock_world)
+        MemSystem::instance()->unlockWorld(getLayerType());
+}
+
 hkpMotion* RigidBody::getMotion() const {
     return getHkBody()->getMotion();
 }
 
+void RigidBody::onInvalidParameter(int code) {
+    sead::Vector3f pos, lin_vel, ang_vel;
+    mRigidBodyAccessor.getPosition(&pos);
+    mRigidBodyAccessor.getLinearVelocity(&lin_vel);
+    mRigidBodyAccessor.getAngularVelocity(&ang_vel);
+    // debug prints?
+    notifyUserTag(code);
+}
+
+void RigidBody::notifyUserTag(int code) {
+    if (mUserTag)
+        mUserTag->m7(this, code);
+}
+
 }  // namespace ksys::phys

src/KingSystem/Physics/RigidBody/physRigidBody.h

@@ -22,6 +22,7 @@ namespace ksys::phys {
 class MotionAccessor;
 struct RigidBodyInstanceParam;
 class RigidBodyMotion;
+class RigidBodyMotionProxy;
 class RigidContactPoints;
 class UserTag;
 
@@ -127,29 +128,28 @@ public:
     // 0x0000007100f8cfa0
     void x_0();
 
-    void setMotionFlag(MotionFlag);
-
     bool isActive() const;
 
-    // 0x0000007100f8d1f8
     bool isFlag8Set() const;
-    // 0x0000007100f8d204
     bool isMotionFlag1Set() const;
-    // 0x0000007100f8d210
     bool isMotionFlag2Set() const;
-    // 0x0000007100f8d21c
     void sub_7100F8D21C();
     // 0x0000007100f8d308
     bool x_6();
 
-    // 0x0000007100f8d680
+    /// Get the motion accessor if it is a RigidBodyMotion. Returns nullptr otherwise.
     RigidBodyMotion* getMotionAccessor() const;
-    // 0x0000007100f90f28 - for internal use
+    /// Get the motion accessor if it is a RigidBodyMotion. Returns nullptr otherwise.
+    /// For internal use by the physics system.
     RigidBodyMotion* getMotionAccessorForProxy() const;
-    // 0x0000007100f8d70c
+
-    void* getMotionAccessorType2Stuff();
+    /// Get the motion accessor if it is a RigidBodyMotionProxy. Returns nullptr otherwise.
-    // 0x0000007100f8d7a8
+    RigidBodyMotionProxy* getMotionProxy() const;
-    void motionAccessorType2Stuff2();
+    /// Get the linked rigid body from the motion proxy (or nullptr if there is none).
+    RigidBody* getLinkedRigidBody() const;
+    /// Reset the linked rigid body if we have a motion proxy.
+    void resetLinkedRigidBody() const;
+
     // 0x0000007100f8d840
     void x_8();
 
@@ -174,12 +174,8 @@ public:
     void x_14(bool a, bool b, bool c);
     // 0x0000007100f8eabc
     void x_15(bool a, bool b);
-    // 0x0000007100f8ec3c
-    bool setLinearVelocityMaybe(const sead::Vector3f& velocity, float x);
-    // 0x0000007100f8ed74
-    bool setAngularVelocityMaybe(const sead::Vector3f& velocity, float x);
     // 0x0000007100f8ee38
-    void x_16();
+    void resetFrozenState();
 
     u32 addContactLayer(ContactLayer);
     u32 removeContactLayer(ContactLayer);
@@ -193,32 +189,52 @@ public:
     void sub_7100F8F9E8(ReceiverMask*, void*);
     void sub_7100F8FA44(ContactLayer, u32);
 
-    // 0x0000007100f9004c
+    void getPosition(sead::Vector3f* position) const;
+    sead::Vector3f getPosition() const;
+
+    void getRotation(sead::Quatf* rotation) const;
+    sead::Quatf getRotation() const;
+
+    void getPositionAndRotation(sead::Vector3f* position, sead::Quatf* rotation) const;
+
     void getTransform(sead::Matrix34f* mtx) const;
+    sead::Matrix34f getTransform() const;
     // 0x0000007100f8fb08
     void setTransform(const sead::Matrix34f& mtx, bool propagate_to_linked_motions);
 
-    // 0x0000007100f8ec3c
     bool setLinearVelocity(const sead::Vector3f& velocity, float epsilon = sead::Mathf::epsilon());
-    // 0x0000007100f9118c
     void getLinearVelocity(sead::Vector3f* velocity) const;
-    // 0x0000007100f911ac
     sead::Vector3f getLinearVelocity() const;
 
-    // 0x0000007100f8ed74
     bool setAngularVelocity(const sead::Vector3f& velocity, float epsilon = sead::Mathf::epsilon());
-    // 0x0000007100f911f8
     void getAngularVelocity(sead::Vector3f* velocity) const;
-    // 0x0000007100f91218
     sead::Vector3f getAngularVelocity() const;
 
+    // 0x0000007100f91264
+    void getPointVelocity(sead::Vector3f* velocity, const sead::Vector3f& point) const;
+
     // 0x0000007100f92b74
     void computeVelocities(hkVector4f* linear_velocity, hkVector4f* angular_velocity,
                            const hkVector4f& position, const hkQuaternionf& rotation);
 
-    // 0x0000007100f93348
+    void setCenterOfMassInLocal(const sead::Vector3f& center);
+    void getCenterOfMassInLocal(sead::Vector3f* center) const;
+    sead::Vector3f getCenterOfMassInLocal() const;
+
+    void getCenterOfMassInWorld(sead::Vector3f* center) const;
+    sead::Vector3f getCenterOfMassInWorld() const;
+
+    void setMaxLinearVelocity(float max);
+    float getMaxLinearVelocity() const;
+
+    void setMaxAngularVelocity(float max);
+    float getMaxAngularVelocity() const;
+
+    void applyLinearImpulse(const sead::Vector3f& impulse);
+    void applyAngularImpulse(const sead::Vector3f& impulse);
+    void applyPointImpulse(const sead::Vector3f& impulse, const sead::Vector3f& point);
+
     void setMass(float mass);
-    // 0x0000007100f933fc
     float getMass() const;
     // 0x0000007100f93498
     float getMassInv() const;
@@ -247,6 +263,7 @@ public:
     bool hasFlag(Flag flag) const { return mFlags.isOn(flag); }
     const auto& getMotionFlags() const { return mMotionFlags; }
     void resetMotionFlagDirect(const MotionFlag flag) { mMotionFlags.reset(flag); }
+    void setMotionFlag(MotionFlag flag);
 
     hkpRigidBody* getHkBody() const { return mHkBody; }
 
@@ -280,7 +297,13 @@ public:
     }
 
 private:
+    ContactLayerType getLayerType() const {
+        return !isMassScaling() ? ContactLayerType::Entity : ContactLayerType::Sensor;
+    }
+
     void createMotionAccessor(sead::Heap* heap);
+    void onInvalidParameter(int code = 0);
+    void notifyUserTag(int code);
 
     sead::CriticalSection mCS;
     sead::TypedBitFlag<Flag, sead::Atomic<u32>> mFlags{};
@@ -288,7 +311,7 @@ private:
     sead::BitFlag32 mContactMask{};
     hkpRigidBody* mHkBody;
     UserTag* mUserTag = nullptr;
-    void* _88 = nullptr;
+    RigidContactPoints* mContactPoints = nullptr;
     void* _90 = nullptr;
     u16 _98 = 0;
     RigidBodyAccessor mRigidBodyAccessor;

src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.cpp

@@ -4,6 +4,9 @@
 
 namespace ksys::phys {
 
+static RigidBodyRequestMgr::Config sRigidBodyRequestMgrConfig;
+static bool sEnableLinearVelocityChecks;
+
 RigidBodyRequestMgr::RigidBodyRequestMgr() = default;
 
 RigidBodyRequestMgr::~RigidBodyRequestMgr() {
@@ -103,4 +106,19 @@ bool RigidBodyRequestMgr::deregisterMotionAccessor(MotionAccessor* accessor) {
     return true;
 }
 
+RigidBodyRequestMgr::Config& RigidBodyRequestMgr::Config::get() {
+    return sRigidBodyRequestMgrConfig;
+}
+
+bool RigidBodyRequestMgr::Config::isLinearVelocityTooHigh(const sead::Vector3f& velocity) {
+    if (!sEnableLinearVelocityChecks)
+        return false;
+
+    return velocity.squaredLength() > get().linear_velocity_threshold_sq;
+}
+
+void RigidBodyRequestMgr::Config::enableLinearVelocityChecks(bool enable) {
+    sEnableLinearVelocityChecks = enable;
+}
+
 }  // namespace ksys::phys

src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h

@@ -20,6 +20,24 @@ class RigidBody;
 
 class RigidBodyRequestMgr : public sead::hostio::Node {
 public:
+    struct Config {
+        float _0 = 0.6;
+        float _4 = 0.7;
+        float _8 = 1.25;
+        float _c = 1.0;
+        float _10 = 0.2;
+        float _14 = 0.9;
+        float _18 = 0.5;
+        float _1c = 1.0;
+        float _20 = 4.0;
+        // 5000m/s (squared)
+        float linear_velocity_threshold_sq = 2.5e7;
+
+        static Config& get();
+        static bool isLinearVelocityTooHigh(const sead::Vector3f& velocity);
+        static void enableLinearVelocityChecks(bool enable);
+    };
+
     RigidBodyRequestMgr();
     virtual ~RigidBodyRequestMgr();
 

src/KingSystem/Physics/System/physMemSystem.h

@@ -36,6 +36,8 @@ public:
     SystemData* getSystemData() const { return mSystemData; }
     MaterialTable* getMaterialTable() const { return mMaterialTable; }
 
+    bool isPaused() const;
+
     void initSystemData(sead::Heap* heap);
 
     RigidContactPoints* allocContactPoints(sead::Heap* heap, int num, const sead::SafeString& name,
@@ -51,6 +53,9 @@ public:
 
     void removeSystemGroupHandler(SystemGroupHandler* handler);
 
+    void lockWorld(ContactLayerType type, void* a = nullptr, int b = 0, bool c = false);
+    void unlockWorld(ContactLayerType type, void* a = nullptr, int b = 0, bool c = false);
+
 private:
     u8 _28[0xa8 - 0x28];
     sead::CriticalSection mCS;

src/KingSystem/Physics/System/physUserTag.cpp

@@ -14,6 +14,6 @@ void UserTag::m4() {}
 
 void UserTag::m5() {}
 
-void UserTag::m7() {}
+void UserTag::m7(RigidBody* rigid_body, int a) {}
 
 }  // namespace ksys::phys

src/KingSystem/Physics/System/physUserTag.h

@@ -6,6 +6,8 @@
 
 namespace ksys::phys {
 
+class RigidBody;
+
 class UserTag {
     SEAD_RTTI_BASE(UserTag)
 public:
@@ -18,7 +20,7 @@ public:
     virtual void m4();
     virtual void m5();
     virtual const sead::SafeString& getName() const { return sead::SafeString::cEmptyString; }
-    virtual void m7();
+    virtual void m7(RigidBody* rigid_body, int a);
     virtual const sead::SafeString& getName2() const { return sead::SafeString::cEmptyString; }
     virtual ~UserTag() = default;
 };