ksys/phys: Add RigidBodyFromResource

data/uking_functions.csv

@@ -83258,20 +83258,20 @@ Address,Quality,Size,Name
 0x0000007100f9899c,O,000068,_ZN4ksys4phys22CylinderWaterRigidBody17getCollisionMasksEPNS0_9RigidBody14CollisionMasksE
 0x0000007100f989e0,O,000288,_ZNK4ksys4phys22CylinderWaterRigidBody27checkDerivedRuntimeTypeInfoEPKN4sead15RuntimeTypeInfo9InterfaceE
 0x0000007100f98b00,O,000092,_ZNK4ksys4phys22CylinderWaterRigidBody18getRuntimeTypeInfoEv
-0x0000007100f98b5c,U,000168,RigidBodyFromResource::ctor
-0x0000007100f98c04,U,000004,j_RigidBody::dtor_0
-0x0000007100f98c08,U,000008,
-0x0000007100f98c10,U,000036,
-0x0000007100f98c34,U,000040,
-0x0000007100f98c5c,U,000108,
-0x0000007100f98cc8,U,000048,
-0x0000007100f98cf8,U,000312,
-0x0000007100f98e30,U,000076,
-0x0000007100f98e7c,U,000076,
-0x0000007100f98ec8,U,000392,
-0x0000007100f99050,U,000008,
-0x0000007100f99058,U,000204,
-0x0000007100f99124,U,000092,
+0x0000007100f98b5c,O,000168,_ZN4ksys4phys21RigidBodyFromResourceC1EfP12hkpRigidBodyNS0_16ContactLayerTypeEPN4sead4HeapENS0_9RigidBody4TypeE
+0x0000007100f98c04,O,000004,_ZN4ksys4phys21RigidBodyFromResourceD1Ev
+0x0000007100f98c08,O,000008,_ZThn32_N4ksys4phys21RigidBodyFromResourceD1Ev
+0x0000007100f98c10,O,000036,_ZN4ksys4phys21RigidBodyFromResourceD0Ev
+0x0000007100f98c34,O,000040,_ZThn32_N4ksys4phys21RigidBodyFromResourceD0Ev
+0x0000007100f98c5c,O,000108,_ZN4ksys4phys21RigidBodyFromResource4initERKNS0_22RigidBodyInstanceParamEPN4sead4HeapE
+0x0000007100f98cc8,O,000048,_ZNK4ksys4phys21RigidBodyFromResource24isBvTreeOrStaticCompoundEv
+0x0000007100f98cf8,O,000312,_ZNK4ksys4phys21RigidBodyFromResource10isMaterialENS0_8MaterialE
+0x0000007100f98e30,O,000076,_ZN4ksys4phys21RigidBodyFromResource17getCollisionMasksEPNS0_9RigidBody14CollisionMasksEPKj
+0x0000007100f98e7c,O,000076,_ZN4ksys4phys21RigidBodyFromResource12updateScale_Eff
+0x0000007100f98ec8,O,000392,_ZN4ksys4phys21RigidBodyFromResource17getNewHavokShape_Ev
+0x0000007100f99050,O,000008,_ZN4ksys4phys21RigidBodyFromResource9getVolumeEv
+0x0000007100f99058,O,000204,_ZNK4ksys4phys21RigidBodyFromResource27checkDerivedRuntimeTypeInfoEPKN4sead15RuntimeTypeInfo9InterfaceE
+0x0000007100f99124,O,000092,_ZNK4ksys4phys21RigidBodyFromResource18getRuntimeTypeInfoEv
 0x0000007100f99180,O,000188,_ZN4ksys4phys16RigidBodyFactory12createSphereEPNS0_22RigidBodyInstanceParamEPN4sead4HeapE
 0x0000007100f9923c,O,000188,_ZN4ksys4phys16RigidBodyFactory13createCapsuleEPNS0_22RigidBodyInstanceParamEPN4sead4HeapE
 0x0000007100f992f8,O,000188,_ZN4ksys4phys16RigidBodyFactory14createCylinderEPNS0_22RigidBodyInstanceParamEPN4sead4HeapE

