From 6118d22c20616e9c19de544ba6170b18cdd10f3e Mon Sep 17 00:00:00 2001 From: theo3 Date: Tue, 23 May 2023 15:15:24 -0700 Subject: [PATCH 1/2] mapObject work --- data/uking_functions.csv | 14 +- .../ActorSystem/actActorConstDataAccess.h | 2 + src/KingSystem/Map/mapObject.h | 12 ++ src/KingSystem/Map/mapObjectLink.cpp | 180 ++++++++++++++++++ src/KingSystem/Map/mapObjectLink.h | 7 + 5 files changed, 208 insertions(+), 7 deletions(-) diff --git a/data/uking_functions.csv b/data/uking_functions.csv index 690f660a5..9a7c8c7fe 100644 --- a/data/uking_functions.csv +++ b/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, diff --git a/src/KingSystem/ActorSystem/actActorConstDataAccess.h b/src/KingSystem/ActorSystem/actActorConstDataAccess.h index b78e9fbce..cf0cfb3c8 100644 --- a/src/KingSystem/ActorSystem/actActorConstDataAccess.h +++ b/src/KingSystem/ActorSystem/actActorConstDataAccess.h @@ -119,6 +119,8 @@ class ActorConstDataAccess : public ActorLinkConstDataAccess { f32 getHorseAvoidOffset() const; bool horseTargetedIsCircularMoveAlways() const; + void setActor687(); + private: Actor* getActor() const; diff --git a/src/KingSystem/Map/mapObject.h b/src/KingSystem/Map/mapObject.h index e3634c808..1d8fc93d0 100644 --- a/src/KingSystem/Map/mapObject.h +++ b/src/KingSystem/Map/mapObject.h @@ -123,6 +123,8 @@ class Object { 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 @@ class Object { 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 @@ class Object { 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; } diff --git a/src/KingSystem/Map/mapObjectLink.cpp b/src/KingSystem/Map/mapObjectLink.cpp index 700ab069d..95aaf47be 100644 --- a/src/KingSystem/Map/mapObjectLink.cpp +++ b/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 @@ bool ObjectLinkArray::checkLink(MapLinkDefType t, bool b) { 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); } diff --git a/src/KingSystem/Map/mapObjectLink.h b/src/KingSystem/Map/mapObjectLink.h index 382aa804d..b4ff79d63 100644 --- a/src/KingSystem/Map/mapObjectLink.h +++ b/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 @@ class ObjectLinkData { 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 @@ class ObjectLinkData { 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); From 1b916db6899276a598d798ea9aac3464951a1cc2 Mon Sep 17 00:00:00 2001 From: theo3 Date: Wed, 31 May 2023 21:40:48 -0700 Subject: [PATCH 2/2] phys::Constraint work --- data/uking_functions.csv | 40 +- .../Constraint/hkpConstraintListener.h | 40 ++ src/KingSystem/Physics/CMakeLists.txt | 3 +- .../Physics/Constraint/physConstraint.cpp | 418 +++++++++++++++++- .../Physics/Constraint/physConstraint.h | 84 +++- .../Constraint/physConstraintListener.h | 12 + .../Physics/RigidBody/physRigidBody.h | 4 +- .../RigidBody/physRigidBodyRequestMgr.h | 6 + src/KingSystem/Physics/System/physSystem.h | 20 +- 9 files changed, 599 insertions(+), 28 deletions(-) create mode 100644 lib/hkStubs/Havok/Physics2012/Dynamics/Constraint/hkpConstraintListener.h create mode 100644 src/KingSystem/Physics/Constraint/physConstraintListener.h diff --git a/data/uking_functions.csv b/data/uking_functions.csv index 9a7c8c7fe..a5b251c6a 100644 --- a/data/uking_functions.csv +++ b/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, diff --git a/lib/hkStubs/Havok/Physics2012/Dynamics/Constraint/hkpConstraintListener.h b/lib/hkStubs/Havok/Physics2012/Dynamics/Constraint/hkpConstraintListener.h new file mode 100644 index 000000000..406cdaa5b --- /dev/null +++ b/lib/hkStubs/Havok/Physics2012/Dynamics/Constraint/hkpConstraintListener.h @@ -0,0 +1,40 @@ +#pragma once + +#include + +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 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) {} +}; diff --git a/src/KingSystem/Physics/CMakeLists.txt b/src/KingSystem/Physics/CMakeLists.txt index a4efb3655..68617e68e 100644 --- a/src/KingSystem/Physics/CMakeLists.txt +++ b/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 - ) +) diff --git a/src/KingSystem/Physics/Constraint/physConstraint.cpp b/src/KingSystem/Physics/Constraint/physConstraint.cpp index 25874e794..399c4f327 100644 --- a/src/KingSystem/Physics/Constraint/physConstraint.cpp +++ b/src/KingSystem/Physics/Constraint/physConstraint.cpp @@ -1,7 +1,423 @@ -#include "KingSystem/Physics/Constraint//physConstraint.h" +#include "KingSystem/Physics/Constraint/physConstraint.h" +#include +#include +#include +#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(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(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; } diff --git a/src/KingSystem/Physics/Constraint/physConstraint.h b/src/KingSystem/Physics/Constraint/physConstraint.h index cfb52638a..a117abbc3 100644 --- a/src/KingSystem/Physics/Constraint/physConstraint.h +++ b/src/KingSystem/Physics/Constraint/physConstraint.h @@ -1,19 +1,97 @@ #pragma once +#include +#include +#include +#include +#include +#include "KingSystem/Physics/System/physSystem.h" -#include +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 mActiveBodies{}; + sead::SafeArray mSleepingBodies{}; + sead::TypedBitFlag mFlags{}; + sead::TypedBitFlag mSysFlags{}; + sead::CriticalSection mCS{}; + float mWeightParent = 1.0; + float mWeightChild = 1.0; + ConstraintCallback** mCallback{}; + sead::SpinLock mSpinLock{}; }; } // namespace ksys::phys diff --git a/src/KingSystem/Physics/Constraint/physConstraintListener.h b/src/KingSystem/Physics/Constraint/physConstraintListener.h new file mode 100644 index 000000000..1b521aed7 --- /dev/null +++ b/src/KingSystem/Physics/Constraint/physConstraintListener.h @@ -0,0 +1,12 @@ +#pragma once + +#include + +namespace ksys::phys { + +// TODO +class ConstraintListener : public hkpConstraintListener { + virtual ~ConstraintListener() = default; +}; + +}; // namespace ksys::phys diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.h b/src/KingSystem/Physics/RigidBody/physRigidBody.h index 6ba837673..8495877bd 100644 --- a/src/KingSystem/Physics/RigidBody/physRigidBody.h +++ b/src/KingSystem/Physics/RigidBody/physRigidBody.h @@ -98,7 +98,7 @@ class RigidBody : public sead::IDisposer, public RigidBase { _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 @@ class RigidBody : public sead::IDisposer, public RigidBase { 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; } diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h b/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h index 1597b6430..e300a5220 100644 --- a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h +++ b/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 @@ class RigidBodyRequestMgr : public sead::hostio::Node { 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; diff --git a/src/KingSystem/Physics/System/physSystem.h b/src/KingSystem/Physics/System/physSystem.h index 07f6b5011..48f218138 100644 --- a/src/KingSystem/Physics/System/physSystem.h +++ b/src/KingSystem/Physics/System/physSystem.h @@ -4,10 +4,12 @@ #include #include #include +#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 @@ class System { 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 @@ class System { // 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 @@ class System { // FIXME: type sead::FixedPtrArray _128; ContactMgr* mContactMgr; - void* _150; + ConstraintMgr* mConstraintMgr; StaticCompoundMgr* mStaticCompoundMgr; RigidBodyRequestMgr* mRigidBodyRequestMgr; RagdollInstanceMgr* mRagdollInstanceMgr; @@ -159,7 +175,7 @@ class System { SystemData* mSystemData; MaterialTable* mMaterialTable; void* _188{}; - void* _190{}; + RigidBody* mWorldRigidBody{}; void* _198{}; void* _1a0{}; sead::Heap* mPhysicsSystemHeap{};