diff --git a/data/uking_functions.csv b/data/uking_functions.csv
index 96f494ce..7d04d4a3 100644
--- a/data/uking_functions.csv
+++ b/data/uking_functions.csv
@@ -83021,11 +83021,11 @@ Address,Quality,Size,Name
 0x0000007100f8faa0,O,000056,_ZN4ksys4phys9RigidBody28addGroundTypeToGroundHitMaskENS0_9GroundHitE
 0x0000007100f8fad8,O,000044,_ZNK4ksys4phys9RigidBody16getGroundHitTypeEv
 0x0000007100f8fb04,O,000004,_ZN4ksys4phys9RigidBody8setColorERKN4sead7Color4fEPKvb
-0x0000007100f8fb08,U,000360,phys::RigidBody::x_34_setTransform
+0x0000007100f8fb08,O,000360,_ZN4ksys4phys9RigidBody12setTransformERKN4sead8Matrix34IfEEb
 0x0000007100f8fc70,O,000196,_ZN4ksys4phys9RigidBody11setPositionERKN4sead7Vector3IfEEb
-0x0000007100f8fd34,U,000012,phys::RigidBody::x_36
-0x0000007100f8fd40,U,000124,phys::RigidBody::x_37
-0x0000007100f8fdbc,U,000328,phys::RigidBody::x_38
+0x0000007100f8fd34,O,000012,_ZNK4ksys4phys9RigidBody16isTransformDirtyEv
+0x0000007100f8fd40,O,000124,_ZN4ksys4phys9RigidBody19updateShapeIfNeededEf
+0x0000007100f8fdbc,O,000328,_ZN4ksys4phys9RigidBody11updateShapeEv
 0x0000007100f8ff04,O,000032,_ZNK4ksys4phys9RigidBody11getPositionEPN4sead7Vector3IfEE
 0x0000007100f8ff24,O,000076,_ZNK4ksys4phys9RigidBody11getPositionEv
 0x0000007100f8ff70,O,000032,_ZNK4ksys4phys9RigidBody11getRotationEPN4sead4QuatIfEE
@@ -83033,12 +83033,12 @@ Address,Quality,Size,Name
 0x0000007100f8ffdc,O,000112,_ZNK4ksys4phys9RigidBody22getPositionAndRotationEPN4sead7Vector3IfEEPNS2_4QuatIfEE
 0x0000007100f9004c,O,000032,_ZNK4ksys4phys9RigidBody12getTransformEPN4sead8Matrix34IfEE
 0x0000007100f9006c,O,000040,_ZNK4ksys4phys9RigidBody12getTransformEv
-0x0000007100f90094,U,000968,phys::RigidBody::x_3
+0x0000007100f90094,O,000968,_ZN4ksys4phys9RigidBody16changeMotionTypeENS0_10MotionTypeE
 0x0000007100f9045c,U,001132,phys::RigidBody::x_39
 0x0000007100f908c8,U,001632,phys::RigidBody::x_40
 0x0000007100f90f28,O,000140,_ZNK4ksys4phys9RigidBody32getEntityMotionAccessorForSensorEv
-0x0000007100f90fb4,U,000332,phys::RigidBody::x_41
-0x0000007100f91100,U,000140,phys::RigidBody::x_42
+0x0000007100f90fb4,O,000332,_ZN4ksys4phys9RigidBody28updateMotionTypeRelatedFlagsEv
+0x0000007100f91100,O,000140,_ZN4ksys4phys9RigidBody32triggerScheduledMotionTypeChangeEv
 0x0000007100f9118c,O,000032,_ZNK4ksys4phys9RigidBody17getLinearVelocityEPN4sead7Vector3IfEE
 0x0000007100f911ac,O,000076,_ZNK4ksys4phys9RigidBody17getLinearVelocityEv
 0x0000007100f911f8,O,000032,_ZNK4ksys4phys9RigidBody18getAngularVelocityEPN4sead7Vector3IfEE
