ksys/phys: Add RigidBodySetFromShape and RigidBodySetFromResource (#234)

13 files changed, 204 insertions, 12 deletions

diff --git a/src/KingSystem/Physics/CMakeLists.txt b/src/KingSystem/Physics/CMakeLists.txt
index 03102915..b1312999 100644
--- a/src/KingSystem/Physics/CMakeLists.txt
+++ b/src/KingSystem/Physics/CMakeLists.txt
@@ -72,6 +72,10 @@ target_sources(uking PRIVATE
     RigidBody/physRigidBodyResource.h
     RigidBody/physRigidBodySet.cpp
     RigidBody/physRigidBodySet.h
+    RigidBody/physRigidBodySetFromResource.cpp
+    RigidBody/physRigidBodySetFromResource.h
+    RigidBody/physRigidBodySetFromShape.cpp
+    RigidBody/physRigidBodySetFromShape.h
     RigidBody/physRigidBodySetParam.cpp
     RigidBody/physRigidBodySetParam.h
     RigidBody/physRigidBodySphere.cpp
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
index e910e5a9..82969256 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
+++ b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
@@ -1808,16 +1808,18 @@ hkpMotion* RigidBody::getMotion() const {
     return getHkBody()->getMotion();
 }
 
-void RigidBody::setEntityMotionFlag1(bool set) {
+void RigidBody::setAlwaysCharacterMassScaling(bool set) {
     if (!isEntity() || !mMotionAccessor)
         return;
-    getEntityMotionAccessor()->changeFlag(RigidBodyMotionEntity::Flag::_1, set);
+    getEntityMotionAccessor()->changeFlag(RigidBodyMotionEntity::Flag::AlwaysCharacterMassScaling,
+                                          set);
 }
 
-bool RigidBody::isEntityMotionFlag1On() const {
+bool RigidBody::isAlwaysCharacterMassScaling() const {
     if (!isEntity() || !mMotionAccessor)
         return false;
-    return getEntityMotionAccessor()->hasFlag(RigidBodyMotionEntity::Flag::_1);
+    return getEntityMotionAccessor()->hasFlag(
+        RigidBodyMotionEntity::Flag::AlwaysCharacterMassScaling);
 }
 
 void RigidBody::setEntityMotionFlag100(bool set) {
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.h b/src/KingSystem/Physics/RigidBody/physRigidBody.h
index 8d2e2164..8838d283 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.h
+++ b/src/KingSystem/Physics/RigidBody/physRigidBody.h
@@ -542,8 +542,8 @@ public:
     // 0x0000007100f96a4c
     void setRequestMgrContactCallbackEnabled(bool enabled);
 
-    void setEntityMotionFlag1(bool set);
-    bool isEntityMotionFlag1On() const;
+    void setAlwaysCharacterMassScaling(bool set);
+    bool isAlwaysCharacterMassScaling() const;
 
     void setEntityMotionFlag100(bool set);
     bool isEntityMotionFlag100On() const;
@@ -588,6 +588,8 @@ public:
     void setUpdateRequestedFlag() { mFlags.set(Flag::UpdateRequested); }
     // Internal.
     void setFlag20() { mFlags.set(Flag::_20); }
+    // Internal.
+    void setNoCharStandingOn(bool set) { mFlags.change(Flag::NoCharStandingOn, set); }
 
     // Internal.
     void onCollisionAdded() {
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyFromResource.cpp b/src/KingSystem/Physics/RigidBody/physRigidBodyFromResource.cpp
index fe64a482..67a3466c 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodyFromResource.cpp
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyFromResource.cpp
@@ -18,7 +18,7 @@ static const char* getRigidBodyResourceName(const hkpRigidBody* hk_body) {
     return "RigidBodyFromResource";
 }
 
-RigidBodyFromResource::RigidBodyFromResource(float volume, hkpRigidBody* hk_body,
+RigidBodyFromResource::RigidBodyFromResource(hkpRigidBody* hk_body, float volume,
                                              ContactLayerType layer_type, sead::Heap* heap,
                                              RigidBody::Type type)
     : RigidBody(type, layer_type, hk_body, getRigidBodyResourceName(hk_body), heap, true),
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyFromResource.h b/src/KingSystem/Physics/RigidBody/physRigidBodyFromResource.h
index f9709faa..779a8b95 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodyFromResource.h
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyFromResource.h
@@ -7,7 +7,7 @@ namespace ksys::phys {
 class RigidBodyFromResource : public RigidBody {
     SEAD_RTTI_OVERRIDE(RigidBodyFromResource, RigidBody)
 public:
-    RigidBodyFromResource(float volume, hkpRigidBody* hk_body, ContactLayerType layer_type,
+    RigidBodyFromResource(hkpRigidBody* hk_body, float volume, ContactLayerType layer_type,
                           sead::Heap* heap, Type type);
     ~RigidBodyFromResource() override;
 
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyFromShape.cpp b/src/KingSystem/Physics/RigidBody/physRigidBodyFromShape.cpp
index 286c6242..9e60cef4 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodyFromShape.cpp
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyFromShape.cpp
@@ -322,7 +322,7 @@ RigidBodyT* RigidBodyFromShape::make(ShapeT* shape, bool set_flag_10,
     body->enableWaterCollision(!param.no_hit_water);
     body->mFlags.change(Flag::NoCharStandingOn, param.no_char_standing_on);
     body->setContactMask(param.contact_mask);
-    body->setEntityMotionFlag1(param.always_character_mass_scaling);
+    body->setAlwaysCharacterMassScaling(param.always_character_mass_scaling);
     body->processUpdateRequests(nullptr, nullptr);
 
     return static_cast<RigidBodyT*>(body);
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyMotionEntity.h b/src/KingSystem/Physics/RigidBody/physRigidBodyMotionEntity.h
index a967ec2a..4f204194 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodyMotionEntity.h
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyMotionEntity.h
@@ -18,7 +18,7 @@ class RigidBodyMotionEntity : public MotionAccessor {
     SEAD_RTTI_OVERRIDE(RigidBodyMotionEntity, MotionAccessor)
 public:
     enum class Flag {
-        _1 = 1 << 0,
+        AlwaysCharacterMassScaling = 1 << 0,
         _2 = 1 << 1,
         _4 = 1 << 2,
         _8 = 1 << 3,
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodySet.h b/src/KingSystem/Physics/RigidBody/physRigidBodySet.h
index fb0e8d44..46f51c73 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodySet.h
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodySet.h
@@ -10,6 +10,7 @@
 namespace ksys::phys {
 
 class RigidBody;
+struct RigidBodyInstanceParam;
 class SystemGroupHandler;
 class UserTag;
 
@@ -68,9 +69,21 @@ public:
     bool hasNoRigidBodyWithFlag8(bool require_motion_flag_1_to_be_unset);
     void requestSuspendGravity(u8 type);
 
-private:
+protected:
     sead::SafeString mName;
     sead::PtrArray<RigidBody> mRigidBodies;
 };
 
+/// Provides the per-body parameters that a RigidBodySet subclass needs to create its bodies.
+class RigidBodySetParamAccessor {
+public:
+    virtual int getNumRigidBodies() const = 0;
+    virtual RigidBody* createRigidBody(int idx, sead::Heap* heap) const = 0;
+    virtual void makeInstanceParam(int idx, RigidBodyInstanceParam* param) const = 0;
+    virtual float getVolume(int idx) const = 0;
+
+protected:
+    ~RigidBodySetParamAccessor() = default;
+};
+
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodySetFromResource.cpp b/src/KingSystem/Physics/RigidBody/physRigidBodySetFromResource.cpp
new file mode 100644
index 00000000..b45d34bd
--- /dev/null
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodySetFromResource.cpp
@@ -0,0 +1,89 @@
+#include "KingSystem/Physics/RigidBody/physRigidBodySetFromResource.h"
+#include <Havok/Common/Serialize/Util/hkNativePackfileUtils.h>
+#include <Havok/Common/Serialize/Util/hkRootLevelContainer.h>
+#include <Havok/Physics2012/Dynamics/World/hkpPhysicsSystem.h>
+#include <Havok/Physics2012/Utilities/Serialize/hkpPhysicsData.h>
+#include <cstring>
+#include "KingSystem/Physics/RigidBody/physRigidBodyFromResource.h"
+#include "KingSystem/Physics/RigidBody/physRigidBodyParam.h"
+#include "KingSystem/Physics/RigidBody/physRigidBodyResource.h"
+
+namespace ksys::phys {
+
+RigidBodySetFromResource::RigidBodySetFromResource(const sead::SafeString& name,
+                                                   RigidBodyResource* resource)
+    : RigidBodySet(name), mResource(resource) {}
+
+RigidBodySetFromResource::~RigidBodySetFromResource() {
+    finalize();
+}
+
+bool RigidBodySetFromResource::init(const RigidBodySetParamAccessor& accessor, sead::Heap* heap) {
+    // Copy the resource data and load it in place.
+    mBufferSize = mResource->getRawSize();
+    mBuffer = new (heap, 0x10) u8[mBufferSize];
+    std::memcpy(mBuffer, mResource->getRawData(), mBufferSize);
+
+    const char* error;
+    auto* container = static_cast<hkRootLevelContainer*>(hkNativePackfileUtils::loadInPlace(
+        mBuffer, static_cast<int>(mBufferSize), nullptr, &error));
+    auto* physics_data = container->findObject<hkpPhysicsData>();
+    const auto& hk_bodies = physics_data->getPhysicsSystems()[0]->getRigidBodies();
+
+    const int num_bodies = hk_bodies.getSize();
+    mRigidBodies.allocBuffer(num_bodies, heap);
+
+    for (int i = 0; i < num_bodies; ++i) {
+        RigidBodyInstanceParam param;
+        accessor.makeInstanceParam(i, &param);
+
+        const auto layer_type = getContactLayerType(param.contact_layer);
+        auto* body = new (heap) RigidBodyFromResource(hk_bodies[i], accessor.getVolume(i),
+                                                      layer_type, heap, RigidBody::Type::_2);
+        mRigidBodies.pushBack(body);
+        body->setUpdateRequestedFlag();
+
+        if (!body->init(param, heap)) {
+            body->processUpdateRequests();
+            return false;
+        }
+
+        body->setCollisionFilterInfo(0);
+
+        if (param.contact_layer == ContactLayer::SensorCustomReceiver) {
+            body->setSensorCustomReceiver(param.receiver_mask, param.system_group_handler);
+        } else if (param.groundhit_mask != 0) {
+            body->setGroundHitMask(param.contact_layer, param.groundhit_mask);
+        } else {
+            body->setContactLayerAndGroundHitAndHandler(param.contact_layer, param.groundhit,
+                                                        param.system_group_handler);
+        }
+
+        body->clearEntityMotionFlag20(!param.ignore_normal_for_impulse);
+        body->enableGroundCollision(!param.no_hit_ground);
+        body->enableWaterCollision(!param.no_hit_water);
+        body->setNoCharStandingOn(param.no_char_standing_on);
+        body->setContactMask(param.contact_mask);
+        body->setAlwaysCharacterMassScaling(param.always_character_mass_scaling);
+        body->processUpdateRequests();
+    }
+
+    return true;
+}
+
+void RigidBodySetFromResource::finalize() {
+    auto& bodies = getRigidBodies();
+    for (int i = 0, n = bodies.size(); i < n; ++i) {
+        bodies[i]->removeFromWorldImmediately();
+        delete bodies[i];
+    }
+    mRigidBodies.freeBuffer();
+
+    if (mBuffer) {
+        hkNativePackfileUtils::unloadInPlace(mBuffer, static_cast<int>(mBufferSize));
+        delete[] mBuffer;
+        mBuffer = nullptr;
+    }
+}
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodySetFromResource.h b/src/KingSystem/Physics/RigidBody/physRigidBodySetFromResource.h
new file mode 100644
index 00000000..d501c5f1
--- /dev/null
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodySetFromResource.h
@@ -0,0 +1,26 @@
+#pragma once
+
+#include "KingSystem/Physics/RigidBody/physRigidBodySet.h"
+
+namespace ksys::phys {
+
+class RigidBodyResource;
+
+/// A rigid body set whose bodies are created from the hkpRigidBodies in a Havok packfile
+/// (RigidBodySetParam::Type::FromResource).
+class RigidBodySetFromResource : public RigidBodySet {
+public:
+    RigidBodySetFromResource(const sead::SafeString& name, RigidBodyResource* resource);
+    ~RigidBodySetFromResource() override;
+
+    bool init(const RigidBodySetParamAccessor& accessor, sead::Heap* heap);
+    void finalize();
+
+private:
+    RigidBodyResource* mResource;
+    /// A copy of the resource data, loaded in place.
+    u8* mBuffer = nullptr;
+    u32 mBufferSize = 0;
+};
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodySetFromShape.cpp b/src/KingSystem/Physics/RigidBody/physRigidBodySetFromShape.cpp
new file mode 100644
index 00000000..5b088638
--- /dev/null
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodySetFromShape.cpp
@@ -0,0 +1,27 @@
+#include "KingSystem/Physics/RigidBody/physRigidBodySetFromShape.h"
+
+namespace ksys::phys {
+
+RigidBodySetFromShape::RigidBodySetFromShape(const sead::SafeString& name) : RigidBodySet(name) {}
+
+RigidBodySetFromShape::~RigidBodySetFromShape() {
+    destroyRigidBodies();
+}
+
+bool RigidBodySetFromShape::init(const RigidBodySetParamAccessor& accessor, sead::Heap* heap) {
+    const int num_bodies = accessor.getNumRigidBodies();
+    mRigidBodies.allocBuffer(num_bodies, heap);
+
+    for (int i = 0; i < num_bodies; ++i) {
+        RigidBody* body = accessor.createRigidBody(i, heap);
+        if (!body) {
+            destroyRigidBodies();
+            return false;
+        }
+        mRigidBodies.pushBack(body);
+    }
+
+    return true;
+}
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodySetFromShape.h b/src/KingSystem/Physics/RigidBody/physRigidBodySetFromShape.h
new file mode 100644
index 00000000..8eef78dd
--- /dev/null
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodySetFromShape.h
@@ -0,0 +1,29 @@
+#pragma once
+
+#include "KingSystem/Physics/RigidBody/physRigidBody.h"
+#include "KingSystem/Physics/RigidBody/physRigidBodySet.h"
+
+namespace ksys::phys {
+
+/// A rigid body set whose bodies are created from the shape parameters in the actor's
+/// physics parameters (RigidBodySetParam::Type::FromShapeType).
+class RigidBodySetFromShape : public RigidBodySet {
+public:
+    explicit RigidBodySetFromShape(const sead::SafeString& name);
+    ~RigidBodySetFromShape() override;
+
+    bool init(const RigidBodySetParamAccessor& accessor, sead::Heap* heap);
+
+private:
+    void destroyRigidBodies() {
+        auto& bodies = getRigidBodies();
+        for (int i = 0, n = bodies.size(); i < n; ++i) {
+            if (bodies[i]->isAddedToWorld())
+                bodies[i]->removeFromWorldImmediately();
+            delete bodies[i];
+        }
+        mRigidBodies.freeBuffer();
+    }
+};
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/System/physStaticCompoundRigidBodyGroup.cpp b/src/KingSystem/Physics/System/physStaticCompoundRigidBodyGroup.cpp
index a3da25c2..f6f0aa2e 100644
--- a/src/KingSystem/Physics/System/physStaticCompoundRigidBodyGroup.cpp
+++ b/src/KingSystem/Physics/System/physStaticCompoundRigidBodyGroup.cpp
@@ -69,7 +69,7 @@ void StaticCompoundRigidBodyGroup::init(const hkpPhysicsSystem& system, sead::Ma
             bool is_entity = sead::SafeString(hk_body->getName()).include("Entity");
             auto layer_type = is_entity ? ContactLayerType::Entity : ContactLayerType::Sensor;
 
-            auto* body = new (heap) RigidBodyFromResource(0.0, hk_body, layer_type, heap,
+            auto* body = new (heap) RigidBodyFromResource(hk_body, 0.0, layer_type, heap,
                                                           RigidBody::Type::StaticCompoundBody);
 
             RigidBodyInstanceParam param;