Merge 1b916db689 into 9c1288e4df

data/uking_functions.csv

@@ -74001,20 +74001,20 @@ Address,Quality,Size,Name
 0x0000007100d4e32c,O,000024,_ZN4ksys3map10ObjectLink27isPlacementLODOrForSaleLinkENS0_14MapLinkDefTypeE
 0x0000007100d4e344,O,000044,_ZN4ksys3map14ObjectLinkDataC1Ev
 0x0000007100d4e370,m,000136,_ZN4ksys3map14ObjectLinkData12deleteArraysEv
-0x0000007100d4e3f8,U,000556,PlacementLinkData::allocLinks
+0x0000007100d4e3f8,O,000556,_ZN4ksys3map14ObjectLinkData10allocLinksEiiiPN4sead4HeapE
 0x0000007100d4e624,O,000212,_ZN4ksys3map14ObjectLinkData16allocLinksToSelfEiPN4sead4HeapE
 0x0000007100d4e6f8,U,000292,PlacementLinkData::dtor
 0x0000007100d4e81c,U,000256,PlacementObj::x
-0x0000007100d4e91c,U,000440,PlacementLinkData::addLink
+0x0000007100d4e91c,M,000440,_ZN4ksys3map14ObjectLinkData7addLinkEPNS0_6ObjectERKN4sead14SafeStringBaseIcEEbS3_PNS0_9MubinIterE
 0x0000007100d4ead4,U,000364,PlacementLinkData::setupLinks
-0x0000007100d4ec40,U,000328,
+0x0000007100d4ec40,O,000328,_ZN4ksys3map14ObjectLinkData14sub_7100D4EC40EPNS0_6ObjectEPNS0_10ObjectLinkES3_
 0x0000007100d4ed88,O,000016,_ZN4ksys3map14ObjectLinkData11setGenGroupEPNS0_8GenGroupE
-0x0000007100d4ed98,U,000052,PlacementLinkData::checkCreateLinkObjRevival
-0x0000007100d4edcc,U,000020,PlacementLinkData::checkDeleteLinkObjRevival
+0x0000007100d4ed98,O,000052,_ZNK4ksys3map14ObjectLinkData25checkCreateLinkObjRevivalEv
+0x0000007100d4edcc,O,000020,_ZNK4ksys3map14ObjectLinkData25checkDeleteLinkObjRevivalEv
 0x0000007100d4ede0,M,000280,_ZN4ksys3map15ObjectLinkArray9checkLinkENS0_14MapLinkDefTypeEb
 0x0000007100d4eef8,O,000056,_ZN4ksys3map15ObjectLinkArray16findLinkWithTypeENS0_14MapLinkDefTypeE
-0x0000007100d4ef30,U,000116,
-0x0000007100d4efa4,U,000332,PlacementLinkData::x_2
+0x0000007100d4ef30,O,000116,_ZN4ksys3map14ObjectLinkData14sub_7100D4EF30ERNS_3act20ActorConstDataAccessE
+0x0000007100d4efa4,m,000332,_ZN4ksys3map14ObjectLinkData21findObjectByActorNameERKN4sead14SafeStringBaseIcEE
 0x0000007100d4f0f0,U,000224,
 0x0000007100d4f1d0,U,000532,
 0x0000007100d4f3e4,U,000496,
@@ -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/ActorSystem/actActorConstDataAccess.h

@@ -119,6 +119,8 @@ public:
     f32 getHorseAvoidOffset() const;
     bool horseTargetedIsCircularMoveAlways() const;
 
+    void setActor687();
+
 private:
     Actor* getActor() const;
 

src/KingSystem/Map/mapObject.h

@@ -123,6 +123,8 @@ public:
     void setRevivalFlagValue(bool value);
     void setFieldATrue();
 
+    bool checkRevivalMaybe(bool a1);
+
     sead::Vector3f getScale() const;
     sead::Vector3f getRotate() const;
     void setTranslate(const sead::Vector3f& translate);
@@ -185,6 +187,12 @@ public:
     const auto& getActorFlags8() const { return mActorFlags8; }
     const auto& getHardModeFlags() const { return mHardModeFlags; }
 
+    bool isLinkTag() const { return mFlags.isOn(Flag::IsLinkTag); }
+    bool isSetFlag8() const { return mFlags.isOn(Flag::_8); }
+    bool isIncrementSave() const { return mFlags.isOn(Flag::IncrementSave); }
+
+    void setFlag8() { mFlags.set(Flag::_8); }
+
     auto getActorDataIdx() const { return mActorDataIdx; }
     auto getIdx() const { return mIdx; }
     auto getId() const { return mId; }
@@ -202,6 +210,10 @@ public:
     ObjectLinkData* getLinkData() const { return mLinkData; }
 
     gdt::FlagHandle getRevivalGameDataFlagHash() const { return mRevivalGameDataFlagHash; }
