phys::Constraint work

data/uking_functions.csv

@@ -82486,32 +82486,32 @@ Address,Quality,Size,Name
 0x0000007100f69d2c,U,000004,nullsub_4196
 0x0000007100f69d30,U,000004,nullsub_4197
 0x0000007100f69d34,U,000004,nullsub_4198
-0x0000007100f69d38,U,000180,Constraint::ctor
+0x0000007100f69d38,M,000180,_ZN4ksys4phys10ConstraintC1EP21hkpConstraintInstancePNS0_9RigidBodyES5_P13hkQuaternionfPv
 0x0000007100f69dec,U,000376,Constraint::dtor
 0x0000007100f69f64,U,000036,Constraint::dtorDelete
 0x0000007100f69f88,U,000036,getPhysicsMemSysField190Or
 0x0000007100f69fac,U,000044,
 0x0000007100f69fd8,U,000024,
-0x0000007100f69ff0,U,000132,
-0x0000007100f6a074,U,000136,
-0x0000007100f6a0fc,U,000132,
-0x0000007100f6a180,U,000088,Constraint::x
-0x0000007100f6a1d8,U,000068,
-0x0000007100f6a21c,U,000012,
-0x0000007100f6a228,U,000184,
-0x0000007100f6a2e0,U,000032,
-0x0000007100f6a300,U,000372,
-0x0000007100f6a474,U,000340,
-0x0000007100f6a5c8,U,000212,
-0x0000007100f6a69c,U,000092,
-0x0000007100f6a6f8,U,000372,
+0x0000007100f69ff0,O,000132,_ZN4ksys4phys10Constraint14sub_7100F69FF0Ev
+0x0000007100f6a074,O,000136,_ZN4ksys4phys10Constraint14sub_7100F6A074Ev
+0x0000007100f6a0fc,O,000132,_ZN4ksys4phys10Constraint20setConstraintWeightsEff
+0x0000007100f6a180,O,000088,_ZN4ksys4phys10Constraint23allocConstraintListenerEPN4sead4HeapE
+0x0000007100f6a1d8,O,000068,_ZN4ksys4phys10Constraint12unlinkIf2SetEv
+0x0000007100f6a21c,O,000012,_ZN4ksys4phys10Constraint6unlinkEv
+0x0000007100f6a228,m,000184,_ZN4ksys4phys10Constraint4wakeEv
+0x0000007100f6a2e0,O,000032,_ZNK4ksys4phys10Constraint15checkIsSleepingEv
+0x0000007100f6a300,O,000372,_ZN4ksys4phys10Constraint12setRigidBodyEPNS0_9RigidBodyES3_
+0x0000007100f6a474,m,000340,_ZN4ksys4phys10Constraint10addToWorldEb
+0x0000007100f6a5c8,m,000212,_ZN4ksys4phys10Constraint8calcMassEv
+0x0000007100f6a69c,O,000092,_ZN4ksys4phys10Constraint14clearRigidBodyENS1_15ConstrainedTypeE
+0x0000007100f6a6f8,m,000372,_ZN4ksys4phys10Constraint23unlinkConstrainedBodiesEbb
 0x0000007100f6a86c,O,000020,_ZN4ksys4phys10Constraint7destroyEPS1_
-0x0000007100f6a880,U,000012,
-0x0000007100f6a88c,U,000160,
-0x0000007100f6a92c,U,000168,
-0x0000007100f6a9d4,U,000208,
-0x0000007100f6aaa4,U,000308,
-0x0000007100f6abd8,U,000044,Constraint::x_0
+0x0000007100f6a880,O,000012,_ZNK4ksys4phys10Constraint10isFlag2SetEv
+0x0000007100f6a88c,O,000160,_ZN4ksys4phys10Constraint10initParentEPNS0_9RigidBodyE
+0x0000007100f6a92c,O,000168,_ZN4ksys4phys10Constraint9initChildEPNS0_9RigidBodyE
+0x0000007100f6a9d4,O,000208,_ZN4ksys4phys10Constraint13initWithGroupEPNS0_28StaticCompoundRigidBodyGroupE
+0x0000007100f6aaa4,m,000308,_ZN4ksys4phys10Constraint10initBodiesEPNS0_9RigidBodyES3_
+0x0000007100f6abd8,W,000044,_ZNK4ksys4phys10Constraint12getRigidBodyEi
 0x0000007100f6ac04,U,000092,
 0x0000007100f6ac60,U,000008,
 0x0000007100f6ac68,U,000064,

