ksys/phys: Implement BoxShape

data/uking_functions.csv

@@ -83547,23 +83547,23 @@ Address,Quality,Size,Name
 0x0000007100fa9fac,O,000120,_ZNK4ksys4phys17RigidBodyAccessor16getPointVelocityEPN4sead7Vector3IfEERKS4_
 0x0000007100faa024,O,000020,_ZNK4ksys4phys17RigidBodyAccessor13getTimeFactorEv
 0x0000007100faa038,O,000064,_ZNK4ksys4phys17RigidBodyAccessor20getDeltaCenterOfMassEPN4sead7Vector3IfEES5_
-0x0000007100faa078,U,000564,
+0x0000007100faa078,O,000564,_ZN4ksys4phys13BoxShapeParam11createShapeEPN4sead4HeapE
 0x0000007100faa2ac,U,000224,
 0x0000007100faa38c,U,000048,
-0x0000007100faa3bc,U,000364,
-0x0000007100faa528,U,000080,
-0x0000007100faa578,U,000088,
-0x0000007100faa5d0,U,000120,
-0x0000007100faa648,U,000292,
-0x0000007100faa76c,U,000020,
-0x0000007100faa780,U,000028,
-0x0000007100faa79c,U,000028,
-0x0000007100faa7b8,U,000644,
-0x0000007100faaa3c,U,000128,
-0x0000007100faaabc,U,000120,
-0x0000007100faab34,U,000204,
-0x0000007100faac00,U,000092,
-0x0000007100faac5c,U,000008,
+0x0000007100faa3bc,O,000364,_ZN4ksys4phys8BoxShapeC1ERKNS0_13BoxShapeParamEP11hkpBoxShapeP23hkpConvexTransformShape
+0x0000007100faa528,O,000080,_ZN4ksys4phys8BoxShapeD1Ev
+0x0000007100faa578,O,000088,_ZN4ksys4phys8BoxShapeD0Ev
+0x0000007100faa5d0,O,000120,_ZN4ksys4phys8BoxShape10setExtentsERKN4sead7Vector3IfEE
+0x0000007100faa648,O,000292,_ZN4ksys4phys8BoxShape12setTranslateERKN4sead7Vector3IfEE
+0x0000007100faa76c,O,000020,_ZNK4ksys4phys8BoxShape9getVolumeEv
+0x0000007100faa780,O,000028,_ZN4ksys4phys8BoxShape13getHavokShapeEv
+0x0000007100faa79c,O,000028,_ZNK4ksys4phys8BoxShape13getHavokShapeEv
+0x0000007100faa7b8,O,000644,_ZN4ksys4phys8BoxShape16updateHavokShapeEv
+0x0000007100faaa3c,O,000128,_ZN4ksys4phys8BoxShape8setScaleEf
+0x0000007100faaabc,O,000120,_ZNK4ksys4phys8BoxShape12getTranslateEPN4sead7Vector3IfEERK12hkTransformf
+0x0000007100faab34,O,000204,_ZNK4ksys4phys8BoxShape27checkDerivedRuntimeTypeInfoEPKN4sead15RuntimeTypeInfo9InterfaceE
+0x0000007100faac00,O,000092,_ZNK4ksys4phys8BoxShape18getRuntimeTypeInfoEv
+0x0000007100faac5c,O,000008,_ZNK4ksys4phys8BoxShape7getTypeEv
 0x0000007100faac64,U,000260,
 0x0000007100faad68,U,000432,
 0x0000007100faaf18,U,000032,

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