+    void setRevivalGameDataFlagHash(const gdt::FlagHandle& value) {
+        mRevivalGameDataFlagHash = value;
+    }
+
     u32 getHashId() const { return mHashId; }
 
     const sead::Vector3f& getTranslate() const { return mTranslate; }

src/KingSystem/Map/mapObjectLink.cpp

@@ -150,6 +150,27 @@ void ObjectLinkData::deleteArrays() {
     mLinksToSelf.links.freeBuffer();
 }
 
+bool ObjectLinkData::allocLinks(s32 num_links_ref, s32 num_links_other, s32 num_links_cs,
+                                sead::Heap* heap) {
+    if (num_links_ref > 0) {
+        if (!mObjects.allocBufferAssert(num_links_ref, heap)) {
+            return false;
+        }
+        mObjects.fill(nullptr);
+    }
+    if (num_links_other > 0) {
+        if (!mLinksOther.links.allocBufferAssert(num_links_other, heap)) {
+            return false;
+        }
+    }
+    if (num_links_cs > 0) {
+        if (!mLinksCs.links.allocBufferAssert(num_links_cs, heap)) {
+            return false;
+        }
+    }
+    return true;
+}
+
 bool ObjectLinkData::allocLinksToSelf(s32 num_links, sead::Heap* heap) {
     if (num_links >= 1) {
         mLinksToSelf.links.tryAllocBuffer(num_links, heap);
@@ -159,6 +180,88 @@ bool ObjectLinkData::allocLinksToSelf(s32 num_links, sead::Heap* heap) {
     return true;
 }
 
+// NON_MATCHING
+bool ObjectLinkData::addLink(Object* dest, const sead::SafeString& definition_name, bool passive,
+                             Object* src, MubinIter* iter) {
+    MapLinkDefType type = ObjectLink::getTypeForName(definition_name);
+    if (type == MapLinkDefType::Invalid) {
+        return false;
+    }
+    if (type == MapLinkDefType::SyncLink && passive) {
+        return false;
+    }
+
+    if (type == MapLinkDefType::Reference) {
+        for (int i = 0; i < mObjects.size(); i++) {
+            if (mObjects[i] == nullptr) {
+                mObjects[i] = dest;
+                return false;
+            }
+        }
+    }
+
+    if (dest != nullptr && passive) {
+        switch (type) {
+        case MapLinkDefType::Create:
+        case MapLinkDefType::Delete:
+        case MapLinkDefType::MtxCopyCreate:
+        case MapLinkDefType::SyncLink:
+        case MapLinkDefType::PlacementLOD:
+        case MapLinkDefType::Reference:
+            break;
+        case MapLinkDefType::BasicSig:
+        case MapLinkDefType::AxisX:
+        case MapLinkDefType::AxisY:
+        case MapLinkDefType::AxisZ:
+        case MapLinkDefType::NAxisX:
+        case MapLinkDefType::NAxisY:
+        case MapLinkDefType::NAxisZ:
+        case MapLinkDefType::GimmickSuccess:
+        case MapLinkDefType::VelocityControl:
+        case MapLinkDefType::BasicSigOnOnly:
+        case MapLinkDefType::Remains:
+        case MapLinkDefType::DeadUp:
+        case MapLinkDefType::LifeZero:
+        case MapLinkDefType::Stable:
+        case MapLinkDefType::ChangeAtnSig:
+            dest = nullptr;
+            break;
+        default:
+            dest = nullptr;
+            break;
+        }
+    }
+
+    ObjectLinkArray* arr = &mLinksCs;
+    if (type == MapLinkDefType::Reference || type == MapLinkDefType::Invalid) {
+        arr = &mLinksOther;
+    }
+
+    if (type == MapLinkDefType::CopyWaitRevival) {
+        if (dest != nullptr) {
+            if (src->isLinkTag() && !src->isSetFlag8() && !src->isIncrementSave() &&
+                !dest->isIncrementSave()) {
+                src->setFlag8();
+                src->setRevivalGameDataFlagHash(dest->getRevivalGameDataFlagHash());
+            }
+        }
+    }
+
+    auto& links = arr->links;
+    for (auto& link : links) {
+        if (link.other_obj == dest && link.type == type) {
+            return false;
+        }
+        if (link.other_obj == nullptr && link.type == MapLinkDefType::Invalid) {
+            link.type = type;
+            link.other_obj = dest;
+            link.iter = *iter;
+            return type != MapLinkDefType::Invalid;
+        }
+    }
+    return true;
+}
+
 ObjectLink* ObjectLinkData::findLinkWithType(MapLinkDefType t) {
     return findLinkWithType_0(t);
 }
@@ -180,11 +283,70 @@ ObjectLink* ObjectLinkData::findLinkWithType_0(MapLinkDefType t) {
     }
 }
 
+bool ObjectLinkData::sub_7100D4EC40(Object* obj, ObjectLink* link, Object* dest) {
+    for (auto& l : mLinksToSelf.links) {
+        auto type = link->type;
+        if (l.other_obj == obj && l.type == link->type) {
+            return true;
+        }
+        if (l.other_obj == nullptr) {
+            l.other_obj = obj;
+            l.type = type;
+            l.iter = link->iter;
+
+            auto& iter = link->iter;
+            switch (type) {
+            default:
+                break;
+            case MapLinkDefType::BasicSig:
+            case MapLinkDefType::BasicSigOnOnly: {
+                bool no_auto_demo_member = false;
+                if (iter.tryGetParamBoolByKey(&no_auto_demo_member, "NoAutoDemoMember")) {
+                    mNoAutoDemoMember = no_auto_demo_member;
+                }
+                break;
+            }
+            case MapLinkDefType::Create:
+            case MapLinkDefType::MtxCopyCreate:
+                if (obj->isLinkTag()) {
+                    bool appear_fade = false;
+                    if (iter.tryGetParamBoolByKey(&appear_fade, "AppearFade")) {
+                        mAppearFade = appear_fade;
+                    }
+                    mCreateLinksSrcObj = obj;
+                }
+                break;
+            case MapLinkDefType::Delete:
+                if (obj->isLinkTag()) {
+                    mDeleteLinksSrcObj = obj;
+                }
+                break;
+            }
+            return true;
+        }
+    }
+    return false;
+}
+
 void ObjectLinkData::setGenGroup(GenGroup* group) {
     if (mGenGroup == nullptr)
         mGenGroup = group;
 }
 
+bool ObjectLinkData::checkCreateLinkObjRevival() const {
+    if (mCreateLinksSrcObj != nullptr) {
+        return !mCreateLinksSrcObj->checkRevivalMaybe(false);
+    }
+    return false;
+}
+
+bool ObjectLinkData::checkDeleteLinkObjRevival() const {
+    if (mDeleteLinksSrcObj != nullptr) {
+        return mDeleteLinksSrcObj->checkRevivalMaybe(true);
+    }
+    return false;
+}
+
 // NON_MATCHING
 bool ObjectLinkArray::checkLink(MapLinkDefType t, bool b) {
     bool x_exists;
@@ -217,6 +379,24 @@ done:
     return acc.checkLinkTagActivated(b, x_exists);
 }
 
+bool ObjectLinkData::sub_7100D4EF30(act::ActorConstDataAccess& accessor) {
+    for (auto& link : mLinksOther.links) {
+        if (link.type == MapLinkDefType::Recreate && link.other_obj == accessor.getMapObject()) {
+            accessor.setActor687();
+            return true;
+        }
+    }
+    return false;
+}
+
+Object* ObjectLinkData::findObjectByActorName(const sead::SafeString& name) {
+    for (auto obj : mObjects) {
+        if (obj->getActorData().mActorName == name)
+            return obj;
+    }
+    return nullptr;
+}
+
 ObjectLink* ObjectLinkArray::findLinkWithType(MapLinkDefType type) {
     return findLinkWithType_0(type);
 }

src/KingSystem/Map/mapObjectLink.h

@@ -8,6 +8,7 @@
 namespace ksys::act {
 class Actor;
 class ActorLinkConstDataAccess;
+class ActorConstDataAccess;
 }  // namespace ksys::act
 
 namespace ksys::map {
@@ -94,8 +95,12 @@ public:
 
     void deleteArrays();
     void release(Object* obj, bool a1);
+    bool allocLinks(s32 num_links_ref, s32 num_links_other, s32 num_links_cs, sead::Heap* heap);
     bool allocLinksToSelf(s32 num_links, sead::Heap* heap);
 
+    bool addLink(Object* dest, const sead::SafeString& definition_name, bool passive, Object* src,
+                 MubinIter* iter);
+
     bool sub_7100D4EC40(Object* src, ObjectLink* link, Object* dest);
     void sub_7100D4FB78(Object* obj);
     bool checkCreateLinkObjRevival() const;
@@ -103,7 +108,9 @@ public:
 
     ObjectLink* findLinkWithType(MapLinkDefType t);
     ObjectLink* findLinkWithType_0(MapLinkDefType t);
+    bool sub_7100D4EF30(act::ActorConstDataAccess& accessor);
 
+    Object* findObjectByActorName(const sead::SafeString& name);
     void setGenGroup(GenGroup* group);
 
     void x_1(act::Actor* actor, Object* obj);

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
@@ -194,4 +195,4 @@ target_sources(uking PRIVATE
     physLayerMaskBuilder.h
     physMaterialMask.cpp
     physMaterialMask.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{};