Implement ksys::phys::CharacterPrismShape class (#106)

5 files changed, 216 insertions, 4 deletions

diff --git a/src/KingSystem/Physics/RigidBody/Shape/CharacterPrism/physCharacterPrismShape.cpp b/src/KingSystem/Physics/RigidBody/Shape/CharacterPrism/physCharacterPrismShape.cpp
index ab2e87e0..1e58d6d8 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/CharacterPrism/physCharacterPrismShape.cpp
+++ b/src/KingSystem/Physics/RigidBody/Shape/CharacterPrism/physCharacterPrismShape.cpp
@@ -1 +1,181 @@
 #include "KingSystem/Physics/RigidBody/Shape/CharacterPrism/physCharacterPrismShape.h"
+#include <Havok/Geometry/Internal/Types/hkcdVertex.h>
+#include <Havok/Physics2012/Collide/Shape/Convex/ConvexVertices/hkpConvexVerticesShape.h>
+#include <Havok/Physics2012/Collide/Shape/Convex/hkpConvexShape.h>
+#include "KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeShape.h"
+#include "KingSystem/Physics/physConversions.h"
+
+namespace ksys::phys {
+
+void CharacterPrismShape::setMaterialMask(const MaterialMask& mask) {
+    mShape->setMaterialMask(mask);
+}
+
+const MaterialMask& CharacterPrismShape::getMaterialMask() const {
+    return mShape->getMaterialMask();
+}
+
+static const sead::Vector3f& sOrigin = sead::Vector3f::zero;
+
+CharacterPrismShape* CharacterPrismShape::make(const CharacterPrismShapeParam& param,
+                                               sead::Heap* heap) {
+    PolytopeShapeParam polytope_shape_param;
+    polytope_shape_param.vertex_num = SHAPE_VERTEX_NUM;
+    polytope_shape_param.common.material = param.common.material;
+    polytope_shape_param.common.sub_material = param.common.sub_material;
+    polytope_shape_param.common.floor_code = param.common.floor_code;
+    polytope_shape_param.common.wall_code = param.common.wall_code;
+    polytope_shape_param.common.item_code_disable_stick = param.common.item_code_disable_stick;
+
+    auto* polytope_shape = PolytopeShape::make(polytope_shape_param, heap);
+
+    /*
+     * Set all vertices.
+     */
+
+    // Set first vertex
+    polytope_shape->setVertex(0, sOrigin + param.offset);
+
+    for (int i = 0, vertex_idx = 1; i < RING_VERTEX_NUM; i++, vertex_idx += 2) {
+        float ring_x = param.radius * sin((float)i * (float)M_PI_4);
+        float ring_z = param.radius * cos((float)i * (float)M_PI_4);
+
+        // First ring
+        polytope_shape->setVertex(vertex_idx, sead::Vector3f(ring_x, param.ring0_distance, ring_z) +
+                                                  param.offset);
+
+        // Second ring
+        polytope_shape->setVertex(
+            vertex_idx + 1, sead::Vector3f(ring_x, param.ring1_distance, ring_z) + param.offset);
+    }
+
+    // Set last vertex
+    polytope_shape->setVertex(SHAPE_VERTEX_NUM - 1,
+                              param.end_vertex_distance * sead::Vector3f::ey + param.offset);
+
+    polytope_shape->updateHavokShape();
+
+    /*
+     * Set all vertices again, but move the bottom so that it lies at y=0.
+     * This is undone if `CharacterPrismShape::setScale` is called.
+     */
+
+    // Get lowest vertex
+    hkcdVertex supporting_vertex;
+    polytope_shape->getVerticesShape()->getSupportingVertex(hkVector4f(0.0f, -1.0f, 0.0f),
+                                                            supporting_vertex);
+    // Get y-value of the lowest point of collision
+    float min_vertex_y =
+        supporting_vertex[1] + 0.0f - polytope_shape->getVerticesShape()->getRadius();
+
+    // Set first vertex
+    // Move to lie at y=0
+    polytope_shape->setVertex(0, sOrigin + param.offset - min_vertex_y * sead::Vector3f::ey);
+
+    for (int i = 0, vertex_idx = 1; i < RING_VERTEX_NUM; i++, vertex_idx += 2) {
+        float ring_x = param.radius * sin((float)i * (float)M_PI_4);
+        float ring_z = param.radius * cos((float)i * (float)M_PI_4);
+
+        // First ring
+        polytope_shape->setVertex(vertex_idx, sead::Vector3f(ring_x, param.ring0_distance, ring_z) +
+                                                  param.offset - min_vertex_y * sead::Vector3f::ey);
+
+        // Second ring
+        polytope_shape->setVertex(
+            vertex_idx + 1, sead::Vector3f(ring_x, param.ring1_distance, ring_z) + param.offset);
+    }
+
+    // Set last vertex
+    polytope_shape->setVertex(SHAPE_VERTEX_NUM - 1,
+                              param.end_vertex_distance * sead::Vector3f::ey + param.offset);
+
+    polytope_shape->updateHavokShape();
+
+    auto* shape = new (heap) CharacterPrismShape;
+    shape->mShape = polytope_shape;
+    shape->mRadius = param.radius;
+    shape->mRing0Distance = param.ring0_distance;
+    shape->mRing1Distance = param.ring1_distance;
+    shape->mEndVertexDistance = param.end_vertex_distance;
+    shape->mOffset.e = param.offset.e;
+    shape->mScale = 1.0f;
+    return shape;
+}
+
+CharacterPrismShape* CharacterPrismShape::clone(sead::Heap* heap) const {
+    CharacterPrismShapeParam param;
+    param.radius = mRadius;
+    param.end_vertex_distance = mEndVertexDistance;
+    param.ring1_distance = mRing1Distance;
+    param.ring0_distance = mRing0Distance;
+    param.offset = mOffset;
+    param.common.material = mShape->getMaterialMask().getMaterial();
+    param.common.sub_material = mShape->getMaterialMask().getSubMaterialName();
+    param.common.floor_code = mShape->getMaterialMask().getFloorCode();
+    param.common.wall_code = mShape->getMaterialMask().getWallCode();
+    auto data = mShape->getMaterialMask().getData();
+    param.common.item_code_disable_stick = data.getCustomFlag(MaterialMaskData::CustomFlag::_0);
+    return make(param, heap);
+}
+
+float CharacterPrismShape::getVolume() const {
+    return mShape->getVolume();
+}
+
+hkpShape* CharacterPrismShape::getHavokShape() {
+    // For some reason, this non-const function is calling the const function `getHavokShapeConst`.
+    return const_cast<hkpShape*>(mShape->getHavokShapeConst());
+}
+
+const hkpShape* CharacterPrismShape::getHavokShape() const {
+    return mShape->getHavokShapeConst();
+}
+
+const hkpShape* CharacterPrismShape::updateHavokShape() {
+    return mShape->updateHavokShape();
+}
+
+void CharacterPrismShape::setScale(float scale) {
+    // `mScale` is scaled relatively by `scale`, but vertices are scaled absolutely.
+    // Using `CharacterPrismShape::setScale` more than once will put `mScale` out of sync with
+    // the actual scale.
+    mScale *= scale;
+
+    float volume = getVolume();
+
+    auto scaled_offset = mOffset * scale;
+    float scaled_end_vertex_distance = mEndVertexDistance * scale;
+    float scaled_radius = mRadius * scale;
+    float ring_y0 = mRing0Distance * scale;
+    float ring_y1 = mRing1Distance * scale;
+
+    // Set first vertex
+    mShape->setVertex(0, sOrigin + scaled_offset);
+
+    for (int i = 0, vertex_idx = 1; i < RING_VERTEX_NUM; i++, vertex_idx += 2) {
+        // For some reason `sin` is used for x and `cos` is used for z.
+        float ring_x = scaled_radius * sin((float)i * (float)M_PI_4);
+        float ring_z = scaled_radius * cos((float)i * (float)M_PI_4);
+
+        // First ring
+        mShape->setVertex(vertex_idx, sead::Vector3f{ring_x, ring_y0, ring_z} + scaled_offset);
+
+        // Second ring
+        mShape->setVertex(vertex_idx + 1, sead::Vector3f{ring_x, ring_y1, ring_z} + scaled_offset);
+    }
+
+    // Set last vertex
+    mShape->setVertex(SHAPE_VERTEX_NUM - 1,
+                      scaled_end_vertex_distance * sead::Vector3f::ey + scaled_offset);
+
+    mShape->setVolume(scale * scale * scale * volume);
+}
+
+CharacterPrismShape::~CharacterPrismShape() {
+    if (mShape) {
+        delete mShape;
+        mShape = nullptr;
+    }
+}
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/RigidBody/Shape/CharacterPrism/physCharacterPrismShape.h b/src/KingSystem/Physics/RigidBody/Shape/CharacterPrism/physCharacterPrismShape.h
index 63e4b238..1724835b 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/CharacterPrism/physCharacterPrismShape.h
+++ b/src/KingSystem/Physics/RigidBody/Shape/CharacterPrism/physCharacterPrismShape.h
@@ -9,25 +9,49 @@ class PolytopeShape;
 
 struct CharacterPrismShapeParam {
     float radius = 0.35;
-    sead::Vector3f translate_0 = {0.5, 1.5, 1.8};
-    sead::Vector3f translate_1 = sead::Vector3f::zero;
+    float ring0_distance = 0.5;
+    float ring1_distance = 1.5;
+    float end_vertex_distance = 1.8;
+    sead::Vector3f offset = sead::Vector3f::zero;
     CommonShapeParam common;
 };
 
+/// A "stretched bipyramid" shape with a top and bottom vertex and two rings of eight vertices
+/// between them.
 class CharacterPrismShape : public Shape {
     SEAD_RTTI_OVERRIDE(CharacterPrismShape, Shape)
+
 public:
+    ~CharacterPrismShape() override;
+
     static CharacterPrismShape* make(const CharacterPrismShapeParam& param, sead::Heap* heap);
     CharacterPrismShape* clone(sead::Heap* heap) const;
 
     void setMaterialMask(const MaterialMask& mask);
     const MaterialMask& getMaterialMask() const;
 
+    ShapeType getType() const override { return ShapeType::CharacterPrism; }
+    float getVolume() const override;
+    hkpShape* getHavokShape() override;
+    const hkpShape* getHavokShape() const override;
+    const hkpShape* updateHavokShape() override;
+    void setScale(float scale) override;
+
     PolytopeShape* getUnderlyingShape() const { return mShape; }
 
 private:
+    static constexpr int RING_VERTEX_NUM = 8;
+    static constexpr int SHAPE_VERTEX_NUM = RING_VERTEX_NUM * 2 + 2;
+
     /// The underlying shape for this character prism shape.
     PolytopeShape* mShape{};
+
+    float mRadius;
+    float mRing0Distance;
+    float mRing1Distance;
+    float mEndVertexDistance;
+    sead::Vector3f mOffset;
+    float mScale;
 };
 
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeShape.h b/src/KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeShape.h
index bc7a64f2..3a62e7a6 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeShape.h
+++ b/src/KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeShape.h
@@ -57,6 +57,8 @@ public:
     void setMaterialMask(const MaterialMask& mask);
     const MaterialMask& getMaterialMask() const { return mMaterialMask; }
 
+    const hkpConvexVerticesShape* getVerticesShape() const { return mHavokShape; }
+
 private:
     bool init(const PolytopeShapeParam& param, sead::Heap* heap);
 
diff --git a/src/KingSystem/Physics/RigidBody/Shape/physShapeParamObj.cpp b/src/KingSystem/Physics/RigidBody/Shape/physShapeParamObj.cpp
index 2daebe8c..90a04023 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/physShapeParamObj.cpp
+++ b/src/KingSystem/Physics/RigidBody/Shape/physShapeParamObj.cpp
@@ -104,8 +104,10 @@ void ShapeParamObj::getPolytope(PolytopeShapeParam* param) const {
 
 void ShapeParamObj::getCharacterPrism(CharacterPrismShapeParam* param) const {
     param->radius = *radius;
-    param->translate_0 = *translate_0;
-    param->translate_1 = *translate_1;
+    param->ring0_distance = translate_0->x;
+    param->ring1_distance = translate_0->y;
+    param->end_vertex_distance = translate_0->z;
+    param->offset = *translate_1;
     getCommon(&param->common);
 }
 
diff --git a/src/KingSystem/Physics/physMaterialMask.h b/src/KingSystem/Physics/physMaterialMask.h
index ba4bedba..33e3b466 100644
--- a/src/KingSystem/Physics/physMaterialMask.h
+++ b/src/KingSystem/Physics/physMaterialMask.h
@@ -41,6 +41,10 @@ union MaterialMaskData {
         raw |= 1 << (decltype(custom_flags)::StartBit() + custom_flag);
     }
 
+    bool getCustomFlag(CustomFlag custom_flag) const {
+        return (raw & 1 << (decltype(custom_flags)::StartBit() + custom_flag)) != 0;
+    }
+
     u32 raw;
     util::BitField<0, 6, u32> material;
     util::BitField<6, 4, int, u32> sub_material;