@@ -4,7 +4,7 @@
 
 namespace ksys::phys {
 
-struct BoxShape;
+class BoxShape;
 struct RigidBodyInstanceParam;
 
 class BoxRigidBody : public RigidBodyFromShape {

src/KingSystem/Physics/RigidBody/Shape/physBoxShape.cpp

@@ -0,0 +1,165 @@
+#include "KingSystem/Physics/RigidBody/Shape/physBoxShape.h"
+#include <Havok/Physics2012/Collide/Shape/Convex/Box/hkpBoxShape.h>
+#include <Havok/Physics2012/Collide/Shape/Convex/ConvexTransform/hkpConvexTransformShape.h>
+#include "KingSystem/Physics/physConversions.h"
+#include "KingSystem/Utils/HeapUtil.h"
+#include "KingSystem/Utils/SafeDelete.h"
+
+namespace ksys::phys {
+
+BoxShape* BoxShapeParam::createShape(sead::Heap* heap) {
+    hkpBoxShape* box = nullptr;
+    if (auto* storage = util::allocStorage<hkpBoxShape>(heap)) {
+        const auto radius = convex_radius;
+        const hkVector4f half_extents{sead::Mathf::max(extents.x / 2 - radius, 0.001),
+                                      sead::Mathf::max(extents.y / 2 - radius, 0.001),
+                                      sead::Mathf::max(extents.z / 2 - radius, 0.001)};
+        box = new (storage) hkpBoxShape(half_extents, radius);
+    }
+
+    hkpConvexTransformShape* transform_shape = nullptr;
+    if (auto* storage = util::allocStorage<hkpConvexTransformShape>(heap)) {
+        sead::Quatf rotation;
+        rotation.setRPY(rotate.x, rotate.y, rotate.z);
+        hkQsTransformf transform{toHkVec4(translate), toHkQuat(rotation)};
+        transform_shape = new (storage) hkpConvexTransformShape(box, transform);
+    }
+
+    if (!box || !transform_shape) {
+        if (box)
+            delete reinterpret_cast<u8*>(box);
+        if (transform_shape)
+            delete reinterpret_cast<u8*>(transform_shape);
+        return nullptr;
+    }
+
+    return new (heap) BoxShape(*this, box, transform_shape);
+}
+
+BoxShape::BoxShape(const BoxShapeParam& param, hkpBoxShape* shape,
+                   hkpConvexTransformShape* transform_shape)
+    : mExtents(param.extents), mTranslate(param.translate), mRotate(param.rotate),
+      mHavokShape(shape), mMaterialMask(param.common.getMaterialMask()),
+      mTransformShape(transform_shape) {
+    const bool no_transform =
+        mTranslate == sead::Vector3f(0, 0, 0) && mRotate == sead::Vector3f(0, 0, 0);
+    mFlags.change(Flag::HasTransform, !no_transform);
+
+    if (param.common.item_code_disable_stick)
+        mMaterialMask.getData().setCustomFlag(MaterialMaskData::CustomFlag::_0);
+
+    setMaterialMask(mMaterialMask);
+}
+
+BoxShape::~BoxShape() {
+    util::deallocateObjectUnsafe(mHavokShape);
+    util::deallocateObjectUnsafe(mTransformShape);
+}
+
+bool BoxShape::setExtents(const sead::Vector3f& extents) {
+    if (mExtents == extents)
+        return false;
+
+    mExtents = extents;
+    mFlags.set(Flag::Dirty);
+    return true;
+}
+
+bool BoxShape::setTranslate(const sead::Vector3f& translate) {
+    if (mTranslate == translate)
+        return false;
+
+    mTranslate = translate;
+
+    const bool had_transform = mFlags.isOn(Flag::HasTransform);
+    const bool no_transform =
+        mTranslate == sead::Vector3f(0, 0, 0) && mRotate == sead::Vector3f(0, 0, 0);
+
+    mFlags.change(Flag::HasTransform, !no_transform);
+    mFlags.change(Flag::DirtyTransform, had_transform != !no_transform);
+    mFlags.set(Flag::Dirty);
+
+    return true;
+}
+
+void BoxShape::setMaterialMask(const MaterialMask& mask) {
+    mMaterialMask = mask;
+
+    if (mHavokShape)
+        mHavokShape->setUserData(mask.getRawData());
+
+    if (mTransformShape)
+        mTransformShape->setUserData(mask.getRawData());
+}
+
+float BoxShape::getVolume() const {
+    return mExtents.x * mExtents.y * mExtents.z;
+}
+
+hkpShape* BoxShape::getHavokShape() {
+    if (mFlags.isOn(Flag::HasTransform))
+        return mTransformShape;
+    return mHavokShape;
+}
+
+const hkpShape* BoxShape::getHavokShape() const {
+    if (mFlags.isOn(Flag::HasTransform))
+        return mTransformShape;
+    return mHavokShape;
+}
+
+const hkpShape* BoxShape::updateHavokShape() {
+    if (mFlags.isOn(Flag::Dirty)) {
+        {
+            const auto radius = mHavokShape->getRadius();
+            const sead::Vector3f half_extents{sead::Mathf::max(mExtents.x / 2 - radius, 0.001),
+                                              sead::Mathf::max(mExtents.y / 2 - radius, 0.001),
+                                              sead::Mathf::max(mExtents.z / 2 - radius, 0.001)};
+            const auto ref_count = mHavokShape->getReferenceCount();
+            mHavokShape = new (mHavokShape) hkpBoxShape(toHkVec4(half_extents), radius);
+            mHavokShape->setReferenceCount(ref_count);
+        }
+        {
+            sead::Quatf rotation;
+            rotation.setRPY(mRotate.x, mRotate.y, mRotate.z);
+            hkQsTransformf transform{toHkVec4(mTranslate), toHkQuat(rotation)};
+            const auto ref_count = mTransformShape->getReferenceCount();
+            mTransformShape = new (mTransformShape) hkpConvexTransformShape(mHavokShape, transform);
+            mTransformShape->setReferenceCount(ref_count);
+        }
+
+        setMaterialMask(mMaterialMask);
+
+        mFlags.reset(Flag::Dirty);
+    }
+
+    if (mFlags.isOn(Flag::DirtyTransform)) {
+        mFlags.reset(Flag::DirtyTransform);
+        return std::as_const(*this).getHavokShape();
+    }
+
+    return nullptr;
+}
+
+void BoxShape::setScale(float scale) {
+    const auto scaled_extents = mExtents * scale;
+    if (mExtents != scaled_extents) {
+        mExtents = scaled_extents;
+        mFlags.set(Flag::Dirty);
+    }
+    setTranslate(mTranslate * scale);
+}
+
+void BoxShape::getTranslate(sead::Vector3f* out, const hkTransformf& transform) const {
+    hkVector4f translate;
+    if (mFlags.isOn(Flag::HasTransform)) {
+        hkVector4f transformed;
+        transformed.setTransformedPos(transform, toHkVec4(mTranslate));
+        translate = transformed;
+    } else {
+        translate = transform.getTranslation();
+    }
+    storeToVec3(out, translate);
+}
+
+}  // namespace ksys::phys

src/KingSystem/Physics/RigidBody/Shape/physBoxShape.h

@@ -1,26 +1,70 @@
 #pragma once
 
 #include <math/seadVector.h>
+#include <prim/seadTypedBitFlag.h>
+#include <thread/seadAtomic.h>
 #include "KingSystem/Physics/RigidBody/Shape/physShape.h"
 #include "KingSystem/Physics/RigidBody/physRigidBody.h"
 #include "KingSystem/Physics/RigidBody/physRigidBodyParam.h"
+#include "KingSystem/Physics/System/physMaterialMask.h"
+
+class hkTransformf;
+class hkpBoxShape;
+class hkpConvexTransformShape;
 
 namespace ksys::phys {
 
 class BoxParam;
 class BoxRigidBody;
+struct BoxShapeParam;
 
-struct BoxShape : Shape {
+class BoxShape : public Shape {
+    SEAD_RTTI_OVERRIDE(BoxShape, Shape)
+public:
+    enum class Flag {
+        /// Whether there is a pending change.
+        Dirty = 1 << 0,
+        /// Whether we have a transform (translation and/or rotation).
+        HasTransform = 1 << 1,
+        /// Whether there is a pending transform change.
+        DirtyTransform = 1 << 2,
+    };
+
+    BoxShape(const BoxShapeParam& param, hkpBoxShape* shape,
+             hkpConvexTransformShape* transform_shape);
     ~BoxShape() override;
 
     BoxRigidBody* createBody(bool flag, const RigidBodyInstanceParam& params, sead::Heap* heap);
+
+    bool setExtents(const sead::Vector3f& extents);
+    bool setTranslate(const sead::Vector3f& translate);
+    void setMaterialMask(const MaterialMask& mask);
+
+    ShapeType getType() const override { return ShapeType::Box; }
+    float getVolume() const override;
+    hkpShape* getHavokShape() override;
+    const hkpShape* getHavokShape() const override;
+    const hkpShape* updateHavokShape() override;
+    void setScale(float scale) override;
+
+    /// Get the transform's translation or mTranslate transformed by the matrix
+    /// (if we have a non-zero translation vector).
+    void getTranslate(sead::Vector3f* out, const hkTransformf& transform) const;
+
+    sead::Vector3f mExtents;
+    sead::TypedBitFlag<Flag, sead::Atomic<u32>> mFlags;
+    sead::Vector3f mTranslate;
+    sead::Vector3f mRotate;
+    hkpBoxShape* mHavokShape{};
+    MaterialMask mMaterialMask{};
+    hkpConvexTransformShape* mTransformShape{};
 };
 
 struct BoxShapeParam {
     BoxShape* createShape(sead::Heap* heap);
 
-    sead::Vector3f translate_1;
-    sead::Vector3f translate_0;
+    sead::Vector3f extents;
+    sead::Vector3f translate;
     sead::Vector3f rotate;
     float convex_radius;
     CommonShapeParam common;

src/KingSystem/Physics/RigidBody/Shape/physCapsuleShape.cpp

@@ -8,9 +8,7 @@ namespace ksys::phys {
 
 CapsuleShape::CapsuleShape(const CapsuleShapeParam& shape_, hkpShape* hkp_shape_)
     : vertex_a(shape_.vertex_a), vertex_b(shape_.vertex_b), radius(shape_.radius),
-      material_mask(shape_.common.material, shape_.common.sub_material, shape_.common.floor_code,
-                    shape_.common.wall_code),
-      shape(hkp_shape_) {
+      material_mask(shape_.common.getMaterialMask()), shape(hkp_shape_) {
     if (shape_.common.item_code_disable_stick)
         material_mask.getData().setCustomFlag(MaterialMaskData::CustomFlag::_0);
     setMaterialMask(material_mask);

src/KingSystem/Physics/RigidBody/Shape/physShape.h

@@ -3,6 +3,7 @@
 #include <prim/seadRuntimeTypeInfo.h>
 #include <prim/seadSafeString.h>
 #include "KingSystem/Physics/System/physDefines.h"
+#include "KingSystem/Physics/System/physMaterialMask.h"
 
 class hkpShape;
 
@@ -28,12 +29,16 @@ public:
     virtual ~Shape() = default;
     virtual hkpShape* getHavokShape() = 0;
     virtual const hkpShape* getHavokShape() const = 0;
-    virtual hkpShape* updateHavokShape() = 0;
+    virtual const hkpShape* updateHavokShape() = 0;
     /// @param scale New scale (relative to the current scale)
     virtual void setScale(float scale) = 0;
 };
 
 struct CommonShapeParam {
+    MaterialMask getMaterialMask(bool flag = false) const {
+        return {material, sub_material, floor_code, wall_code, flag};
+    }
+
     Material material;
     const char* sub_material = sead::SafeString::cEmptyString.cstr();
     FloorCode floor_code;

src/KingSystem/Physics/RigidBody/Shape/physShapeParamObj.cpp

@@ -90,8 +90,8 @@ void ShapeParamObj::getCylinder(CylinderShapeParam* param) const {
 }
 
 void ShapeParamObj::getBox(BoxShapeParam* param) const {
-    param->translate_0 = *translate_0;
-    param->translate_1 = *translate_1;
+    param->translate = *translate_0;
+    param->extents = *translate_1;
     param->rotate = *rotate;
     param->convex_radius = *convex_radius;
     getCommon(&param->common);

src/KingSystem/Utils/HeapUtil.h

@@ -124,9 +124,15 @@ KSYS_ALWAYS_INLINE inline sead::Heap* tryCreateDualHeap(sead::Heap* parent) {
                              sead::Heap::cHeapDirection_Forward, false);
 }
 
+/// Allocate uninitialised, suitably aligned storage for an object of type T.
+template <typename T>
+inline void* allocStorage(sead::Heap* heap) {
+    return heap->alloc(sizeof(T), alignof(T));
+}
+
 template <typename T, typename... Args>
 inline T* alloc(sead::Heap* heap, Args&&... args) {
-    void* storage = heap->alloc(sizeof(T), alignof(T));
+    void* storage = allocStorage<T>(heap);
     return new (storage) T(std::forward<Args>(args)...);
 }
 

src/KingSystem/Utils/SafeDelete.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include <type_traits>
+
 namespace ksys::util {
 
 template <typename T>
@@ -33,4 +35,24 @@ inline void safeDeleteHeap(T*& heap) {
     }
 }
 
+/// @warning This does not call T's destructor.
+///          This should only be used if T is trivially destructible (or if you know T's destructor
+///          can be skipped).
+template <typename T>
+inline void deallocateObjectUnsafe(T*& pointer) {
+    if (pointer) {
+        operator delete(pointer);
+        pointer = nullptr;
+    }
+}
+
+/// @warning This does not call T's destructor.
+template <typename T>
+inline void deallocateObject(T*& pointer) {
+    static_assert(std::is_trivially_destructible_v<T>,
+                  "T must be trivially destructible: use safeDelete "
+                  "(or use deallocateObjectUnsafe if you know T's destructor can be skipped)");
+    deallocateObjectUnsafe(pointer);
+}
+
 }  // namespace ksys::util