ksys/phys: Finish CapsuleShape and add CapsuleRigidBody

src/KingSystem/Physics/CMakeLists.txt

@@ -48,6 +48,8 @@ target_sources(uking PRIVATE
   RigidBody/Shape/physBoxWaterRigidBody.h
   RigidBody/Shape/physBoxWaterShape.cpp
   RigidBody/Shape/physBoxWaterShape.h
+  RigidBody/Shape/physCapsuleRigidBody.cpp
+  RigidBody/Shape/physCapsuleRigidBody.h
   RigidBody/Shape/physCapsuleShape.cpp
   RigidBody/Shape/physCapsuleShape.h
   RigidBody/Shape/physCharacterPrismShape.cpp

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

@@ -0,0 +1,76 @@
+#include "KingSystem/Physics/RigidBody/Shape/physCapsuleRigidBody.h"
+#include <Havok/Physics2012/Dynamics/Entity/hkpRigidBody.h>
+#include "KingSystem/Physics/RigidBody/Shape/physCapsuleShape.h"
+#include "KingSystem/Physics/RigidBody/physRigidBodyFactory.h"
+#include "KingSystem/Utils/SafeDelete.h"
+
+namespace ksys::phys {
+
+CapsuleRigidBody* CapsuleRigidBody::make(RigidBodyInstanceParam* param, sead::Heap* heap) {
+    return RigidBodyFactory::createCapsule(param, heap);
+}
+
+CapsuleRigidBody::CapsuleRigidBody(hkpRigidBody* hk_body, CapsuleShape* 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) {}
+
+CapsuleRigidBody::~CapsuleRigidBody() {
+    if (hasFlag(RigidBody::Flag::_10) && mShape) {
+        util::safeDelete(mShape);
+    }
+}
+
+void CapsuleRigidBody::setRadius(float radius) {
+    if (mShape->setRadius(radius))
+        updateShape();
+}
+
+void CapsuleRigidBody::setVertices(const sead::Vector3f& va, const sead::Vector3f& vb) {
+    if (mShape->setVertices(va, vb))
+        updateShape();
+}
+
+float CapsuleRigidBody::getRadius() const {
+    return mShape->getRadius();
+}
+
+void CapsuleRigidBody::getVertices(sead::Vector3f* va, sead::Vector3f* vb) const {
+    mShape->getVertices(va, vb);
+}
+
+void CapsuleRigidBody::transformVertices(sead::Vector3f* va, sead::Vector3f* vb) {
+    lock();
+    const hkTransform& transform = getHkBody()->getMotion()->getTransform();
+    mShape->transformVertices(va, vb, transform);
+    unlock();
+}
+
+void CapsuleRigidBody::setMaterialMask(const MaterialMask& mask) {
+    mShape->setMaterialMask(mask);
+}
+
+const MaterialMask& CapsuleRigidBody::getMaterialMask() const {
+    return mShape->material_mask;
+}
+
+float CapsuleRigidBody::getVolume() {
+    return mShape->getVolume();
+}
+
+Shape* CapsuleRigidBody::getShape_() {
+    return mShape;
+}
+
+const Shape* CapsuleRigidBody::getShape_() const {
+    return mShape;
+}
+
+u32 CapsuleRigidBody::getCollisionMasks(RigidBody::CollisionMasks* masks) {
+    masks->ignored_layers = ~mContactMask.getDirect();
+    masks->collision_filter_info = getCollisionFilterInfo();
+    masks->material_mask = getMaterialMask().getRawData();
+    return 0;
+}
+
+}  // namespace ksys::phys

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

@@ -0,0 +1,39 @@
+#pragma once
+
+#include <math/seadVector.h>
+#include "KingSystem/Physics/RigidBody/physRigidBodyFromShape.h"
+
+namespace ksys::phys {
+
+struct CapsuleShape;
+
+class CapsuleRigidBody : public RigidBodyFromShape {
+    SEAD_RTTI_OVERRIDE(CapsuleRigidBody, RigidBodyFromShape)
+public:
+    static CapsuleRigidBody* make(RigidBodyInstanceParam* param, sead::Heap* heap);
+
+    CapsuleRigidBody(hkpRigidBody* hk_body, CapsuleShape* shape, ContactLayerType layer_type,
+                     const sead::SafeString& name, bool set_flag_10, sead::Heap* heap);
+    ~CapsuleRigidBody() override;
+
+    void setRadius(float radius);
+    void setVertices(const sead::Vector3f& va, const sead::Vector3f& vb);
+
+    float getRadius() const;
+    void getVertices(sead::Vector3f* va, sead::Vector3f* vb) const;
+    void transformVertices(sead::Vector3f* va, sead::Vector3f* vb);
+
+    void setMaterialMask(const MaterialMask& mask);
+    const MaterialMask& getMaterialMask() const;
+
+    float getVolume() override;
+
+protected:
+    Shape* getShape_() override;
+    const Shape* getShape_() const override;
+    u32 getCollisionMasks(CollisionMasks* masks) override;
+
+    CapsuleShape* mShape{};
+};
+
+}  // namespace ksys::phys

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

@@ -21,27 +21,24 @@ void CapsuleShape::setMaterialMask(const MaterialMask& mask) {
         shape->setUserData(mask.getRawData());
 }
 
-CapsuleShape* CapsuleShapeParam::createShape(sead::Heap* heap) {
+CapsuleShape* CapsuleShape::make(const CapsuleShapeParam& param, sead::Heap* heap) {
     void* ptr = util::allocStorage<hkpCapsuleShape>(heap);
     if (ptr == nullptr)
         return nullptr;
 
     auto* hk_shape =
-        new (ptr) hkpCapsuleShape(hkVector4(vertex_a.x, vertex_a.y, vertex_a.z),
-                                  hkVector4(vertex_b.x, vertex_b.y, vertex_b.z), radius);
-    return new (heap) CapsuleShape(*this, hk_shape);
+        new (ptr) hkpCapsuleShape(toHkVec4(param.vertex_a), toHkVec4(param.vertex_b), param.radius);
+    return new (heap) CapsuleShape(param, hk_shape);
 }
 
-CapsuleShape* CapsuleShape::clone(sead::Heap* heap) {
+CapsuleShape* CapsuleShape::clone(sead::Heap* heap) const {
     CapsuleShapeParam param_clone;
     param_clone.radius = radius;
     param_clone.vertex_a = vertex_a;
     param_clone.vertex_b = vertex_b;
 
-    CapsuleShape* cloned = param_clone.createShape(heap);
-    cloned->material_mask = material_mask;
-    if (cloned->shape != nullptr)
-        cloned->shape->setUserData(material_mask.getRawData());
+    CapsuleShape* cloned = make(param_clone, heap);
+    cloned->setMaterialMask(material_mask);
     return cloned;
 }
 
@@ -95,8 +92,28 @@ const hkpShape* CapsuleShape::getHavokShape() const {
     return shape;
 }
 
-void CapsuleShape::sub_7100FABE80(sead::Vector3f* veca, sead::Vector3f* vecb,
-                                  const hkTransformf& rb_vec) {
+hkpShape* CapsuleShape::updateHavokShape() {
+    if (!flags.isOn(Flag::Modified))
+        return nullptr;
+
+    const auto ref_count = shape->getReferenceCount();
+    shape = new (shape) hkpCapsuleShape(toHkVec4(vertex_a), toHkVec4(vertex_b), radius);
+    shape->setReferenceCount(ref_count);
+
+    setMaterialMask(material_mask);
+
+    flags.reset(Flag::Modified);
+    return nullptr;
+}
+
+// NON_MATCHING: float regalloc
+void CapsuleShape::setScale(float scale) {
+    setRadius(radius * scale);
+    setVertices(vertex_a * scale, vertex_b * scale);
+}
+
+void CapsuleShape::transformVertices(sead::Vector3f* veca, sead::Vector3f* vecb,
+                                     const hkTransformf& rb_vec) {
     if (veca != nullptr) {
         hkVector4 tmp;
         tmp.setTransformedPos(rb_vec, toHkVec4(vertex_a));

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

@@ -18,8 +18,6 @@ class CapsuleParam;
 struct CapsuleShape;
 
 struct CapsuleShapeParam {
-    CapsuleShape* createShape(sead::Heap* heap);
-
     sead::Vector3f vertex_a;
     sead::Vector3f vertex_b;
     f32 radius;
@@ -33,6 +31,9 @@ public:
         Modified = 1 << 0,
     };
 
+    static CapsuleShape* make(const CapsuleShapeParam& param, sead::Heap* heap);
+    CapsuleShape* clone(sead::Heap* heap) const;
+
     CapsuleShape(const CapsuleShapeParam& shape_, hkpShape* hkp_shape_);
     ~CapsuleShape() override;
 
@@ -43,13 +44,11 @@ public:
     hkpShape* updateHavokShape() override;
     void setScale(float scale) override;
 
-    RigidBody* createBody(bool flag, const RigidBodyInstanceParam& params, sead::Heap* heap);
-    CapsuleShape* clone(sead::Heap* heap);
     f32 getRadius() const;
     void getVertices(sead::Vector3f* va, sead::Vector3f* vb) const;
     bool setRadius(f32 r);
     bool setVertices(const sead::Vector3f& va, const sead::Vector3f& vb);
-    void sub_7100FABE80(sead::Vector3f* veca, sead::Vector3f* vecb, const hkTransformf& rb_vec);
+    void transformVertices(sead::Vector3f* veca, sead::Vector3f* vecb, const hkTransformf& rb_vec);
     void setMaterialMask(const MaterialMask& mask);
 
     sead::Vector3f vertex_a;

src/KingSystem/Physics/RigidBody/physRigidBodyFactory.cpp

@@ -1,6 +1,7 @@
 #include "KingSystem/Physics/RigidBody/physRigidBodyFactory.h"
 #include "KingSystem/Physics/RigidBody/Shape/physBoxShape.h"
 #include "KingSystem/Physics/RigidBody/Shape/physBoxWaterShape.h"
+#include "KingSystem/Physics/RigidBody/Shape/physCapsuleRigidBody.h"
 #include "KingSystem/Physics/RigidBody/Shape/physCapsuleShape.h"
 #include "KingSystem/Physics/RigidBody/Shape/physCylinderShape.h"
 #include "KingSystem/Physics/RigidBody/Shape/physCylinderWaterShape.h"
@@ -9,6 +10,16 @@
 
 namespace ksys::phys {
 
+template <typename RigidBodyType, typename ShapeType, typename ParamType>
+static RigidBodyType* createRigidBody(RigidBodyInstanceParam* param, sead::Heap* heap) {
+    if (param->isDynamicSensor())
+        param->motion_type = MotionType::Keyframed;
+
+    auto* v = sead::DynamicCast<ParamType>(param);
+    auto* shape = ShapeType::make(v->shape, heap);
+    return RigidBodyFromShape::make<RigidBodyType, ShapeType>(*shape, true, *param, heap);
+}
+
 RigidBody* RigidBodyFactory::createSphere(RigidBodyInstanceParam* params, sead::Heap* heap) {
     if (params->isDynamicSensor())
         params->motion_type = MotionType::Keyframed;
@@ -18,13 +29,9 @@ RigidBody* RigidBodyFactory::createSphere(RigidBodyInstanceParam* params, sead::
     return shape->createBody(true, *params, heap);
 }
 
-RigidBody* RigidBodyFactory::createCapsule(RigidBodyInstanceParam* params, sead::Heap* heap) {
-    if (params->isDynamicSensor())
-        params->motion_type = MotionType::Keyframed;
-
-    auto* v = sead::DynamicCast<CapsuleParam>(params);
-    auto* shape = v->shape.createShape(heap);
-    return shape->createBody(true, *params, heap);
+CapsuleRigidBody* RigidBodyFactory::createCapsule(RigidBodyInstanceParam* params,
+                                                  sead::Heap* heap) {
+    return createRigidBody<CapsuleRigidBody, CapsuleShape, CapsuleParam>(params, heap);
 }
 
 RigidBody* RigidBodyFactory::createCylinder(RigidBodyInstanceParam* params, sead::Heap* heap) {
@@ -45,16 +52,6 @@ RigidBody* RigidBodyFactory::createCylinderWater(RigidBodyInstanceParam* params,
     return shape->createBody(true, *params, heap);
 }
 
-template <typename RigidBodyType, typename ShapeType, typename ParamType>
-static RigidBodyType* createRigidBody(RigidBodyInstanceParam* param, sead::Heap* heap) {
-    if (param->isDynamicSensor())
-        param->motion_type = MotionType::Keyframed;
-
-    auto* v = sead::DynamicCast<ParamType>(param);
-    auto* shape = ShapeType::make(v->shape, heap);
-    return RigidBodyFromShape::make<RigidBodyType, ShapeType>(*shape, true, *param, heap);
-}
-
 BoxRigidBody* RigidBodyFactory::createBox(RigidBodyInstanceParam* params, sead::Heap* heap) {
     return createRigidBody<BoxRigidBody, BoxShape, BoxParam>(params, heap);
 }

src/KingSystem/Physics/RigidBody/physRigidBodyFactory.h

@@ -10,13 +10,14 @@ namespace ksys::phys {
 
 class BoxRigidBody;
 class BoxWaterRigidBody;
+class CapsuleRigidBody;
 class RigidBody;
 struct RigidBodyInstanceParam;
 
 class RigidBodyFactory {
 public:
     static RigidBody* createSphere(RigidBodyInstanceParam* params, sead::Heap* heap);
-    static RigidBody* createCapsule(RigidBodyInstanceParam* params, sead::Heap* heap);
+    static CapsuleRigidBody* createCapsule(RigidBodyInstanceParam* params, sead::Heap* heap);
     static RigidBody* createCylinder(RigidBodyInstanceParam* params, sead::Heap* heap);
     static RigidBody* createCylinderWater(RigidBodyInstanceParam* params, sead::Heap* heap);
     static BoxRigidBody* createBox(RigidBodyInstanceParam* params, sead::Heap* heap);