@@ -83053,8 +83053,8 @@ Address,Quality,Size,Name
 0x0000007100f9210c,U,000968,phys::RigidBody::x_50
 0x0000007100f924d4,U,000644,phys::RigidBody::x_51
 0x0000007100f92758,U,000528,phys::RigidBody::x_52
-0x0000007100f92968,U,000524,phys::RigidBody::x_53
-0x0000007100f92b74,U,000140,phys::RigidBody::x_54
+0x0000007100f92968,O,000524,_ZN4ksys4phys9RigidBody25computeVelocityForWarpingEPN4sead7Vector3IfEERKS4_b
+0x0000007100f92b74,O,000140,_ZN4ksys4phys9RigidBody17computeVelocitiesEP10hkVector4fS3_RKS2_RK13hkQuaternionf
 0x0000007100f92c00,O,000128,_ZN4ksys4phys9RigidBody22setCenterOfMassInLocalERKN4sead7Vector3IfEE
 0x0000007100f92c80,O,000016,_ZNK4ksys4phys9RigidBody22getCenterOfMassInLocalEPN4sead7Vector3IfEE
 0x0000007100f92c90,O,000052,_ZNK4ksys4phys9RigidBody22getCenterOfMassInLocalEv
@@ -83125,7 +83125,7 @@ Address,Quality,Size,Name
 0x0000007100f963a4,O,000156,_ZNK4ksys4phys9RigidBody22isEntityMotionFlag80OnEv
 0x0000007100f96440,O,000156,_ZN4ksys4phys9RigidBody25setMagneMassScalingFactorEf
 0x0000007100f964dc,O,000148,_ZNK4ksys4phys9RigidBody25getMagneMassScalingFactorEv
-0x0000007100f96570,O,000008,_ZN4ksys4phys9RigidBody3m10Ev
+0x0000007100f96570,O,000008,_ZN4ksys4phys9RigidBody11getNewShapeEv
 0x0000007100f96578,O,000008,_ZN4ksys4phys9RigidBody3m11Ev
 0x0000007100f96580,O,000240,_ZN4ksys4phys9RigidBody13resetPositionEv
 0x0000007100f96670,O,000144,_ZN4ksys4phys9RigidBody7getNameEv
diff --git a/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h b/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h
index 3508cfbd..157c8b93 100644
--- a/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h
+++ b/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h
@@ -20,9 +20,14 @@ public:
     HK_FORCE_INLINE hkSimdFloat32 getRealPart() const;
     HK_FORCE_INLINE const hkVector4f& getImag() const;
 
+    HK_FORCE_INLINE hkFloat32 getAngle() const;
+    hkSimdFloat32 getAngleSr() const;
+
     HK_FORCE_INLINE void mul(hkQuaternionfParameter q);
     HK_FORCE_INLINE void setMul(hkQuaternionfParameter q0, hkQuaternionfParameter q1);
 
+    HK_FORCE_INLINE void setInverse(hkQuaternionfParameter q);
+
     HK_FORCE_INLINE static const hkQuaternionf& getIdentity();
 
     hkVector4f m_vec;
@@ -62,6 +67,10 @@ inline const hkVector4f& hkQuaternionf::getImag() const {
     return m_vec;
 }
 
+inline hkFloat32 hkQuaternionf::getAngle() const {
+    return getAngleSr().val();
+}
+
 inline void hkQuaternionf::mul(hkQuaternionfParameter q) {
     setMul(*this, q);
 }
@@ -79,6 +88,10 @@ inline void hkQuaternionf::setMul(hkQuaternionfParameter r, hkQuaternionfParamet
     m_vec.setXYZ_W(vec, (rReal * qReal) - rImag.dot<3>(qImag));
 }
 
+inline void hkQuaternionf::setInverse(const hkQuaternionf& q) {
+    m_vec.setNeg<3>(q.getImag());
+}
+
 inline const hkQuaternionf& hkQuaternionf::getIdentity() {
     return reinterpret_cast<const hkQuaternionf&>(g_vectorfConstants[HK_QUADREAL_0001]);
 }