lib/hkStubs/CMakeLists.txt

@@ -108,8 +108,9 @@ add_library(hkStubs OBJECT
   Havok/Physics2012/Collide/Query/CastUtil/hkpWorldRayCastInput.h
   Havok/Physics2012/Collide/Shape/hkpShape.h
   Havok/Physics2012/Collide/Shape/hkpShapeBase.h
-  Havok/Physics2012/Collide/Shape/hkpShapeType.h
+  Havok/Physics2012/Collide/Shape/hkpShapeBuffer.h
   Havok/Physics2012/Collide/Shape/hkpShapeContainer.h
+  Havok/Physics2012/Collide/Shape/hkpShapeType.h
   Havok/Physics2012/Collide/Shape/Compound/Collection/hkpShapeCollection.h
   Havok/Physics2012/Collide/Shape/Compound/Collection/List/hkpListShape.h
   Havok/Physics2012/Collide/Shape/Compound/Tree/hkpBvTreeShape.h
@@ -168,6 +169,8 @@ add_library(hkStubs OBJECT
 
   Havok/Physics2012/Internal/Collide/Mopp/Code/hkpMoppCode.h
 
+  Havok/Physics2012/Utilities/Collide/ShapeUtils/ShapeScaling/hkpShapeScalingUtility.h
+  Havok/Physics2012/Utilities/Dynamics/ScaleSystem/hkpSystemScalingUtility.h
   Havok/Physics2012/Utilities/Serialize/hkpPhysicsData.h
 )
 

lib/hkStubs/Havok/Physics2012/Collide/Shape/hkpShapeBuffer.h

@@ -0,0 +1,14 @@
+#pragma once
+
+constexpr int HK_SHAPE_BUFFER_SIZE = 512;
+
+struct hkpShapeBufferStorage {
+    alignas(16) char m_storage[HK_SHAPE_BUFFER_SIZE];
+
+    template <typename T>
+    operator T*() {  // NOLINT(google-explicit-constructor)
+        return reinterpret_cast<T*>(m_storage);
+    }
+};
+
+using hkpShapeBuffer = hkpShapeBufferStorage;

lib/hkStubs/Havok/Physics2012/Collide/Shape/hkpShapeContainer.h

@@ -2,8 +2,7 @@
 
 #include <Havok/Common/Base/hkBase.h>
 #include <Havok/Physics2012/Collide/Shape/hkpShape.h>
-
-class hkpShapeBuffer;
+#include <Havok/Physics2012/Collide/Shape/hkpShapeBuffer.h>
 
 class hkpShapeContainer {
 public:

lib/hkStubs/Havok/Physics2012/Utilities/Collide/ShapeUtils/ShapeScaling/hkpShapeScalingUtility.h

@@ -0,0 +1,25 @@
+#pragma once
+
+#include <Havok/Common/Base/hkBase.h>
+
+class hkpShape;
+
+class hkpShapeScalingUtility {
+public:
+    HK_DECLARE_CLASS_ALLOCATOR(hkpShapeScalingUtility)
+
+    struct ShapePair {
+        HK_DECLARE_CLASS_ALLOCATOR(ShapePair)
+
+        hkpShape* originalShape;
+        hkpShape* newShape;
+    };
+
+    static hkpShape* scaleShapeSimd(hkpShape* shape, hkSimdRealParameter scale,
+                                    hkArray<ShapePair>* doneShapes = nullptr);
+
+    static hkpShape* scaleShape(hkpShape* shape, hkReal scale,
+                                hkArray<ShapePair>* doneShapes = nullptr) {
+        return scaleShapeSimd(shape, hkSimdReal(scale), doneShapes);
+    }
+};

lib/hkStubs/Havok/Physics2012/Utilities/Dynamics/ScaleSystem/hkpSystemScalingUtility.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include <Havok/Common/Base/hkBase.h>
+#include <Havok/Physics2012/Utilities/Collide/ShapeUtils/ShapeScaling/hkpShapeScalingUtility.h>
+
+class hkpPhysicsSystem;
+
+class hkpSystemScalingUtility {
+public:
+    static void scaleSystemSimd(hkpPhysicsSystem* system, hkSimdRealParameter scale,
+                                hkArray<hkpShapeScalingUtility::ShapePair>* doneShapes = nullptr);
+
+    static void scaleSystem(hkpPhysicsSystem* system, hkReal scale,
+                            hkArray<hkpShapeScalingUtility::ShapePair>* doneShapes = nullptr) {
+        scaleSystemSimd(system, hkSimdReal(scale), doneShapes);
+    }
+};

src/KingSystem/Physics/CMakeLists.txt

@@ -23,6 +23,8 @@ target_sources(uking PRIVATE
   RigidBody/physRigidBodyAccessor.h
   RigidBody/physRigidBodyFactory.cpp
   RigidBody/physRigidBodyFactory.h
+  RigidBody/physRigidBodyFromResource.cpp
+  RigidBody/physRigidBodyFromResource.h
   RigidBody/physRigidBodyFromShape.cpp
   RigidBody/physRigidBodyFromShape.h
   RigidBody/physRigidBodyMotionEntity.cpp

src/KingSystem/Physics/RigidBody/Shape/Box/physBoxRigidBody.cpp

@@ -65,7 +65,7 @@ const Shape* BoxRigidBody::getShape_() const {
     return mShape;
 }
 
-u32 BoxRigidBody::getCollisionMasks(RigidBody::CollisionMasks* masks) {
+u32 BoxRigidBody::getCollisionMasks(RigidBody::CollisionMasks* masks, const u32* unk) {
     masks->ignored_layers = ~mContactMask.getDirect();
     masks->collision_filter_info = getCollisionFilterInfo();
     masks->material_mask = getMaterialMask().getRawData();

src/KingSystem/Physics/RigidBody/Shape/Box/physBoxRigidBody.h

@@ -33,7 +33,7 @@ public:
 protected:
     Shape* getShape_() override;
     const Shape* getShape_() const override;
-    u32 getCollisionMasks(CollisionMasks* masks) override;
+    u32 getCollisionMasks(RigidBody::CollisionMasks* masks, const u32* unk) override;
 
     BoxShape* mShape;
 };

src/KingSystem/Physics/RigidBody/Shape/BoxWater/physBoxWaterRigidBody.cpp

@@ -55,7 +55,7 @@ const Shape* BoxWaterRigidBody::getShape_() const {
     return mShape;
 }
 
-u32 BoxWaterRigidBody::getCollisionMasks(RigidBody::CollisionMasks* masks) {
+u32 BoxWaterRigidBody::getCollisionMasks(RigidBody::CollisionMasks* masks, const u32* unk) {
     masks->ignored_layers = ~mContactMask.getDirect();
     masks->collision_filter_info = getCollisionFilterInfo();
     masks->material_mask = getMaterialMask().getRawData();

src/KingSystem/Physics/RigidBody/Shape/BoxWater/physBoxWaterRigidBody.h

@@ -31,7 +31,7 @@ public:
 protected:
     Shape* getShape_() override;
     const Shape* getShape_() const override;
-    u32 getCollisionMasks(CollisionMasks* masks) override;
+    u32 getCollisionMasks(RigidBody::CollisionMasks* masks, const u32* unk) override;
 
     BoxWaterShape* mShape;
     u32 _d8{};

src/KingSystem/Physics/RigidBody/Shape/Capsule/physCapsuleRigidBody.cpp

@@ -66,7 +66,7 @@ const Shape* CapsuleRigidBody::getShape_() const {
     return mShape;
 }
 
-u32 CapsuleRigidBody::getCollisionMasks(RigidBody::CollisionMasks* masks) {
+u32 CapsuleRigidBody::getCollisionMasks(RigidBody::CollisionMasks* masks, const u32* unk) {
     masks->ignored_layers = ~mContactMask.getDirect();
     masks->collision_filter_info = getCollisionFilterInfo();
     masks->material_mask = getMaterialMask().getRawData();

src/KingSystem/Physics/RigidBody/Shape/Capsule/physCapsuleRigidBody.h

@@ -31,7 +31,7 @@ public:
 protected:
     Shape* getShape_() override;
     const Shape* getShape_() const override;
-    u32 getCollisionMasks(CollisionMasks* masks) override;
+    u32 getCollisionMasks(RigidBody::CollisionMasks* masks, const u32* unk) override;
 
     CapsuleShape* mShape{};
 };

src/KingSystem/Physics/RigidBody/Shape/Cylinder/physCylinderRigidBody.cpp

@@ -65,7 +65,7 @@ const Shape* CylinderRigidBody::getShape_() const {
     return mShape;
 }
 
-u32 CylinderRigidBody::getCollisionMasks(RigidBody::CollisionMasks* masks) {
+u32 CylinderRigidBody::getCollisionMasks(RigidBody::CollisionMasks* masks, const u32* unk) {
     masks->ignored_layers = ~mContactMask.getDirect();
     masks->collision_filter_info = getCollisionFilterInfo();
     masks->material_mask = getMaterialMask().getRawData();

src/KingSystem/Physics/RigidBody/Shape/Cylinder/physCylinderRigidBody.h

@@ -30,7 +30,7 @@ public:
 protected:
     Shape* getShape_() override;
     const Shape* getShape_() const override;
-    u32 getCollisionMasks(CollisionMasks* masks) override;
+    u32 getCollisionMasks(RigidBody::CollisionMasks* masks, const u32* unk) override;
 
 private:
     CylinderShape* mShape;

src/KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterRigidBody.cpp

@@ -59,7 +59,7 @@ const Shape* CylinderWaterRigidBody::getShape_() const {
     return mShape;
 }
 
-u32 CylinderWaterRigidBody::getCollisionMasks(RigidBody::CollisionMasks* masks) {
+u32 CylinderWaterRigidBody::getCollisionMasks(RigidBody::CollisionMasks* masks, const u32* unk) {
     masks->ignored_layers = ~mContactMask.getDirect();
     masks->collision_filter_info = getCollisionFilterInfo();
     masks->material_mask = getMaterialMask().getRawData();

src/KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterRigidBody.h

@@ -30,7 +30,7 @@ public:
 protected:
     Shape* getShape_() override;
     const Shape* getShape_() const override;
-    u32 getCollisionMasks(CollisionMasks* masks) override;
+    u32 getCollisionMasks(RigidBody::CollisionMasks* masks, const u32* unk) override;
 
 private:
     CylinderWaterShape* mShape{};

src/KingSystem/Physics/RigidBody/TeraMesh/physTeraMeshRigidBody.h

@@ -9,7 +9,7 @@ class TeraMeshRigidBody : public RigidBody {
 public:
     TeraMeshRigidBody(hkpRigidBody* hk_body, sead::Heap* heap);
 
-    u32 getCollisionMasks(CollisionMasks* masks) override;
+    u32 getCollisionMasks(RigidBody::CollisionMasks* masks, const u32* unk) override;
 };
 
 }  // namespace ksys::phys

src/KingSystem/Physics/RigidBody/physRigidBody.h

@@ -520,7 +520,7 @@ public:
 
 protected:
     // FIXME: return type
-    virtual u32 getCollisionMasks(CollisionMasks* masks) = 0;
+    virtual u32 getCollisionMasks(RigidBody::CollisionMasks* masks, const u32* unk) = 0;
 
     /// Called whenever a shape update is requested.
     /// @return the new shape to use for the Havok rigid body or null to keep the current hkpShape

src/KingSystem/Physics/RigidBody/physRigidBodyFromResource.cpp

@@ -0,0 +1,149 @@
+#include "KingSystem/Physics/RigidBody/physRigidBodyFromResource.h"
+#include <Havok/Physics2012/Collide/Shape/hkpShapeContainer.h>
+#include <Havok/Physics2012/Dynamics/Entity/hkpRigidBody.h>
+#include <Havok/Physics2012/Dynamics/World/hkpPhysicsSystem.h>
+#include <Havok/Physics2012/Utilities/Dynamics/ScaleSystem/hkpSystemScalingUtility.h>
+#include <math/seadMathCalcCommon.h>
+#include "KingSystem/Physics/RigidBody/physRigidBodyParam.h"
+#include "KingSystem/Physics/System/physMaterialMask.h"
+#include "KingSystem/Physics/physConversions.h"
+
+namespace ksys::phys {
+
+static const char* getRigidBodyResourceName(const hkpRigidBody* hk_body) {
+    if (const char* name = hk_body->getName())
+        return name;
+
+    return "RigidBodyFromResource";
+}
+
+RigidBodyFromResource::RigidBodyFromResource(float volume, hkpRigidBody* hk_body,
+                                             ContactLayerType layer_type, sead::Heap* heap,
+                                             RigidBody::Type type)
+    : RigidBody(type, layer_type, hk_body, getRigidBodyResourceName(hk_body), heap, true),
+      mShape(hk_body->getCollidable()->getShape()), mVolume(volume) {}
+
+RigidBodyFromResource::~RigidBodyFromResource() = default;
+
+bool RigidBodyFromResource::init(const RigidBodyInstanceParam& param, sead::Heap* heap) {
+    if (!initMotionAccessor(param, heap, true))
+        return false;
+
+    updateCollidableQualityType(param.toi);
+    getHkBody()->getCollidableRw()->setMotionState(getHkBody()->getMotion()->getMotionState());
+
+    if (auto* shape = getHkBody()->getCollidable()->getShape()) {
+        hkVector4 extent_out;
+        getHkBody()->updateCachedShapeInfo(shape, extent_out);
+    }
+
+    return true;
+}
+
+bool RigidBodyFromResource::isBvTreeOrStaticCompound() const {
+    switch (mShape->getType()) {
+    case hkcdShapeType::TRI_SAMPLED_HEIGHT_FIELD_BV_TREE:
+    case hkcdShapeType::MOPP:
+    case hkcdShapeType::STATIC_COMPOUND:
+    case hkcdShapeType::BV_COMPRESSED_MESH:
+    case hkcdShapeType::BV_TREE:
+        return true;
+    default:
+        return false;
+    }
+}
+
+bool RigidBodyFromResource::isMaterial(Material material) const {
+    if (isBvTreeOrStaticCompound())
+        return false;
+
+    bool found_child_shape_with_material = false;
+    if (auto* container = mShape->getContainer()) {
+        for (auto key = container->getFirstKey(); key != HK_INVALID_SHAPE_KEY;
+             key = container->getNextKey(key)) {
+            hkpShapeBuffer buffer;
+            auto* shape = container->getChildShape(key, buffer);
+            if (!shape)
+                return false;
+
+            MaterialMask shape_material{MaterialMaskData(shape->getUserData())};
+            if (int(shape_material.getMaterial()) == material) {
+                found_child_shape_with_material = true;
+                break;
+            }
+        }
+    } else {
+        MaterialMask shape_material{MaterialMaskData(mShape->getUserData())};
+        if (int(shape_material.getMaterial()) == material)
+            return true;
+    }
+
+    return found_child_shape_with_material;
+}
+
+// FIXME: move this to the appropriate header
+// 0x0000007100fd0a1c
+u32 getCollisionFilterInfoFromCollidable(u32* material_mask, u32* collision_filter_info,
+                                         const hkpCollidable* collidable, const u32* unk);
+
+u32 RigidBodyFromResource::getCollisionMasks(RigidBody::CollisionMasks* masks, const u32* unk) {
+    masks->ignored_layers = ~mContactMask;
+    auto* collidable = getHkBody()->getCollidable();
+    if (unk != nullptr) {
+        return getCollisionFilterInfoFromCollidable(&masks->material_mask,
+                                                    &masks->collision_filter_info, collidable, unk);
+    }
+    masks->material_mask = collidable->getShape()->getUserData();
+    masks->collision_filter_info = collidable->getCollisionFilterInfo();
+    return 0;
+}
+
+float RigidBodyFromResource::updateScale_(float scale, float old_scale) {
+    if (getHkBody()->getCollidable()->getShape()->getType() == hkcdShapeType::BV_COMPRESSED_MESH) {
+        static_cast<void>(getPosition());
+        return old_scale;
+    }
+
+    mNewScale = scale;
+    updateShape();
+    return scale;
+}
+
+const hkpShape* RigidBodyFromResource::getNewHavokShape_() {
+    if (sead::Mathf::equalsEpsilon(mCurrentScale, mNewScale))
+        return nullptr;
+
+    const float ratio = mNewScale / mCurrentScale;
+
+    mVolume = ratio * ratio * ratio * mVolume;
+
+    const hkTransformf saved_transform = getHkBody()->getTransform();
+    hkTransformf identity;
+    identity.setIdentity();
+    getHkBody()->setTransform(identity);
+
+    // Actually scale it now.
+    // XXX: hkpSystemScalingUtility is not supposed to be used at runtime.
+    hkpPhysicsSystem system;
+    system.addRigidBody(getHkBody());
+    hkpSystemScalingUtility::scaleSystem(&system, ratio);
+    getHkBody()->setTransform(saved_transform);
+
+    if (getHkBody()->getMass() > 0) {
+        setMass(getHkBody()->getMass());
+        setCenterOfMassInLocal(toVec3(getHkBody()->getCenterOfMassLocal()));
+
+        hkMatrix3 inertia;
+        getHkBody()->getInertiaLocal(inertia);
+        setInertiaLocal({inertia.get<0, 0>(), inertia.get<1, 1>(), inertia.get<2, 2>()});
+    }
+
+    mCurrentScale = mNewScale;
+    return nullptr;
+}
+
+float RigidBodyFromResource::getVolume() {
+    return mVolume;
+}
+
+}  // namespace ksys::phys

src/KingSystem/Physics/RigidBody/physRigidBodyFromResource.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include "KingSystem/Physics/RigidBody/physRigidBody.h"
+
+namespace ksys::phys {
+
+class RigidBodyFromResource : public RigidBody {
+    SEAD_RTTI_OVERRIDE(RigidBodyFromResource, RigidBody)
+public:
+    RigidBodyFromResource(float volume, hkpRigidBody* hk_body, ContactLayerType layer_type,
+                          sead::Heap* heap, Type type);
+    ~RigidBodyFromResource() override;
+
+    bool init(const RigidBodyInstanceParam& param, sead::Heap* heap);
+
+    bool isBvTreeOrStaticCompound() const;
+    bool isMaterial(Material material) const;
+
+    float getVolume() override;
+    u32 getCollisionMasks(RigidBody::CollisionMasks* masks, const u32* unk) override;
+
+protected:
+    float updateScale_(float scale, float old_scale) override;
+    const hkpShape* getNewHavokShape_() override;
+
+private:
+    const hkpShape* mShape = nullptr;
+    float mVolume = 1.0;
+    float mCurrentScale = 1.0;
+    float mNewScale = 1.0;
+};
+
+}  // namespace ksys::phys