ksys/phys: Implement RigidBodyParam::createRigidBody

Also includes a bunch of fixes to the shape and param classes.

11 files changed, 261 insertions, 46 deletions

diff --git a/src/KingSystem/Physics/RigidBody/Shape/Box/physBoxShape.h b/src/KingSystem/Physics/RigidBody/Shape/Box/physBoxShape.h
index 9c8497dd..86c5ba72 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/Box/physBoxShape.h
+++ b/src/KingSystem/Physics/RigidBody/Shape/Box/physBoxShape.h
@@ -65,19 +65,17 @@ public:
 };
 
 struct BoxShapeParam {
-    sead::Vector3f extents;
-    sead::Vector3f translate;
-    sead::Vector3f rotate;
+    sead::Vector3f extents = {0.5, 0.5, 0.5};
+    sead::Vector3f translate = sead::Vector3f::zero;
+    sead::Vector3f rotate = sead::Vector3f::zero;
     float convex_radius = 0.05;
     CommonShapeParam common;
 };
 
-class BoxParam : public RigidBodyInstanceParam {
+class BoxParam : public RigidBodyInstanceParam, public BoxShapeParam {
     SEAD_RTTI_OVERRIDE(BoxParam, RigidBodyInstanceParam)
 public:
-    u8 _90;
-    float _94;
-    BoxShapeParam shape;
+    BoxParam() : RigidBodyInstanceParam(ShapeType::Box) {}
 };
 
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/RigidBody/Shape/Capsule/physCapsuleShape.h b/src/KingSystem/Physics/RigidBody/Shape/Capsule/physCapsuleShape.h
index 04cf1d3a..3a5618aa 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/Capsule/physCapsuleShape.h
+++ b/src/KingSystem/Physics/RigidBody/Shape/Capsule/physCapsuleShape.h
@@ -18,9 +18,9 @@ class CapsuleParam;
 struct CapsuleShape;
 
 struct CapsuleShapeParam {
-    sead::Vector3f vertex_a;
-    sead::Vector3f vertex_b;
-    f32 radius;
+    sead::Vector3f vertex_a = -1 * sead::Vector3f::ey;
+    sead::Vector3f vertex_b = +1 * sead::Vector3f::ey;
+    f32 radius = 1.0;
     CommonShapeParam common;
 };
 
@@ -60,12 +60,10 @@ public:
     hkpShape* shape;
 };
 
-class CapsuleParam : public RigidBodyInstanceParam {
+class CapsuleParam : public RigidBodyInstanceParam, public CapsuleShapeParam {
     SEAD_RTTI_OVERRIDE(CapsuleParam, RigidBodyInstanceParam)
 public:
-    u8 _90;
-    float _94;
-    CapsuleShapeParam shape;
+    CapsuleParam() : RigidBodyInstanceParam(ShapeType::Capsule) {}
 };
 
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/RigidBody/Shape/Cylinder/physCylinderShape.h b/src/KingSystem/Physics/RigidBody/Shape/Cylinder/physCylinderShape.h
index 9eed38b1..82ff0109 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/Cylinder/physCylinderShape.h
+++ b/src/KingSystem/Physics/RigidBody/Shape/Cylinder/physCylinderShape.h
@@ -58,28 +58,22 @@ 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;
+    sead::Vector3f vertex_a = -1 * sead::Vector3f::ey;
     /// The radius of the circular bases.
-    float radius;
+    float radius = 1.0;
     /// The center of the second circular base.
-    sead::Vector3f vertex_b;
+    sead::Vector3f vertex_b = +1 * sead::Vector3f::ey;
     /// Additional shell radius around the cylinder.
     /// @warning This is ignored by CylinderShape.
     float convex_radius = 0.05;
     CommonShapeParam common;
 };
 