diff --git a/src/KingSystem/ActorSystem/actPhysicsUserTag.h b/src/KingSystem/ActorSystem/actPhysicsUserTag.h
index 8388b106..d076c15e 100644
--- a/src/KingSystem/ActorSystem/actPhysicsUserTag.h
+++ b/src/KingSystem/ActorSystem/actPhysicsUserTag.h
@@ -20,7 +20,7 @@ public:
 
     void m2(void* a) override;
     void m3(void* a, void* b, float c) override;
-    void m4() override;
+    void onBodyShapeChanged(phys::RigidBody* body) override;
     void m5() override;
     const sead::SafeString& getName() const override;
     void m7(phys::RigidBody* rigid_body, int a) override;
diff --git a/src/KingSystem/Physics/RigidBody/physMotionAccessor.h b/src/KingSystem/Physics/RigidBody/physMotionAccessor.h
index d5b82caa..51f7d4d0 100644
--- a/src/KingSystem/Physics/RigidBody/physMotionAccessor.h
+++ b/src/KingSystem/Physics/RigidBody/physMotionAccessor.h
@@ -61,6 +61,8 @@ public:
     hkpRigidBody* getHkBody() const { return mBody->getHkBody(); }
     u32 get10() const { return _10; }
     u32 get14() const { return _14; }
+    void increment10() { ++_10; }
+    void increment14() { ++_14; }
 
 protected:
     RigidBody* mBody = nullptr;
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
index cf97d063..3db6c834 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
+++ b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
@@ -35,6 +35,14 @@ static bool isVectorInvalid(const sead::Vector3f& vec) {
     return false;
 }
 