lib/hkStubs/Havok/Physics2012/Dynamics/Constraint/hkpConstraintListener.h

@@ -0,0 +1,40 @@
+#pragma once
+
+#include <Havok/Common/Base/Types/hkBaseTypes.h>
+
+class hkpConstraintInstance;
+class hkpWorld;
+
+struct hkpConstraintBrokenEvent {
+    enum EventSource {
+        EVENT_SOURCE_UNKNOWN,
+        EVENT_SOURCE_BREAKABLE_CONSTRAINT,
+        EVENT_SOURCE_FLEXIBLE_JOINT,
+    };
+
+    hkpConstraintBrokenEvent(hkpWorld* world, hkpConstraintInstance* i, EventSource es)
+        : m_world(world), m_constraintInstance(i), m_eventSource(es), m_eventSourceDetails(0),
+          m_constraintBroken(true), m_actualImpulse(0.0f), m_impulseLimit(0.0f) {}
+
+    hkpWorld* m_world;
+    hkpConstraintInstance* m_constraintInstance;
+
+    hkEnum<EventSource, hkUint8> m_eventSource;
+    hkUint8 m_eventSourceDetails;
+    hkBool m_constraintBroken;
+    hkReal m_actualImpulse;
+    hkReal m_impulseLimit;
+};
+
+class hkpConstraintListener {
+public:
+    virtual ~hkpConstraintListener() {}
+
+    virtual void constraintAddedCallback(hkpConstraintInstance* constraint) {}
+
+    virtual void constraintRemovedCallback(hkpConstraintInstance* constraint) {}
+
+    virtual void constraintDeletedCallback(hkpConstraintInstance* constraint) {}
+
+    virtual void constraintBreakingCallback(const hkpConstraintBrokenEvent& event) {}
+};

src/KingSystem/Physics/CMakeLists.txt