-class CylinderParam : public RigidBodyInstanceParam {
+class CylinderParam : public RigidBodyInstanceParam, public CylinderShapeParam {
     SEAD_RTTI_OVERRIDE(CylinderParam, RigidBodyInstanceParam)
 public:
-    u8 _90;
-    float _94;
-    CylinderShapeParam shape;
+    CylinderParam() : RigidBodyInstanceParam(ShapeType::Cylinder) {}
 };
 
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterShape.cpp b/src/KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterShape.cpp
index 2a3298c4..f9b65b07 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterShape.cpp
+++ b/src/KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterShape.cpp
@@ -131,9 +131,8 @@ bool CylinderWaterShape::setHeight(float height) {
     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);
+    CylinderShapeParam param;
     param.radius = getRadius();
     param.vertex_a.x = getHeight();
     auto* cloned = make(param, heap);
diff --git a/src/KingSystem/Physics/RigidBody/Shape/List/physListShape.h b/src/KingSystem/Physics/RigidBody/Shape/List/physListShape.h
index cdeecb5d..de29ab30 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/List/physListShape.h
+++ b/src/KingSystem/Physics/RigidBody/Shape/List/physListShape.h
@@ -20,7 +20,7 @@ struct SphereShapeParam;
 class MaterialMask;
 
 struct ListShapeParam {
-    u8 num_shapes = 1;
+    u8 num_shapes = 4;
 };
 
 class ListShape : public Shape {
@@ -97,10 +97,10 @@ private:
     sead::TypedBitFlag<Flag, sead::Atomic<u32>> mFlags{Flag::NeedsHavokShapeUpdate};
 };
 
-class ListShapeRigidBodyParam : public RigidBodyInstanceParam {
+class ListShapeRigidBodyParam : public RigidBodyInstanceParam, public ListShapeParam {
     SEAD_RTTI_OVERRIDE(ListShapeRigidBodyParam, RigidBodyInstanceParam)
 public:
-    ListShapeParam shape;
+    ListShapeRigidBodyParam() : RigidBodyInstanceParam(ShapeType::List) {}
 };
 
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeShape.h b/src/KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeShape.h
index 9913fc99..dc55ca0d 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeShape.h
+++ b/src/KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeShape.h
@@ -17,7 +17,7 @@ namespace ksys::phys {
 
 struct PolytopeShapeParam {
     /// Number of vertices.
-    u16 vertex_num;
+    u16 vertex_num = 32;
     CommonShapeParam common;
 };
 
@@ -75,10 +75,10 @@ constexpr PolytopeShape::Flag operator|(PolytopeShape::Flag a, PolytopeShape::Fl
     return PolytopeShape::Flag(u32(a) | u32(b));
 }
 
-class PolytopeParam : public RigidBodyInstanceParam {
+class PolytopeParam : public RigidBodyInstanceParam, public PolytopeShapeParam {
     SEAD_RTTI_OVERRIDE(PolytopeParam, RigidBodyInstanceParam)
 public:
-    PolytopeShapeParam shape;
+    PolytopeParam() : RigidBodyInstanceParam(ShapeType::Polytope) {}
 };
 
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/RigidBody/Shape/Sphere/physSphereShape.h b/src/KingSystem/Physics/RigidBody/Shape/Sphere/physSphereShape.h
index fd5e7c0f..f9453d67 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/Sphere/physSphereShape.h
+++ b/src/KingSystem/Physics/RigidBody/Shape/Sphere/physSphereShape.h
@@ -10,8 +10,8 @@ namespace ksys::phys {
 class SphereParam;
 
 struct SphereShapeParam {
-    sead::Vector3f translate;
-    float radius;
+    sead::Vector3f translate = sead::Vector3f::zero;
+    float radius = 1.0;
     CommonShapeParam common;
 };
 
@@ -29,12 +29,10 @@ private:
     MaterialMask mMaterialMask;
 };
 
-class SphereParam : public RigidBodyInstanceParam {
+class SphereParam : public RigidBodyInstanceParam, public SphereShapeParam {
     SEAD_RTTI_OVERRIDE(SphereParam, RigidBodyInstanceParam)
 public:
-    u8 _90;
-    float _94;
-    SphereShapeParam shape;
+    SphereParam() : RigidBodyInstanceParam(ShapeType::Sphere) {}
 };
 
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.h b/src/KingSystem/Physics/RigidBody/physRigidBody.h
index cdf99692..e7925e35 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.h
+++ b/src/KingSystem/Physics/RigidBody/physRigidBody.h
@@ -488,7 +488,7 @@ public:
     bool isEntityMotionFlag40On() const;
 
     // 0x0000007100f955c0 - FIXME: types
-    void processUpdateRequests(void* data, void* data2);
+    void processUpdateRequests(void* data = nullptr, void* data2 = nullptr);
 
     void clearFlag2000000(bool clear);
     void clearFlag4000000(bool clear);
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyFactory.cpp b/src/KingSystem/Physics/RigidBody/physRigidBodyFactory.cpp
index f598c51c..9e7cf5bf 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodyFactory.cpp
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyFactory.cpp
@@ -20,7 +20,7 @@ static RigidBodyType* createRigidBody(RigidBodyInstanceParam* param, sead::Heap*
         param->motion_type = MotionType::Keyframed;
 
     auto* v = sead::DynamicCast<ParamType>(param);
-    auto* shape = ShapeType::make(v->shape, heap);
+    auto* shape = ShapeType::make(*v, heap);
     return RigidBodyFromShape::make<RigidBodyType, ShapeType>(shape, true, *param, heap);
 }
 
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyParam.cpp b/src/KingSystem/Physics/RigidBody/physRigidBodyParam.cpp
index 143d8158..45aca215 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodyParam.cpp
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyParam.cpp
@@ -1,6 +1,26 @@
 #include "KingSystem/Physics/RigidBody/physRigidBodyParam.h"
+#include <basis/seadRawPrint.h>
+#include <functional>
+#include "KingSystem/Physics/RigidBody/Shape/Box/physBoxRigidBody.h"
+#include "KingSystem/Physics/RigidBody/Shape/Box/physBoxShape.h"
+#include "KingSystem/Physics/RigidBody/Shape/BoxWater/physBoxWaterRigidBody.h"
+#include "KingSystem/Physics/RigidBody/Shape/BoxWater/physBoxWaterShape.h"
+#include "KingSystem/Physics/RigidBody/Shape/Capsule/physCapsuleRigidBody.h"
+#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/List/physListShape.h"
+#include "KingSystem/Physics/RigidBody/Shape/List/physListShapeRigidBody.h"
+#include "KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeRigidBody.h"
+#include "KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeShape.h"
+#include "KingSystem/Physics/RigidBody/Shape/Sphere/physSphereRigidBody.h"
+#include "KingSystem/Physics/RigidBody/Shape/Sphere/physSphereShape.h"
 #include "KingSystem/Physics/RigidBody/Shape/physShapeParamObj.h"
+#include "KingSystem/Physics/RigidBody/physRigidBodyFactory.h"
 #include "KingSystem/Physics/System/physEntityGroupFilter.h"
+#include "KingSystem/Utils/SafeDelete.h"
 
 namespace ksys::phys {
 
@@ -96,6 +116,193 @@ void RigidBodyParam::makeInstanceParam(RigidBodyInstanceParam* param) const {
     receiverMaskGetSensorLayerMaskForType(&param->receiver_mask, *info.receiver_type);
 }
 
+template <auto Getter, typename ParamT>
+static bool makeParamImpl(const RigidBodyParam& param, RigidBodyInstanceParam* out,
+                          ShapeType shape_type) {
+    if (param.getShapeType() != shape_type)
+        return false;
+
+    param.makeInstanceParam(out);
+    std::invoke(Getter, param.shapes[0], static_cast<ParamT*>(out));
+    return true;
+}
+
+template <auto Getter, typename ParamT>
+static bool makeParam(const RigidBodyParam& param, ParamT* out, ShapeType shape_type) {
+    return makeParamImpl<Getter, ParamT>(param, out, shape_type);
+}
+
+static bool makeSphereParam(const RigidBodyParam& param, SphereParam* out) {
+    return makeParam<&ShapeParamObj::getSphere>(param, out, ShapeType::Sphere);
+}
+
+static bool makeCapsuleParam(const RigidBodyParam& param, CapsuleParam* out) {
+    return makeParam<&ShapeParamObj::getCapsule>(param, out, ShapeType::Capsule);
+}
+
+static bool makeBoxParam(const RigidBodyParam& param, BoxParam* out) {
+    return makeParam<&ShapeParamObj::getBox>(param, out, ShapeType::Box);
+}
+
+static bool makeCylinderParam(const RigidBodyParam& param, CylinderParam* out) {
+    return makeParam<&ShapeParamObj::getCylinder>(param, out, ShapeType::Cylinder);
+}
+
+static bool makePolytopeParam(const RigidBodyParam& param, PolytopeParam* out) {
+    return makeParam<&ShapeParamObj::getPolytope>(param, out, ShapeType::Polytope);
+}
+
+RigidBody*
+RigidBodyParam::createRigidBody(SystemGroupHandler* group_handler, sead::Heap* heap,
+                                RigidBodyParam::CreateFixedBoxWithNoCollision no_collision) const {
+    if (no_collision == CreateFixedBoxWithNoCollision::Yes) {
+        BoxParam param;
+        makeInstanceParam(&param);
+
+        if (getContactLayerType(param.contact_layer) == ContactLayerType::Entity)
+            param.contact_layer = ContactLayer::EntityNoHit;
+        else
+            param.contact_layer = ContactLayer::SensorNoHit;
+
+        param.motion_type = MotionType::Fixed;
+        param.translate = *info.bounding_center;
+        param.extents = *info.bounding_extents;
+        param.system_group_handler = group_handler;
+        auto* body = BoxRigidBody::make(&param, heap);
+        body->setFlag20();
+        return body;
+    }
+
+    const int num_shapes = *info.shape_num;
+
+    if (num_shapes == 1) {
+        switch (getShapeType()) {
+        case ShapeType::Sphere: {
+            SphereParam param;
+            makeSphereParam(*this, &param);
+            param.system_group_handler = group_handler;
+            return SphereRigidBody::make(&param, heap);
+        }
+
+        case ShapeType::Capsule: {
+            CapsuleParam param;
+            makeCapsuleParam(*this, &param);
+            param.system_group_handler = group_handler;
+            return CapsuleRigidBody::make(&param, heap);
+        }
+
+        case ShapeType::Box: {
+            BoxParam param;
+            makeBoxParam(*this, &param);
+            param.system_group_handler = group_handler;
+            if (param.contact_layer == ContactLayer::EntityWater)
+                return BoxWaterRigidBody::make(&param, heap);
+            return BoxRigidBody::make(&param, heap);
+        }
+
+        case ShapeType::Cylinder: {
+            CylinderParam param;
+            makeCylinderParam(*this, &param);
+            param.system_group_handler = group_handler;
+            if (param.contact_layer == ContactLayer::EntityWater)
+                return CylinderWaterRigidBody::make(&param, heap);
+            return CylinderRigidBody::make(&param, heap);
+        }
+
+        case ShapeType::Polytope: {
+            PolytopeParam param;
+            makePolytopeParam(*this, &param);
+            param.system_group_handler = group_handler;
+
+            auto* body = PolytopeRigidBody::make(&param, heap);
+            for (int i = 0, n = *shapes[0].vertex_num; i < n; ++i)
+                body->setVertex(i, *shapes[0].vertices[i]);
+
+            body->setVolume(*info.volume);
+            body->updateShape();
+            return body;
+        }
+
+        case ShapeType::List:
+        case ShapeType::CharacterPrism:
+        case ShapeType::BoxWater:
+        case ShapeType::CylinderWater:
+        case ShapeType::Unknown:
+            break;
+        }
+        SEAD_ASSERT_MSG(false, "unexpected shape type (shape_num=1)");
+        return nullptr;
+    }
+
+    // Multiple shapes. Use a ListShape.
+
+    ListShapeRigidBodyParam list_param;
+    makeInstanceParam(&list_param);
+    list_param.system_group_handler = group_handler;
+    list_param.num_shapes = num_shapes;
+
+    auto* list_body = ListShapeRigidBody::make(&list_param, heap);
+    list_body->setUpdateRequestedFlag();
+
+    for (int i = 0; i < num_shapes; ++i) {
+        switch (shapes[i].getShapeType()) {
+        case ShapeType::Sphere: {
+            SphereShapeParam param;
+            shapes[i].getSphere(&param);
+            list_body->replaceWithNewSphere(i, param, heap);
+            continue;
+        }
+
+        case ShapeType::Capsule: {
+            CapsuleShapeParam param;
+            shapes[i].getCapsule(&param);
+            list_body->replaceWithNewCapsule(i, param, heap);
+            continue;
+        }
+
+        case ShapeType::Box: {
+            BoxShapeParam param;
+            shapes[i].getBox(&param);
+            list_body->replaceWithNewBox(i, param, heap);
+            continue;
+        }
+
+        case ShapeType::Cylinder: {
+            CylinderShapeParam param;
+            shapes[i].getCylinder(&param);
+            list_body->replaceWithNewCylinder(i, param, heap);
+            continue;
+        }
+
+        case ShapeType::Polytope: {
+            PolytopeShapeParam param;
+            shapes[i].getPolytope(&param);
+            auto* polytope =
+                static_cast<PolytopeShape*>(list_body->replaceWithNewPolytope(i, param, heap));
+            for (int vertex_idx = 0, n = *shapes[i].vertex_num; vertex_idx < n; ++vertex_idx)
+                polytope->setVertex(vertex_idx, *shapes[i].vertices[vertex_idx]);
+            continue;
+        }
+
+        case ShapeType::List:
+        case ShapeType::CharacterPrism:
+        case ShapeType::BoxWater:
+        case ShapeType::CylinderWater:
+        case ShapeType::Unknown:
+            break;
+        }
+        SEAD_ASSERT_MSG(false, "unexpected shape type for list shape");
+        util::safeDelete(list_body);
+    }
+
+    if (!list_body)
+        return nullptr;
+
+    list_body->updateShape();
+    list_body->processUpdateRequests();
+    return list_body;
+}
+
 ContactLayer RigidBodyParam::getContactLayer() const {
     return contactLayerFromText(*info.layer);
 }
@@ -108,6 +315,13 @@ MotionType RigidBodyParam::getMotionType() const {
     return motionTypeFromText(*info.motion_type);
 }
 
+ShapeType RigidBodyParam::getShapeType() const {
+    if (*info.shape_num > 1)
+        return ShapeType::List;
+
+    return shapes[0].getShapeType();
+}
+
 namespace {
 constexpr const char* navmesh_types[] = {
     "NOT_USE",
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyParam.h b/src/KingSystem/Physics/RigidBody/physRigidBodyParam.h
index c253d5db..a1d893ad 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodyParam.h
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyParam.h
@@ -5,12 +5,15 @@
 #include <agl/Utils/aglParameterObj.h>
 #include <container/seadBuffer.h>
 #include <prim/seadRuntimeTypeInfo.h>
+#include "KingSystem/Physics/RigidBody/Shape/physShape.h"
 #include "KingSystem/Physics/System/physDefines.h"
 #include "KingSystem/Utils/Types.h"
 
 namespace ksys::phys {
 
+struct ShapeParamObj;
 class SystemGroupHandler;
+class RigidBody;
 
 // TODO: maybe move this to NavMesh/?
 enum class NavMeshType {
@@ -46,13 +49,14 @@ enum class NavMeshSubMaterial {
     None = 0x15,
 };
 
-struct ShapeParamObj;
-
 struct RigidBodyInstanceParam {
     SEAD_RTTI_BASE(RigidBodyInstanceParam)
 public:
+    RigidBodyInstanceParam() = default;
+    explicit RigidBodyInstanceParam(ShapeType type) : shape_type(type) {}
+
     const char* name = "no name";
-    u32 _10 = -1;
+    ShapeType shape_type = ShapeType::Unknown;
     MotionType motion_type = MotionType::Dynamic;
     float mass = 1.0f;
     sead::Vector3f inertia = sead::Vector3f::ones;
@@ -146,6 +150,11 @@ struct RigidBodyParam : agl::utl::ParameterList {
         void postRead_() override;
     };
 
+    enum class CreateFixedBoxWithNoCollision : bool {
+        Yes = true,
+        No = false,
+    };
+
     RigidBodyParam();
     ~RigidBodyParam() override;
     RigidBodyParam(const RigidBodyParam&) = delete;
@@ -153,14 +162,19 @@ struct RigidBodyParam : agl::utl::ParameterList {
 
     bool parse(const agl::utl::ResParameterList& res_list, sead::Heap* heap);
 
-    // TODO: types and names
-    void* createRigidBody(void* x, sead::Heap* heap, bool y);
     void makeInstanceParam(RigidBodyInstanceParam* param) const;
+
+    RigidBody* createRigidBody(
+        SystemGroupHandler* group_handler, sead::Heap* heap,
+        CreateFixedBoxWithNoCollision no_collision = CreateFixedBoxWithNoCollision::No) const;
+
+    // TODO: types and names
     void* createEntityShape(void* x, void* y, sead::Heap* heap);
 
     ContactLayer getContactLayer() const;
     GroundHit getGroundHit() const;
     MotionType getMotionType() const;
+    ShapeType getShapeType() const;
 
     Info info;
     sead::Buffer<ShapeParamObj> shapes;