ksys/phys: Implement `SphereShape` and `SphereRigidBody` (#92)

4 files changed, 283 insertions, 4 deletions

diff --git a/src/KingSystem/Physics/RigidBody/Shape/Sphere/physSphereRigidBody.cpp b/src/KingSystem/Physics/RigidBody/Shape/Sphere/physSphereRigidBody.cpp
index 816b793a..2ed40ff9 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/Sphere/physSphereRigidBody.cpp
+++ b/src/KingSystem/Physics/RigidBody/Shape/Sphere/physSphereRigidBody.cpp
@@ -1 +1,76 @@
 #include "KingSystem/Physics/RigidBody/Shape/Sphere/physSphereRigidBody.h"
+#include <Havok/Physics2012/Dynamics/Entity/hkpRigidBody.h>
+#include "KingSystem/Physics/RigidBody/Shape/Sphere/physSphereShape.h"
+
+namespace ksys::phys {
+
+SphereRigidBody* SphereRigidBody::make(RigidBodyInstanceParam* param, sead::Heap* heap) {
+    return createSphere(param, heap);
+}
+
+SphereRigidBody::SphereRigidBody(hkpRigidBody* hk_body, SphereShape* shape,
+                                 ContactLayerType layer_type, const sead::SafeString& name,
+                                 bool set_flag_10, sead::Heap* heap)
+    : RigidBodyFromShape(hk_body, layer_type, name, set_flag_10, heap), mShape(shape) {}
+
+SphereRigidBody::~SphereRigidBody() {
+    if (hasFlag(RigidBody::Flag::_10) && mShape) {
+        delete mShape;
+        mShape = nullptr;
+    }
+}
+
+void SphereRigidBody::setRadius(float radius) {
+    if (mShape->setRadius(radius))
+        updateShape();
+}
+
+void SphereRigidBody::setTranslate(const sead::Vector3f& translate) {
+    if (mShape->setTranslate(translate))
+        updateShape();
+}
+
+float SphereRigidBody::getRadius() const {
+    return mShape->mRadius;
+}
+
+const sead::Vector3f& SphereRigidBody::getTranslate() const {
+    return mShape->mTranslate;
+}
+
+void SphereRigidBody::getTransformedTranslate(sead::Vector3f* translate) {
+    lock();
+    const auto& transform = getHkBody()->getMotion()->getMotionState()->getTransform();
+    unlock();
+    mShape->getTranslate(translate, transform);
+}
+
+void SphereRigidBody::setMaterialMask(const MaterialMask& mask) {
+    mShape->setMaterialMask(mask);
+}
+
+const MaterialMask& SphereRigidBody::getMaterialMask() const {
+    return mShape->mMaterialMask;
+}
+
+float SphereRigidBody::getVolume() {
+    return mShape->getVolume();
+}
+
+Shape* SphereRigidBody::getShape_() {
+    return mShape;
+}
+
+const Shape* SphereRigidBody::getShape_() const {
+    return mShape;
+}
+
+u32 SphereRigidBody::getCollisionMasks(RigidBody::CollisionMasks* masks, const u32* unk,
+                                       const sead::Vector3f& contact_point) {
+    masks->ignored_layers = ~mContactMask.getDirect();
+    masks->collision_filter_info = getCollisionFilterInfo();
+    masks->material_mask = getMaterialMask().getRawData();
+    return 0;
+}
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/RigidBody/Shape/Sphere/physSphereRigidBody.h b/src/KingSystem/Physics/RigidBody/Shape/Sphere/physSphereRigidBody.h
index dfc29094..20896b9a 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/Sphere/physSphereRigidBody.h
+++ b/src/KingSystem/Physics/RigidBody/Shape/Sphere/physSphereRigidBody.h
@@ -15,12 +15,20 @@ public:
                     const sead::SafeString& name, bool set_flag_10, sead::Heap* heap);
     ~SphereRigidBody() override;
 
+    /// Set the sphere radius and trigger a shape update.
+    void setRadius(float radius);
+    /// Set the sphere translation and trigger a shape update.
+    void setTranslate(const sead::Vector3f& translate);
+
+    float getRadius() const;
+    const sead::Vector3f& getTranslate() const;
+    void getTransformedTranslate(sead::Vector3f* translate);
+
+    void setMaterialMask(const MaterialMask& mask);
     const MaterialMask& getMaterialMask() const;
 
     float getVolume() override;
 
-    void setRadius(float radius);
-
 protected:
     Shape* getShape_() override;
     const Shape* getShape_() const override;
diff --git a/src/KingSystem/Physics/RigidBody/Shape/Sphere/physSphereShape.cpp b/src/KingSystem/Physics/RigidBody/Shape/Sphere/physSphereShape.cpp
index ff497871..46f211de 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/Sphere/physSphereShape.cpp
+++ b/src/KingSystem/Physics/RigidBody/Shape/Sphere/physSphereShape.cpp
@@ -1 +1,161 @@
 #include "KingSystem/Physics/RigidBody/Shape/Sphere/physSphereShape.h"
+#include "KingSystem/Physics/physConversions.h"
+#include "KingSystem/Utils/HeapUtil.h"
+#include "KingSystem/Utils/SafeDelete.h"
+#include "math/seadMathCalcCommon.h"
+
+#include <Havok/Physics2012/Collide/Shape/Convex/ConvexTranslate/hkpConvexTranslateShape.h>
+#include <Havok/Physics2012/Collide/Shape/Convex/Sphere/hkpSphereShape.h>
+
+namespace ksys::phys {
+
+SphereShape* SphereShape::make(const SphereShapeParam& param, sead::Heap* heap) {
+    hkpSphereShape* sphere = nullptr;
+    if (auto* storage = heap->alloc(sizeof(hkpSphereShape), 0x10)) {
+        sphere = new (storage) hkpSphereShape(param.radius);
+    }
+
+    hkpConvexTranslateShape* translate_shape = nullptr;
+    if (auto* storage = util::allocStorage<hkpConvexTranslateShape>(heap)) {
+        translate_shape = new (storage) hkpConvexTranslateShape(sphere, toHkVec4(param.translate));
+
+        if (sphere != nullptr) {
+            return new (heap) SphereShape(param, sphere, translate_shape);
+        }
+    }
+
+    if (sphere) {
+        delete reinterpret_cast<u8*>(sphere);
+    } else if (translate_shape) {
+        delete reinterpret_cast<u8*>(translate_shape);
+    }
+
+    return nullptr;
+}
+
+SphereShape* SphereShape::clone(sead::Heap* heap) const {
+    SphereShapeParam param;
+
+    param.radius = mRadius;
+    param.translate = mTranslate;
+
+    auto* cloned = make(param, heap);
+    cloned->setMaterialMask(mMaterialMask);
+    return cloned;
+}
+
+SphereShape::SphereShape(const SphereShapeParam& param, hkpSphereShape* shape,
+                         hkpConvexTranslateShape* translate_shape)
+    : mRadius(param.radius), mHavokShape(shape), mTranslate(param.translate),
+      mMaterialMask(param.common.getMaterialMask()), mTranslateShape(translate_shape) {
+    const bool no_transform = mTranslate == 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);
+}
+
+SphereShape::~SphereShape() {
+    util::deallocateObjectUnsafe(mHavokShape);
+    util::deallocateObjectUnsafe(mTranslateShape);
+}
+
+void SphereShape::setMaterialMask(const MaterialMask& mask) {
+    mMaterialMask = mask;
+
+    if (mHavokShape)
+        mHavokShape->setUserData(mask.getRawData());
+
+    if (mTranslateShape)
+        mTranslateShape->setUserData(mask.getRawData());
+}
+
+float SphereShape::getVolume() const {
+    return 4 / 3 * sead::Mathf::pi() * mRadius * mRadius * mRadius;
+}
+
+hkpShape* SphereShape::getHavokShape() {
+    if (mFlags.isOn(Flag::HasTransform))
+        return mTranslateShape;
+
+    return mHavokShape;
+};
+
+const hkpShape* SphereShape::getHavokShape() const {
+    if (mFlags.isOn(Flag::HasTransform))
+        return mTranslateShape;
+    return mHavokShape;
+};
+
+const hkpShape* SphereShape::updateHavokShape() {
+    if (mFlags.isOn(Flag::Dirty)) {
+        {
+            const auto ref_count = mHavokShape->getReferenceCount();
+            mHavokShape = new (mHavokShape) hkpSphereShape(mRadius);
+            mHavokShape->setReferenceCount(ref_count);
+        }
+        {
+            const auto ref_count = mTranslateShape->getReferenceCount();
+            mTranslateShape =
+                new (mTranslateShape) hkpConvexTranslateShape(mHavokShape, toHkVec4(mTranslate));
+            mTranslateShape->setReferenceCount(ref_count);
+        }
+
+        setMaterialMask(mMaterialMask);
+        mFlags.reset(Flag::Dirty);
+    }
+
+    if (mFlags.isOn(Flag::DirtyTransform)) {
+        mFlags.reset(Flag::DirtyTransform);
+        return getHavokShapeConst();
+    }
+
+    return nullptr;
+};
+
+bool SphereShape::setRadius(float radius) {
+    if (radius == mRadius || radius <= 0.0f) {
+        return false;
+    }
+
+    mRadius = radius;
+    mFlags.set(Flag::Dirty);
+    return true;
+}
+
+void SphereShape::setScale(float scale) {
+    setRadius(mRadius * scale);
+    setTranslate(mTranslate * scale);
+}
+
+bool SphereShape::setTranslate(const sead::Vector3f& translate) {
+    if (mTranslate == translate)
+        return false;
+
+    mTranslate = translate;
+
+    const bool had_transform = mFlags.isOn(Flag::HasTransform);
+    const bool no_transform = translate == 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 SphereShape::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
\ No newline at end of file
diff --git a/src/KingSystem/Physics/RigidBody/Shape/Sphere/physSphereShape.h b/src/KingSystem/Physics/RigidBody/Shape/Sphere/physSphereShape.h
index f9453d67..2d3a2182 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/Sphere/physSphereShape.h
+++ b/src/KingSystem/Physics/RigidBody/Shape/Sphere/physSphereShape.h
@@ -1,10 +1,16 @@
 #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"
 
+class hkpSphereShape;
+class hkpConvexTranslateShape;
+class hkTransformf;
+
 namespace ksys::phys {
 
 class SphereParam;
@@ -18,15 +24,45 @@ struct SphereShapeParam {
 class SphereShape : public Shape {
     SEAD_RTTI_OVERRIDE(SphereShape, 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,
+    };
+
     static SphereShape* make(const SphereShapeParam& param, sead::Heap* heap);
+
+    SphereShape(const SphereShapeParam& param, hkpSphereShape* shape,
+                hkpConvexTranslateShape* translate_shape);
+    ~SphereShape() override;
+
     SphereShape* clone(sead::Heap* heap) const;
 
+    bool setTranslate(const sead::Vector3f& translate);
+    bool setRadius(float radius);
     void setMaterialMask(const MaterialMask& mask);
     const MaterialMask& getMaterialMask() const { return mMaterialMask; }
 
-private:
-    char _8[0x28 - 0x8];
+    ShapeType getType() const override { return ShapeType::Sphere; }
+    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;
+
+    float mRadius;
+    hkpSphereShape* mHavokShape{};
+    sead::TypedBitFlag<Flag, sead::Atomic<u32>> mFlags;
+    sead::Vector3f mTranslate;
     MaterialMask mMaterialMask;
+    hkpConvexTranslateShape* mTranslateShape{};
 };
 
 class SphereParam : public RigidBodyInstanceParam, public SphereShapeParam {