+static bool isMatrixInvalid(const sead::Matrix34f& matrix) {
+    for (float x : matrix.a) {
+        if (std::isnan(x))
+            return true;
+    }
+    return false;
+}
+
 RigidBody::RigidBody(Type type, ContactLayerType layer_type, hkpRigidBody* hk_body,
                      const sead::SafeString& name, sead::Heap* heap, bool a7)
     : mCS(heap), mHkBody(hk_body), mRigidBodyAccessor(hk_body), mType(type) {
@@ -884,6 +892,153 @@ sead::Matrix34f RigidBody::getTransform() const {
     return transform;
 }
 
+void RigidBody::setTransform(const sead::Matrix34f& mtx, bool propagate_to_linked_motions) {
+    if (isMatrixInvalid(mtx)) {
+        onInvalidParameter();
+        return;
+    }
+
+    mMotionAccessor->setTransform(mtx, propagate_to_linked_motions);
+}
+
+bool RigidBody::isTransformDirty() const {
+    return mMotionFlags.isOn(MotionFlag::DirtyTransform);
+}
+
+void RigidBody::updateShape() {
+    if (isFlag8Set()) {
+        setMotionFlag(MotionFlag::DirtyShape);
+        return;
+    }
+
+    auto* shape = getNewShape();
+    if (shape) {
+        mHkBody->setShape(shape);
+        if (isEntity() && mMotionAccessor)
+            mMotionAccessor->increment14();
+    } else {
+        mHkBody->updateShape();
+        if (isEntity() && mMotionAccessor)
+            mMotionAccessor->increment10();
+    }
+
+    if (mUserTag)
+        mUserTag->onBodyShapeChanged(this);
+}
+
+void RigidBody::updateShapeIfNeeded(float x) {
+    if (!hasFlag(Flag::_10))
+        return;
+
+    if (x <= 0.0)
+        x = 1.0;
+
+    if (sead::Mathf::equalsEpsilon(_b0, x))
+        return;
+
+    _b0 = m12(x, _b0);
+    updateShape();
+}
+
+void RigidBody::changeMotionType(MotionType motion_type) {
+    if (getMotionType() == motion_type)
+        return;
+
+    if (isFlag8Set()) {
+        switch (motion_type) {
+        case MotionType::Dynamic:
+            if (isEntity()) {
+                setMotionFlag(MotionFlag::Dynamic);
+                mMotionFlags.reset(MotionFlag::Fixed);
+                mMotionFlags.reset(MotionFlag::Keyframed);
+            }
+            break;
+        case MotionType::Fixed:
+            setMotionFlag(MotionFlag::Fixed);
+            mMotionFlags.reset(MotionFlag::Dynamic);
+            mMotionFlags.reset(MotionFlag::Keyframed);
+            break;
+        case MotionType::Keyframed:
+            setMotionFlag(MotionFlag::Keyframed);
+            mMotionFlags.reset(MotionFlag::Dynamic);
+            mMotionFlags.reset(MotionFlag::Fixed);
+            break;
+        case MotionType::Unknown:
+        case MotionType::Invalid:
+            break;
+        }
+        return;
+    }
+
+    switch (motion_type) {
+    case MotionType::Dynamic:
+        if (!isEntity())
+            return;
+        mMotionFlags.set(MotionFlag::Dynamic);
+        break;
+    case MotionType::Fixed:
+        mMotionFlags.set(MotionFlag::Fixed);
+        break;
+    case MotionType::Keyframed:
+        mMotionFlags.set(MotionFlag::Keyframed);
+        break;
+    case MotionType::Unknown:
+    case MotionType::Invalid:
+        break;
+    }
+
+    doChangeMotionType(motion_type, getMotionType());
+    mMotionFlags.set(MotionFlag::DirtyMass);
+    mMotionFlags.set(MotionFlag::DirtyInertiaLocal);
+    mMotionFlags.set(MotionFlag::DirtyMaxVelOrTimeFactor);
+    mMotionFlags.set(MotionFlag::DirtyDampingOrGravityFactor);
+    mMotionFlags.set(MotionFlag::DirtyCenterOfMassLocal);
+    x_40();
+}
+
+void RigidBody::updateMotionTypeRelatedFlags() {
+    if (hasFlag(Flag::_20000000) || hasFlag(Flag::_80000000) || hasFlag(Flag::_40000000))
+        return;
+
+    switch (getMotionType()) {
+    case MotionType::Dynamic:
+        mFlags.set(Flag::_80000000);
+        mFlags.reset(Flag::_20000000);
+        mFlags.reset(Flag::_40000000);
+        return;
+    case MotionType::Fixed:
+        mFlags.set(Flag::_40000000);
+        mFlags.reset(Flag::_20000000);
+        mFlags.reset(Flag::_80000000);
+        return;
+    case MotionType::Keyframed:
+        mFlags.set(Flag::_20000000);
+        mFlags.reset(Flag::_40000000);
+        mFlags.reset(Flag::_80000000);
+        return;
+    case MotionType::Unknown:
+    case MotionType::Invalid:
+        break;
+    }
+
+    mFlags.reset(Flag::_20000000);
+    mFlags.reset(Flag::_40000000);
+    mFlags.reset(Flag::_80000000);
+}
+
+void RigidBody::triggerScheduledMotionTypeChange() {
+    if (hasFlag(Flag::_20000000)) {
+        changeMotionType(MotionType::Keyframed);
+        mFlags.reset(Flag::_20000000);
+    } else if (hasFlag(Flag::_40000000)) {
+        changeMotionType(MotionType::Fixed);
+        mFlags.reset(Flag::_40000000);
+    } else if (hasFlag(Flag::_80000000)) {
+        changeMotionType(MotionType::Dynamic);
+        mFlags.reset(Flag::_80000000);
+    }
+}
+
 bool RigidBody::setLinearVelocity(const sead::Vector3f& velocity, float epsilon) {
     if (isVectorInvalid(velocity)) {
         onInvalidParameter();
@@ -939,6 +1094,43 @@ void RigidBody::getPointVelocity(sead::Vector3f* velocity, const sead::Vector3f&
     velocity->add(getLinearVelocity());
 }
 
+void RigidBody::computeVelocityForWarping(sead::Vector3f* linear_velocity,
+                                          const sead::Vector3f& target_position,
+                                          bool take_angular_velocity_into_account) {
+    const float factor = getVelocityComputeTimeFactor();
+    const auto hk_target_pos = toHkVec4(target_position);
+    auto hk_current_pos = toHkVec4(getPosition());
+
+    if (take_angular_velocity_into_account) {
+        const auto center = getCenterOfMassInLocal();
+        if (center.x == 0 && center.y == 0 && center.z == 0) {
+            hkVector4f rel_pos;
+            rel_pos.setSub(hk_current_pos, toHkVec4(getCenterOfMassInWorld()));
+
+            hkVector4f correction;
+            correction.setCross(toHkVec4(getAngularVelocity()), rel_pos);
+            correction.mul(1.0f / factor);
+            hk_current_pos.add(correction);
+        }
+    }
+
+    hkVector4f result;
+    result.setSub(hk_target_pos, hk_current_pos);
+    result.mul(factor);
+    storeToVec3(linear_velocity, result);
+}
+
+void RigidBody::computeVelocities(hkVector4f* linear_velocity, hkVector4f* angular_velocity,
+                                  const hkVector4f& position, const hkQuaternionf& rotation) {
+    const float factor = getVelocityComputeTimeFactor();
+    computeVelocities(linear_velocity, angular_velocity, position, rotation, factor);
+}
+
+float RigidBody::getVelocityComputeTimeFactor() const {
+    const float time_factor = getTimeFactor();
+    return time_factor == 0 ? 0 : (1.f / (time_factor * System::instance()->get64()));
+}
+
 void RigidBody::setCenterOfMassInLocal(const sead::Vector3f& center) {
     sead::Vector3f current_center;
     mMotionAccessor->getCenterOfMassInLocal(&current_center);
@@ -1403,7 +1595,7 @@ void RigidBody::clearFlag8000000(bool clear) {
         updateDeactivation();
 }
 
-void* RigidBody::m10() {
+const hkpShape* RigidBody::getNewShape() {
     return nullptr;
 }
 
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.h b/src/KingSystem/Physics/RigidBody/physRigidBody.h
index db828d9f..de8101e8 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.h
+++ b/src/KingSystem/Physics/RigidBody/physRigidBody.h
@@ -18,6 +18,7 @@ class hkQuaternionf;
 class hkVector4f;
 class hkpCollidable;
 class hkpRigidBody;
+class hkpShape;
 class hkpMaxSizeMotion;
 class hkpMotion;
 
@@ -79,6 +80,10 @@ public:
         _2000000 = 1 << 25,
         _4000000 = 1 << 26,
         _8000000 = 1 << 27,
+        _10000000 = 1 << 28,
+        _20000000 = 1 << 29,
+        _40000000 = 1 << 30,
+        _80000000 = 1 << 31,
     };
 
     enum class MotionFlag {
@@ -96,7 +101,7 @@ public:
         DirtyCenterOfMassLocal = 1 << 11,
         DirtyInertiaLocal = 1 << 12,
         DirtyDampingOrGravityFactor = 1 << 13,
-        _4000 = 1 << 14,
+        DirtyShape = 1 << 14,
         _8000 = 1 << 15,
         _10000 = 1 << 16,
         _20000 = 1 << 17,
@@ -266,8 +271,19 @@ public:
 
     void getTransform(sead::Matrix34f* mtx) const;
     sead::Matrix34f getTransform() const;
-    // 0x0000007100f8fb08
     void setTransform(const sead::Matrix34f& mtx, bool propagate_to_linked_motions);
+    bool isTransformDirty() const;
+
+    void updateShape();
+    void updateShapeIfNeeded(float x);
+
+    void changeMotionType(MotionType motion_type);
+    // 0x0000007100f9045c - calls a bunch of Havok world functions
+    void doChangeMotionType(MotionType x, MotionType y);
+    // 0x0000007100f908c8
+    void x_40();
+    void updateMotionTypeRelatedFlags();
+    void triggerScheduledMotionTypeChange();
 
     bool setLinearVelocity(const sead::Vector3f& velocity, float epsilon = sead::Mathf::epsilon());
     void getLinearVelocity(sead::Vector3f* velocity) const;
@@ -279,9 +295,16 @@ public:
 
     void getPointVelocity(sead::Vector3f* velocity, const sead::Vector3f& point) const;
 
-    // 0x0000007100f92b74
+    /// Compute the linear velocity that would be necessary to instantly warp to the target.
+    void computeVelocityForWarping(sead::Vector3f* linear_velocity,
+                                   const sead::Vector3f& target_position,
+                                   bool take_angular_velocity_into_account);
     void computeVelocities(hkVector4f* linear_velocity, hkVector4f* angular_velocity,
                            const hkVector4f& position, const hkQuaternionf& rotation);
+    // 0x0000007100f91780
+    void computeVelocities(hkVector4f* linear_velocity, hkVector4f* angular_velocity,
+                           const hkVector4f& position, const hkQuaternionf& rotation, float factor);
+    float getVelocityComputeTimeFactor() const;
 
     void setCenterOfMassInLocal(const sead::Vector3f& center);
     void getCenterOfMassInLocal(sead::Vector3f* center) const;
@@ -416,7 +439,7 @@ public:
     bool isEntityMotionFlag200On() const;
 
     virtual void m9() = 0;
-    virtual void* m10();
+    virtual const hkpShape* getNewShape();
     virtual void* m11();
     virtual float m12(float x, float y);
     virtual void resetPosition();
diff --git a/src/KingSystem/Physics/System/physSystem.h b/src/KingSystem/Physics/System/physSystem.h
index 783e7643..efdc0d7a 100644
--- a/src/KingSystem/Physics/System/physSystem.h
+++ b/src/KingSystem/Physics/System/physSystem.h
@@ -30,6 +30,7 @@ class System {
     virtual ~System();
 
 public:
+    float get64() const { return _64; }
     float getTimeFactor() const { return mTimeFactor; }
     GroupFilter* getGroupFilter(ContactLayerType type) const;
     ContactMgr* getContactMgr() const { return mContactMgr; }
@@ -61,7 +62,11 @@ public:
     void unlockWorld(ContactLayerType type, void* a = nullptr, int b = 0, bool c = false);
 
 private:
-    u8 _28[0x74 - 0x28];
+    u8 _28[0x64 - 0x28];
+    float _64 = 1.0 / 30.0;
+    float _68 = 1.0 / 30.0;
+    float _6c = 1.0;
+    float _70 = 1.0 / 30.0;
     float mTimeFactor{};
     u8 _78[0xa8 - 0x78];
     sead::CriticalSection mCS;
diff --git a/src/KingSystem/Physics/System/physUserTag.cpp b/src/KingSystem/Physics/System/physUserTag.cpp
index 8a2fe69d..03f0fcb8 100644
--- a/src/KingSystem/Physics/System/physUserTag.cpp
+++ b/src/KingSystem/Physics/System/physUserTag.cpp
@@ -10,7 +10,7 @@ void UserTag::m3(void* a, void* b, float c) {
     // FIXME
 }
 
-void UserTag::m4() {}
+void UserTag::onBodyShapeChanged(RigidBody* body) {}
 
 void UserTag::m5() {}
 
diff --git a/src/KingSystem/Physics/System/physUserTag.h b/src/KingSystem/Physics/System/physUserTag.h
index 1d2ea480..5942ee45 100644
--- a/src/KingSystem/Physics/System/physUserTag.h
+++ b/src/KingSystem/Physics/System/physUserTag.h
@@ -17,7 +17,7 @@ public:
     virtual void m2(void* a);
     // a and b are probably physics bodies?
     virtual void m3(void* a, void* b, float c);
-    virtual void m4();
+    virtual void onBodyShapeChanged(RigidBody* body);
     virtual void m5();
     virtual const sead::SafeString& getName() const { return sead::SafeString::cEmptyString; }
     virtual void m7(RigidBody* rigid_body, int a);