ksys/phys: Implement PolytopeRigidBody

2 files changed, 90 insertions, 1 deletions

diff --git a/src/KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeRigidBody.cpp b/src/KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeRigidBody.cpp
index 256ab6f6..b0b608c9 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeRigidBody.cpp
+++ b/src/KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeRigidBody.cpp
@@ -1 +1,66 @@
 #include "KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeRigidBody.h"
+#include "KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeShape.h"
+#include "KingSystem/Physics/RigidBody/physRigidBodyFactory.h"
+#include "KingSystem/Utils/SafeDelete.h"
+
+namespace ksys::phys {
+
+PolytopeRigidBody* PolytopeRigidBody::make(RigidBodyInstanceParam* param, sead::Heap* heap) {
+    return RigidBodyFactory::createPolytope(param, heap);
+}
+
+PolytopeRigidBody::PolytopeRigidBody(hkpRigidBody* hk_body, PolytopeShape* 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) {}
+
+PolytopeRigidBody::~PolytopeRigidBody() {
+    if (hasFlag(RigidBody::Flag::_10) && mShape) {
+        util::safeDelete(mShape);
+    }
+}
+
+bool PolytopeRigidBody::setVertex(int vertex_idx, const sead::Vector3f& vertex) {
+    return mShape->setVertex(vertex_idx, vertex);
+}
+
+u16 PolytopeRigidBody::getNumVertices() const {
+    return mShape->getNumVertices();
+}
+
+const sead::Vector3f& PolytopeRigidBody::getVertex(int vertex_idx) const {
+    return mShape->getVertex(vertex_idx);
+}
+
+void PolytopeRigidBody::setMaterialMask(const MaterialMask& mask) {
+    mShape->setMaterialMask(mask);
+}
+
+const MaterialMask& PolytopeRigidBody::getMaterialMask() const {
+    return mShape->getMaterialMask();
+}
+
+float PolytopeRigidBody::getVolume() {
+    return mShape->getVolume();
+}
+
+void PolytopeRigidBody::setVolume(float volume) {
+    mShape->setVolume(volume);
+}
+
+Shape* PolytopeRigidBody::getShape_() {
+    return mShape;
+}
+
+const Shape* PolytopeRigidBody::getShape_() const {
+    return mShape;
+}
+
+u32 PolytopeRigidBody::getCollisionMasks(RigidBody::CollisionMasks* masks, const u32* unk) {
+    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/Polytope/physPolytopeRigidBody.h b/src/KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeRigidBody.h
index d830d793..b5b462f8 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeRigidBody.h
+++ b/src/KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeRigidBody.h
@@ -4,10 +4,34 @@
 
 namespace ksys::phys {
 
+class PolytopeShape;
+
 class PolytopeRigidBody : public RigidBodyFromShape {
     SEAD_RTTI_OVERRIDE(PolytopeRigidBody, RigidBodyFromShape)
 public:
-    // TODO
+    static PolytopeRigidBody* make(RigidBodyInstanceParam* param, sead::Heap* heap);
+
+    PolytopeRigidBody(hkpRigidBody* hk_body, PolytopeShape* shape, ContactLayerType layer_type,
+                      const sead::SafeString& name, bool set_flag_10, sead::Heap* heap);
+    ~PolytopeRigidBody() override;
+
+    bool setVertex(int vertex_idx, const sead::Vector3f& vertex);
+    u16 getNumVertices() const;
+    const sead::Vector3f& getVertex(int vertex_idx) const;
+
+    void setMaterialMask(const MaterialMask& mask);
+    const MaterialMask& getMaterialMask() const;
+
+    float getVolume() override;
+    void setVolume(float volume);
+
+protected:
+    Shape* getShape_() override;
+    const Shape* getShape_() const override;
+    u32 getCollisionMasks(RigidBody::CollisionMasks* masks, const u32* unk) override;
+
+private:
+    PolytopeShape* mShape{};
 };
 
 }  // namespace ksys::phys