@@ -8,6 +8,7 @@ target_sources(uking PRIVATE
 
     Constraint/physConstraint.cpp
     Constraint/physConstraint.h
+    Constraint/physConstraintListener.h
 
     Ragdoll/physRagdollConfig.cpp
     Ragdoll/physRagdollConfig.h

src/KingSystem/Physics/Constraint/physConstraint.cpp

@@ -1,7 +1,423 @@
-#include "KingSystem/Physics/Constraint//physConstraint.h"
+#include "KingSystem/Physics/Constraint/physConstraint.h"
+#include <Havok/Physics2012/Dynamics/Constraint/hkpConstraintInstance.h>
+#include <Havok/Physics2012/Dynamics/Entity/hkpRigidBody.h>
+#include <heap/seadHeap.h>
+#include "KingSystem/Physics/Constraint/physConstraintListener.h"
+#include "KingSystem/Physics/RigidBody/physRigidBody.h"
+#include "KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h"
+#include "KingSystem/Physics/StaticCompound/physStaticCompoundMgr.h"
+#include "KingSystem/Physics/System/physSystem.h"
+#include "KingSystem/Utils/HeapUtil.h"
 
 namespace ksys::phys {
 
+Constraint::Constraint(hkpConstraintInstance* constraint, RigidBody* parent, RigidBody* child,
+                       hkQuaternionf* quat, void* cs_option)
+    : mConstraint(constraint), mCsOption(cs_option) {
+    mDirection = quat->m_vec[0];
+    mSleepingBodies[ConstrainedType::Parent] = parent;
+    mSleepingBodies[ConstrainedType::Child] = child;
+
+    mActiveBodies[ConstrainedType::Parent] = System::instance()->getWorldRigidBody();
+    mActiveBodies[ConstrainedType::Child] = nullptr;
+
+    mConstraint->setUserData(reinterpret_cast<hkUlong>(this));
+
+    if (constraint->getPriority() == hkpConstraintInstance::PRIORITY_TOI) {
+        mFlags.set(Flag::TOIEnabled);
+    }
+}
+
+void Constraint::sub_7100F69FF0() {
+    auto lock = sead::makeScopedLock(mCS);
+
+    if (mSysFlags.isOn(SysFlag::_2)) {
+        mSysFlags.reset(SysFlag::_2);
+        mSysFlags.set(SysFlag::HasSleepingBodies);
+        return;
+    }
+
+    if (mSysFlags.isOff(SysFlag::HasSleepingBodies)) {
+        pushToSystem();
+    }
+}
+
+void Constraint::sub_7100F6A074() {
+    auto lock = sead::makeScopedLock(mCS);
+
+    if (mSysFlags.isOn(SysFlag::HasSleepingBodies)) {
+        mSysFlags.reset(SysFlag::HasSleepingBodies);
+        mSysFlags.set(SysFlag::_2);
+        return;
+    }
+
+    if (mFlags.isOn(Flag::Active)) {
+        pushToSystem2();
+    }
+}
+
+void Constraint::setConstraintWeights(float parent_weight, float child_weight) {
+    auto lock = sead::makeScopedLock(mCS);
+
+    if (mSysFlags.isOff(SysFlag::_4)) {
+        pushToSystem4();
+    }
+    mWeightParent = parent_weight;
+    mWeightChild = child_weight;
+}
+
+void Constraint::pushToSystem() {
+    auto lock = sead::makeScopedLock(mCS);
+    if (mSysFlags.isZero()) {
+        System::instance()->getRigidBodyRequestMgr()->pushConstraint(this);
+    }
+    mSysFlags.set(SysFlag::HasSleepingBodies);
+}
+
+void Constraint::pushToSystem2() {
+    auto lock = sead::makeScopedLock(mCS);
+    if (mSysFlags.isZero()) {
+        System::instance()->getRigidBodyRequestMgr()->pushConstraint(this);
+    }
+    mSysFlags.set(SysFlag::_2);
+}
+
+void Constraint::pushToSystem4() {
+    auto lock = sead::makeScopedLock(mCS);
+    if (mSysFlags.isZero()) {
+        System::instance()->getRigidBodyRequestMgr()->pushConstraint(this);
+    }
+    mSysFlags.set(SysFlag::_4);
+}
+
+void Constraint::pushToSystem8() {
+    auto lock = sead::makeScopedLock(mCS);
+    if (mSysFlags.isZero()) {
+        System::instance()->getRigidBodyRequestMgr()->pushConstraint(this);
+    }
+    mSysFlags.set(SysFlag::_8);
+}
+
+void Constraint::allocConstraintListener(sead::Heap* heap) {
+    void* listener_storage = util::allocStorage<ConstraintListener>(heap, 0x10);
+    mListener = new (listener_storage) ConstraintListener;
+    mConstraint->addConstraintListener(mListener);
+}
+
+void Constraint::unlinkIf2Set() {
+    auto lock = sead::makeScopedLock(mCS);
+    if (mSysFlags.isOn(SysFlag::_2)) {
+        unlinkConstrainedBodies(true, true);
+    }
+}
+
+void Constraint::unlink() {
+    unlinkConstrainedBodies(true, true);
+}
+
+void Constraint::wake() {
+    auto lock = sead::makeScopedLock(mCS);
+
+    if (mFlags.isOn(Flag::Active)) {
+        if (mSleepingBodies[ConstrainedType::Parent]) {
+            setRigidBody(mActiveBodies[ConstrainedType::Parent],
+                         mSleepingBodies[ConstrainedType::Child]);
+            mSleepingBodies[ConstrainedType::Parent] = nullptr;
+        }
+        if (mSleepingBodies[ConstrainedType::Child]) {
+            setRigidBody(mActiveBodies[ConstrainedType::Parent],
+                         mSleepingBodies[ConstrainedType::Child]);
+            mSleepingBodies[ConstrainedType::Child] = nullptr;
+        }
+    }
+
+    if (mSysFlags.isOn(SysFlag::HasSleepingBodies)) {
+        if (checkIsSleeping()) {
+            addToWorld(true);
+        } else {
+            addToWorld(false);
+        }
+    }
+
+    if (mSysFlags.isOn(SysFlag::_4)) {
+        calcMass();
+    }
+    mSysFlags.makeAllZero();
+}
+
+bool Constraint::checkIsSleeping() const {
+    return mSleepingBodies[ConstrainedType::Parent] || mSleepingBodies[ConstrainedType::Child];
+}
+
+bool Constraint::setRigidBody(RigidBody* existing_body, RigidBody* body) {
+    bool result = false;
+    if (existing_body != body && body &&
+        (System::instance()->getWorldRigidBody() || body->isAddedToWorld())) {
+        mFlags.set(Flag::IsModifyingConstrained);
+
+        bool already_active = mFlags.isOn(Flag::Active);
+
+        hkpRigidBody* existing_hk = existing_body ? existing_body->getHkBody() : nullptr;
+        hkpRigidBody* body_hk = body->getHkBody();
+
+        if (already_active) {
+            System::instance()->removeConstraintFromWorld(mConstraint);
+            if (mConstraintParam) {
+                System::instance()->removeConstraintParam(mConstraintParam);
+            }
+            mFlags.reset(Flag::Active);
+            mFlags.set(Flag::PendingRemove);
+        }
+
+        if (mActiveBodies[ConstrainedType::Parent] == existing_body) {
+            mActiveBodies[ConstrainedType::Parent] = body;
+        } else if (mActiveBodies[ConstrainedType::Child] == existing_body) {
+            mActiveBodies[ConstrainedType::Child] = body;
+        }
+
+        mConstraint->replaceEntity(existing_hk, body_hk);
+        if (already_active) {
+            System::instance()->addConstraintToWorld(mConstraint);
+            if (mConstraintParam) {
+                System::instance()->addConstraintParam(mConstraintParam);
+            }
+            mFlags.set(Flag::Active);
+            mFlags.reset(Flag::PendingRemove);
+        }
+        body->setConstraintAttached();
+
+        result = true;
+    }
+    mFlags.reset(Flag::IsModifyingConstrained);
+    return result;
+}
+
+bool Constraint::addToWorld(bool restore_sleeping) {
+    RigidBody* parent;
+    RigidBody* child;
+
+    if (restore_sleeping) {
+        if (mSleepingBodies[ConstrainedType::Parent]) {
+            parent = mSleepingBodies[ConstrainedType::Parent];
+        } else {
+            parent = mActiveBodies[ConstrainedType::Parent];
+        }
+        if (mSleepingBodies[ConstrainedType::Child]) {
+            child = mSleepingBodies[ConstrainedType::Child];
+        } else {
+            child = mActiveBodies[ConstrainedType::Child];
+        }
+    } else {
+        parent = mActiveBodies[ConstrainedType::Parent];
+        child = mActiveBodies[ConstrainedType::Child];
+    }
+
+    if (child == System::instance()->getWorldRigidBody()) {
+        return false;
+    }
+
+    if (parent->isSensor()) {
+        return false;
+    }
+
+    if (child && child->isSensor()) {
+        return false;
+    }
+
+    if (!parent->isAddedToWorld()) {
+        return false;
+    }
+
+    if (child && !child->isAddedToWorld()) {
+        return false;
+    }
+
+    if (mFlags.isOn(Flag::Active)) {
+        return false;
+    }
+
+    auto lock = sead::makeScopedLock(mSpinLock);
+
+    if (restore_sleeping) {
+        if (mSleepingBodies[ConstrainedType::Parent]) {
+            setRigidBody(mActiveBodies[ConstrainedType::Parent], parent);
+            mSleepingBodies[ConstrainedType::Parent] = nullptr;
+        }
+        if (mSleepingBodies[ConstrainedType::Child]) {
+            setRigidBody(mActiveBodies[ConstrainedType::Child], child);
+            mSleepingBodies[ConstrainedType::Child] = nullptr;
+        }
+    }
+
+    if (mCallback) {
+        (**mCallback)(mCallback, this, false);
+    }
+    System::instance()->addConstraintToWorld(mConstraint);
+    if (mConstraintParam) {
+        System::instance()->addConstraintParam(mConstraintParam);
+    }
+    mFlags.set(Flag::Active);
+    mFlags.reset(Flag::PendingRemove);
+    return true;
+}
+
+// NON_MATCHING: float stuff
+void Constraint::calcMass() {
+    if (mConstraint->getOwner() == nullptr) {
+        return;
+    }
+
+    auto* parent = mActiveBodies[ConstrainedType::Parent];
+    if (parent == nullptr) {
+        return;
+    }
+
+    auto* child = mActiveBodies[ConstrainedType::Child];
+    if (child == nullptr) {
+        return;
+    }
+
+    const auto* mot_parent = parent->getHkBody()->getMotion();
+    const auto* mot_child = child->getHkBody()->getMotion();
+
+    const auto mass_parent = mot_parent->getMass();
+    const auto mass_child = mot_child->getMass();
+
+    bool parent_heavier = mot_parent->getMass() < mot_child->getMass();
+
+    const auto effective_mass_parent = mass_parent * mWeightChild;
+    const auto effective_mass_child = mass_child * mWeightParent;
+
+    float ratio_b = effective_mass_parent / effective_mass_child;
+    if (!parent_heavier) {
+        ratio_b = 1.0f;
+    }
+
+    float ratio_a = effective_mass_child / effective_mass_parent;
+    if (parent_heavier) {
+        ratio_a = 1.0f;
+    }
+
+    hkVector4f vec_a;
+    hkVector4f vec_b;
+
+    vec_a.setAll(ratio_a);
+    vec_b.setAll(ratio_b);
+    mConstraint->setVirtualMassInverse(vec_b, vec_a);
+}
+
+void Constraint::clearRigidBody(ConstrainedType type) {
+    auto lock = sead::makeScopedLock(mCS);
+
+    mSleepingBodies[type] = nullptr;
+    mSysFlags.reset(SysFlag::_8);
+}
+
+// NON_MATCHING: return branch inverted
+void Constraint::unlinkConstrainedBodies(bool activate_entity, bool attach_to_world) {
+    mSysFlags.reset(SysFlag::_8);
+    mSleepingBodies[ConstrainedType::Child] = nullptr;
+
+    bool active = mFlags.isOn(Flag::Active);
+    if (active && mConstraint->getOwner() != nullptr) {
+        if (active) {
+            System::instance()->lockWorld(ContactLayerType::Entity);
+        }
+        mSpinLock.lock();
+
+        if (activate_entity) {
+            if (mActiveBodies[ConstrainedType::Parent])
+                mActiveBodies[ConstrainedType::Parent]->getHkBody()->activate();
+            if (mActiveBodies[ConstrainedType::Child])
+                mActiveBodies[ConstrainedType::Child]->getHkBody()->activate();
+        }
+        auto* cs = mConstraint;
+        System::instance()->getRigidBodyRequestMgr()->removeConstraintFromWorld(cs);
+
+        if (mConstraintParam) {
+            System::instance()->removeConstraintParam(mConstraintParam);
+        }
+        mFlags.set(Flag::PendingRemove);
+        mFlags.reset(Flag::Active);
+        mSpinLock.unlock();
+        if (active) {
+            System::instance()->unlockWorld(ContactLayerType::Entity);
+        }
+        return;
+    }
+
+    if (attach_to_world &&
+        mActiveBodies[ConstrainedType::Child] != System::instance()->getWorldRigidBody()) {
+        if (active) {
+            System::instance()->lockWorld(ContactLayerType::Entity);
+        }
+        mSpinLock.lock();
+        if (mActiveBodies[ConstrainedType::Child]) {
+            setRigidBody(mActiveBodies[ConstrainedType::Child],
+                         System::instance()->getWorldRigidBody());
+        }
+        mSpinLock.unlock();
+        if (active) {
+            System::instance()->unlockWorld(ContactLayerType::Entity);
+        }
+    }
+}
+
+bool Constraint::isFlag2Set() const {
+    return mSysFlags.isOn(SysFlag::_2);
+}
+
+bool Constraint::initParent(RigidBody* body) {
+    return initBody(body, ConstrainedType::Parent);
+}
+
+bool Constraint::initChild(RigidBody* body) {
+    return initBody(body, ConstrainedType::Child);
+}
+
+bool Constraint::initBody(RigidBody* body, ConstrainedType type) {
+    if (mSysFlags.isOn(SysFlag::_2)) {
+        return false;
+    }
+
+    auto lock = sead::makeScopedLock(mCS);
+    mSleepingBodies[type] = body;
+    pushToSystem8();
+    return true;
+}
+
+bool Constraint::initWithGroup(StaticCompoundRigidBodyGroup* grp) {
+    if (grp == nullptr) {
+        return false;
+    }
+
+    auto* mgr = System::instance()->getStaticCompoundMgr();
+    if (mgr == nullptr) {
+        return false;
+    }
+
+    auto* body = mgr->getRigidBody(grp);
+    if (body == nullptr) {
+        return false;
+    }
+
+    return initChild(body);
+}
+
+// NON_MATCHING: stack reg swapped
+bool Constraint::initBodies(RigidBody* a, RigidBody* b) {
+    bool parent_ok = initParent(a);
+    bool child_ok = initChild(b);
+    return parent_ok && child_ok;
+}
+
+RigidBody* Constraint::getRigidBody(s32 idx) const {
+    if (mSleepingBodies[idx]) {
+        return mSleepingBodies[idx];
+    }
+    return mActiveBodies[idx];
+}
+
 void Constraint::destroy(Constraint* instance) {
     delete instance;
 }

src/KingSystem/Physics/Constraint/physConstraint.h

@@ -1,19 +1,97 @@
 #pragma once
+#include <container/seadSafeArray.h>
+#include <prim/seadEnum.h>
+#include <prim/seadTypedBitFlag.h>
+#include <thread/seadCriticalSection.h>
+#include <thread/seadSpinLock.h>
+#include "KingSystem/Physics/System/physSystem.h"
 
-#include <prim/seadRuntimeTypeInfo.h>
+class hkQuaternionf;
+class hkpConstraintInstance;
+class hkpRigidBody;
 
 namespace ksys::phys {
 
+class RigidBody;
+class StaticCompoundRigidBodyGroup;
+class ConstraintParam;
+class ConstraintListener;
+
 class Constraint {
     SEAD_RTTI_BASE(Constraint)
 
 public:
-    // FIXME: types
-    Constraint();
+    SEAD_ENUM(ConstrainedType,Parent,Child)
+
+    typedef void (*ConstraintCallback)(void*, Constraint* constraint, bool a2);
+
+    Constraint(hkpConstraintInstance* constraint, RigidBody* parent, RigidBody* child,
+               hkQuaternionf* quat, void* cs_option);
     virtual ~Constraint();
 
+    bool initParent(RigidBody* body);
+    bool initChild(RigidBody* body);
+    bool initBody(RigidBody* body, ConstrainedType type);
+    bool initWithGroup(StaticCompoundRigidBodyGroup* grp);
+    bool initBodies(RigidBody* parent, RigidBody* child);
+
+    void allocConstraintListener(sead::Heap* heap);
+
+    void wake();
+    bool setRigidBody(RigidBody* existing_body, RigidBody* body);
+    bool addToWorld(bool restore_sleeping);
+    void calcMass();
+    void unlinkIf2Set();
+    void unlink();
+    void clearRigidBody(ConstrainedType type);
+    void unlinkConstrainedBodies(bool activate_entity, bool world_constraint);
+
+    void sub_7100F69FF0();
+    void sub_7100F6A074();
+    void setConstraintWeights(float parent_weight, float child_weight);
+    void pushToSystem();
+    void pushToSystem2();
+    void pushToSystem4();
+    void pushToSystem8();
+
+    RigidBody* getRigidBody(s32 idx) const;
+    bool isFlag2Set() const;
+    bool checkIsSleeping() const;
+
     /// No-op if instance is null.
     static void destroy(Constraint* instance);
+
+    enum class Flag : u16 {
+        Active = 1 << 0,
+        _2 = 1 << 1,
+        _4 = 1 << 2,
+        PendingRemove = 1 << 3,
+        TOIEnabled = 1 << 4,
+        IsModifyingConstrained = 1 << 5,
+    };
+
+    enum class SysFlag : u16 {
+        HasSleepingBodies = 1 << 0,
+        _2 = 1 << 1,
+        _4 = 1 << 2,
+        _8 = 1 << 3,
+    };
+
+private:
+    hkpConstraintInstance* mConstraint{};
+    float mDirection{};
+    void* mCsOption{};
+    ConstraintParam* mConstraintParam{};
+    ConstraintListener* mListener{};
+    sead::SafeArray<RigidBody*, 2> mActiveBodies{};
+    sead::SafeArray<RigidBody*, 2> mSleepingBodies{};
+    sead::TypedBitFlag<Flag> mFlags{};
+    sead::TypedBitFlag<SysFlag> mSysFlags{};
+    sead::CriticalSection mCS{};
+    float mWeightParent = 1.0;
+    float mWeightChild = 1.0;
+    ConstraintCallback** mCallback{};
+    sead::SpinLock mSpinLock{};
 };
 
 }  // namespace ksys::phys

src/KingSystem/Physics/Constraint/physConstraintListener.h

@@ -0,0 +1,12 @@
+#pragma once
+
+#include <Havok/Physics2012/Dynamics/Constraint/hkpConstraintListener.h>
+
+namespace ksys::phys {
+
+// TODO
+class ConstraintListener : public hkpConstraintListener {
+    virtual ~ConstraintListener() = default;
+};
+
+};  // namespace ksys::phys

src/KingSystem/Physics/RigidBody/physRigidBody.h

@@ -98,7 +98,7 @@ public:
         _100000 = 1 << 20,
         _200000 = 1 << 21,
         _400000 = 1 << 22,
-        _800000 = 1 << 23,
+        ConstraintAttached = 1 << 23,
         _1000000 = 1 << 24,
         _2000000 = 1 << 25,
         _4000000 = 1 << 26,
@@ -500,6 +500,8 @@ public:
     void setFlag200() { mFlags.set(Flag::_200); }
     void resetFlag200() { mFlags.reset(Flag::_200); }
 
+    void setConstraintAttached() { mFlags.set(Flag::ConstraintAttached); }
+
     hkpRigidBody* getHkBody() const { return mHkBody; }
 
     Type getType() const { return mType; }

src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h

@@ -15,11 +15,13 @@
 #include "KingSystem/Utils/Types.h"
 
 class hkpEntity;
+class hkpConstraintInstance;
 
 namespace ksys::phys {
 
 class MotionAccessor;
 class RigidBody;
+class Constraint;
 
 class RigidBodyRequestMgr : public sead::hostio::Node {
 public:
@@ -63,6 +65,10 @@ public:
     bool registerMotionAccessor(MotionAccessor* accessor);
     bool deregisterMotionAccessor(MotionAccessor* accessor);
 
+    void pushConstraint(Constraint* constraint);
+    void addConstraintToWorld(hkpConstraintInstance* constraint);
+    void removeConstraintFromWorld(hkpConstraintInstance* constraint);
+
 private:
     struct Unk1;
     struct Unk3;

src/KingSystem/Physics/System/physSystem.h

@@ -4,10 +4,12 @@
 #include <container/seadPtrArray.h>
 #include <heap/seadDisposer.h>
 #include <thread/seadCriticalSection.h>
+#include "KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h"
 #include "KingSystem/Physics/physDefines.h"
 #include "KingSystem/Utils/Types.h"
 
 class hkpWorld;
+class hkpConstraintInstance;
 
 namespace ksys::phys {
 
@@ -16,6 +18,8 @@ class ContactLayerCollisionInfo;
 class ContactLayerCollisionInfoGroup;
 class ContactMgr;
 class ContactPointInfo;
+class ConstraintMgr;
+class ConstraintParam;
 class GroupFilter;
 class LayerContactPointInfo;
 class MaterialTable;
@@ -51,6 +55,15 @@ public:
     RagdollInstanceMgr* getRagdollInstanceMgr() const { return mRagdollInstanceMgr; }
     SystemData* getSystemData() const { return mSystemData; }
     MaterialTable* getMaterialTable() const { return mMaterialTable; }
+    RigidBody* getWorldRigidBody() const { return mWorldRigidBody; }
+
+    void addConstraintToWorld(hkpConstraintInstance* constraint) {
+        getRigidBodyRequestMgr()->addConstraintToWorld(constraint);
+    }
+
+    void removeConstraintFromWorld(hkpConstraintInstance* constraint) {
+        getRigidBodyRequestMgr()->removeConstraintFromWorld(constraint);
+    }
 
     bool isPaused() const;
 
@@ -134,6 +147,9 @@ public:
     // 0x0000007101216cec
     sead::Heap* getPhysicsTempHeap(LowPriority low_priority) const;
 
+    void addConstraintParam(ConstraintParam* cs_param);
+    void removeConstraintParam(ConstraintParam* cs_param);
+
 private:
     u8 _28[0x64 - 0x28];
     float _64 = 1.0 / 30.0;
@@ -151,7 +167,7 @@ private:
     // FIXME: type
     sead::FixedPtrArray<void*, 2> _128;
     ContactMgr* mContactMgr;
-    void* _150;
+    ConstraintMgr* mConstraintMgr;
     StaticCompoundMgr* mStaticCompoundMgr;
     RigidBodyRequestMgr* mRigidBodyRequestMgr;
     RagdollInstanceMgr* mRagdollInstanceMgr;
@@ -159,7 +175,7 @@ private:
     SystemData* mSystemData;
     MaterialTable* mMaterialTable;
     void* _188{};
-    void* _190{};
+    RigidBody* mWorldRigidBody{};
     void* _198{};
     void* _1a0{};
     sead::Heap* mPhysicsSystemHeap{};