ksys/phys: Implement CylinderWaterShape

9 files changed, 188 insertions, 22 deletions

diff --git a/src/KingSystem/Physics/CMakeLists.txt b/src/KingSystem/Physics/CMakeLists.txt
index 47377a3d..ed4321a6 100644
--- a/src/KingSystem/Physics/CMakeLists.txt
+++ b/src/KingSystem/Physics/CMakeLists.txt
@@ -58,6 +58,8 @@ target_sources(uking PRIVATE
   RigidBody/Shape/Cylinder/physCylinderRigidBody.h
   RigidBody/Shape/Cylinder/physCylinderShape.cpp
   RigidBody/Shape/Cylinder/physCylinderShape.h
+  RigidBody/Shape/CylinderWater/physCylinderWaterRigidBody.cpp
+  RigidBody/Shape/CylinderWater/physCylinderWaterRigidBody.h
   RigidBody/Shape/CylinderWater/physCylinderWaterShape.cpp
   RigidBody/Shape/CylinderWater/physCylinderWaterShape.h
   RigidBody/Shape/Polytope/physPolytopeShape.cpp
diff --git a/src/KingSystem/Physics/RigidBody/Shape/Cylinder/physCylinderShape.h b/src/KingSystem/Physics/RigidBody/Shape/Cylinder/physCylinderShape.h
index 35c08290..9eed38b1 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/Cylinder/physCylinderShape.h
+++ b/src/KingSystem/Physics/RigidBody/Shape/Cylinder/physCylinderShape.h
@@ -58,6 +58,10 @@ private:
 };
 
 struct CylinderShapeParam {
+    CylinderShapeParam() = default;
+    CylinderShapeParam(const sead::Vector3f& va, const sead::Vector3f& vb)
+        : vertex_a(va), vertex_b(vb) {}
+
     /// The center of the first circular base.
     sead::Vector3f vertex_a;
     /// The radius of the circular bases.
diff --git a/src/KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterRigidBody.cpp b/src/KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterRigidBody.cpp
new file mode 100644
index 00000000..48f26c95
--- /dev/null
+++ b/src/KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterRigidBody.cpp
@@ -0,0 +1 @@
+#include "KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterRigidBody.h"
diff --git a/src/KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterRigidBody.h b/src/KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterRigidBody.h
new file mode 100644
index 00000000..fc638816
--- /dev/null
+++ b/src/KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterRigidBody.h
@@ -0,0 +1,12 @@
+#pragma once
+
+#include "KingSystem/Physics/RigidBody/physRigidBodyFromShape.h"
+
+namespace ksys::phys {
+
+// TODO
+class CylinderWaterRigidBody : public RigidBodyFromShape {
+    SEAD_RTTI_OVERRIDE(CylinderWaterRigidBody, RigidBodyFromShape)
+};
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterShape.cpp b/src/KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterShape.cpp
index e69de29b..d4d452da 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterShape.cpp
+++ b/src/KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterShape.cpp
@@ -0,0 +1,127 @@
+#include "KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterShape.h"
+#include <Havok/Physics2012/Collide/Shape/HeightField/hkpHeightFieldShape.h>
+#include "KingSystem/Utils/HeapUtil.h"
+#include "KingSystem/Utils/SafeDelete.h"
+
+namespace ksys::phys {
+
+class alignas(16) HavokCylinderWaterShape : public hkpHeightFieldShape {
+public:
+    HK_DECLARE_CLASS_ALLOCATOR(HavokCylinderWaterShape)
+
+    HavokCylinderWaterShape() : hkpHeightFieldShape(hkcdShapeType::HEIGHT_FIELD) {}
+
+    void setRadius(float radius) { m_radius = radius; }
+
+    void getAabb(const hkTransform& localToWorld, hkReal tolerance, hkAabb& aabbOut) const override;
+
+    hkBool castRay(const hkpShapeRayCastInput& input, hkpShapeRayCastOutput& output) const override;
+
+    void castRayWithCollector(const hkpShapeRayCastInput& input, const hkpCdBody& cdBody,
+                              hkpRayHitCollector& collector) const override;
+
+    void collideSpheres(const CollideSpheresInput& input,
+                        SphereCollisionOutput* outputArray) const override;
+
+    void castSphere(const hkpSphereCastInput& input, const hkpCdBody& cdBody,
+                    hkpRayHitCollector& collector) const override {}
+
+    float m_height = 1.0;
+    float m_radius = 1.0;
+};
+
+CylinderWaterShape* CylinderWaterShape::make(const CylinderShapeParam& param, sead::Heap* heap) {
+    void* storage = util::allocStorage<HavokCylinderWaterShape>(heap);
+    if (!storage)
+        return nullptr;
+
+    auto* hk_shape = new (storage) HavokCylinderWaterShape;
+    return new (heap) CylinderWaterShape(param, hk_shape);
+}
+
+CylinderWaterShape::CylinderWaterShape(const CylinderShapeParam& param,
+                                       HavokCylinderWaterShape* shape)
+    : mMaterialMask(param.common.getMaterialMask()), mHavokShape(shape) {
+    if (param.common.item_code_disable_stick)
+        mMaterialMask.getData().setCustomFlag(MaterialMaskData::CustomFlag::_0);
+
+    setMaterialMask(mMaterialMask);
+    mHavokShape->setRadius(param.radius);
+    mHavokShape->m_height = param.vertex_a.x;
+}
+
+CylinderWaterShape::~CylinderWaterShape() {
+    util::deallocateObjectUnsafe(mHavokShape);
+}
+
+void CylinderWaterShape::setMaterialMask(const MaterialMask& mask) {
+    mMaterialMask = mask;
+    if (mHavokShape)
+        mHavokShape->setUserData(mask.getRawData());
+}
+
+bool CylinderWaterShape::setRadius(float radius) {
+    if (!mHavokShape)
+        return false;
+
+    if (mHavokShape->m_radius == radius)
+        return false;
+
+    mHavokShape->m_radius = radius;
+    mFlags.set(Flag::Dirty);
+    return true;
+}
+
+bool CylinderWaterShape::setHeight(float height) {
+    if (mHavokShape->m_height == height)
+        return false;
+
+    mHavokShape->m_height = height;
+    mFlags.set(Flag::Dirty);
+    return true;
+}
+
+// NON_MATCHING: useless store to param.vertex_a.x
+CylinderWaterShape* CylinderWaterShape::clone(sead::Heap* heap) const {
+    CylinderShapeParam param(-sead::Vector3f::ey, sead::Vector3f::ey);
+    param.radius = getRadius();
+    param.vertex_a.x = getHeight();
+    auto* cloned = make(param, heap);
+    cloned->setMaterialMask(mMaterialMask);
+    return cloned;
+}
+
+float CylinderWaterShape::getRadius() const {
+    return mHavokShape->m_radius;
+}
+
+float CylinderWaterShape::getHeight() const {
+    return mHavokShape->m_height;
+}
+
+float CylinderWaterShape::getVolume() const {
+    return getHeight() * (sead::Mathf::piHalf() * getRadius() * getRadius());
+}
+
+hkpShape* CylinderWaterShape::getHavokShape() {
+    return mHavokShape;
+}
+
+const hkpShape* CylinderWaterShape::getHavokShape() const {
+    return mHavokShape;
+}
+
+const hkpShape* CylinderWaterShape::updateHavokShape() {
+    if (mFlags.isOn(Flag::Dirty)) {
+        // Nothing to do.
+        mFlags.reset(Flag::Dirty);
+    }
+    return nullptr;
+}
+
+void CylinderWaterShape::setScale(float scale) {
+    setRadius(getRadius() * scale);
+    setHeight(getHeight() * scale);
+}
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterShape.h b/src/KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterShape.h
index e9d26b13..93b06b1a 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterShape.h
+++ b/src/KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterShape.h
@@ -1,28 +1,48 @@
 #pragma once
 
+#include <prim/seadTypedBitFlag.h>
+#include <thread/seadAtomic.h>
+#include "KingSystem/Physics/RigidBody/Shape/Cylinder/physCylinderShape.h"
 #include "KingSystem/Physics/RigidBody/physRigidBody.h"
 #include "KingSystem/Physics/RigidBody/physRigidBodyParam.h"
+#include "KingSystem/Physics/System/physMaterialMask.h"
 
 namespace ksys::phys {
 
-class CylinderWaterParam;
+class HavokCylinderWaterShape;
 
-struct CylinderWaterShape {
-    virtual ~CylinderWaterShape();
+class CylinderWaterShape : public Shape {
+    SEAD_RTTI_OVERRIDE(CylinderWaterShape, Shape)
+public:
+    enum class Flag {
+        Dirty = 1 << 0,
+    };
 
-    RigidBody* createBody(bool flag, const RigidBodyInstanceParam& params, sead::Heap* heap);
-};
+    static CylinderWaterShape* make(const CylinderShapeParam& param, sead::Heap* heap);
 
-struct WaterCylinderShapeParam {
-    CylinderWaterShape* createShape(sead::Heap* heap);
-};
+    CylinderWaterShape(const CylinderShapeParam& param, HavokCylinderWaterShape* shape);
+    ~CylinderWaterShape() override;
 
-class CylinderWaterParam : public RigidBodyInstanceParam {
-    SEAD_RTTI_OVERRIDE(CylinderWaterParam, RigidBodyInstanceParam)
-public:
-    u8 _90;
-    float _94;
-    WaterCylinderShapeParam shape;
+    void setMaterialMask(const MaterialMask& mask);
+    bool setRadius(float radius);
+    bool setHeight(float height);
+
+    CylinderWaterShape* clone(sead::Heap* heap) const;
+
+    float getRadius() const;
+    float getHeight() const;
+
+    float getVolume() const override;
+    hkpShape* getHavokShape() override;
+    const hkpShape* getHavokShape() const override;
+    const hkpShape* updateHavokShape() override;
+    void setScale(float scale) override;
+    ShapeType getType() const override { return ShapeType::CylinderWater; }
+
+private:
+    MaterialMask mMaterialMask;
+    sead::TypedBitFlag<Flag, sead::Atomic<u32>> mFlags;
+    HavokCylinderWaterShape* mHavokShape;
 };
 
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/RigidBody/Shape/physShape.h b/src/KingSystem/Physics/RigidBody/Shape/physShape.h
index 0cd660f5..9f0f91e6 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/physShape.h
+++ b/src/KingSystem/Physics/RigidBody/Shape/physShape.h
@@ -17,6 +17,7 @@ enum class ShapeType {
     Polytope = 4,
     CharacterPrism = 6,
     BoxWater = 7,
+    CylinderWater = 8,
     Unknown = -1,
 };
 
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyFactory.cpp b/src/KingSystem/Physics/RigidBody/physRigidBodyFactory.cpp
index e7c75c75..8bd1d621 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodyFactory.cpp
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyFactory.cpp
@@ -5,6 +5,7 @@
 #include "KingSystem/Physics/RigidBody/Shape/Capsule/physCapsuleShape.h"
 #include "KingSystem/Physics/RigidBody/Shape/Cylinder/physCylinderRigidBody.h"
 #include "KingSystem/Physics/RigidBody/Shape/Cylinder/physCylinderShape.h"
+#include "KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterRigidBody.h"
 #include "KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterShape.h"
 #include "KingSystem/Physics/RigidBody/Shape/Sphere/physSphereShape.h"
 #include "KingSystem/Physics/RigidBody/physRigidBodyFromShape.h"
@@ -40,13 +41,9 @@ CylinderRigidBody* RigidBodyFactory::createCylinder(RigidBodyInstanceParam* para
     return createRigidBody<CylinderRigidBody, CylinderShape, CylinderParam>(params, heap);
 }
 
-RigidBody* RigidBodyFactory::createCylinderWater(RigidBodyInstanceParam* params, sead::Heap* heap) {
-    if (params->isDynamicSensor())
-        params->motion_type = MotionType::Keyframed;
-
-    auto* v = sead::DynamicCast<CylinderWaterParam>(params);
-    auto* shape = v->shape.createShape(heap);
-    return shape->createBody(true, *params, heap);
+CylinderWaterRigidBody* RigidBodyFactory::createCylinderWater(RigidBodyInstanceParam* params,
+                                                              sead::Heap* heap) {
+    return createRigidBody<CylinderWaterRigidBody, CylinderWaterShape, CylinderParam>(params, heap);
 }
 
 BoxRigidBody* RigidBodyFactory::createBox(RigidBodyInstanceParam* params, sead::Heap* heap) {
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyFactory.h b/src/KingSystem/Physics/RigidBody/physRigidBodyFactory.h
index e3da1c1e..2e3b2927 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodyFactory.h
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyFactory.h
@@ -12,6 +12,7 @@ class BoxRigidBody;
 class BoxWaterRigidBody;
 class CapsuleRigidBody;
 class CylinderRigidBody;
+class CylinderWaterRigidBody;
 class RigidBody;
 struct RigidBodyInstanceParam;
 
@@ -20,7 +21,8 @@ public:
     static RigidBody* createSphere(RigidBodyInstanceParam* params, sead::Heap* heap);
     static CapsuleRigidBody* createCapsule(RigidBodyInstanceParam* params, sead::Heap* heap);
     static CylinderRigidBody* createCylinder(RigidBodyInstanceParam* params, sead::Heap* heap);
-    static RigidBody* createCylinderWater(RigidBodyInstanceParam* params, sead::Heap* heap);
+    static CylinderWaterRigidBody* createCylinderWater(RigidBodyInstanceParam* params,
+                                                       sead::Heap* heap);
     static BoxRigidBody* createBox(RigidBodyInstanceParam* params, sead::Heap* heap);
     static BoxWaterRigidBody* createBoxWater(RigidBodyInstanceParam* params, sead::Heap* heap);
     static RigidBody* createPolytope(RigidBodyInstanceParam* params, sead::Heap* heap);