ksys/phys: Add most of PolytopeShape

src/KingSystem/Physics/CMakeLists.txt

@@ -64,6 +64,8 @@ target_sources(uking PRIVATE
   RigidBody/Shape/CylinderWater/physCylinderWaterRigidBody.h
   RigidBody/Shape/CylinderWater/physCylinderWaterShape.cpp
   RigidBody/Shape/CylinderWater/physCylinderWaterShape.h
+  RigidBody/Shape/Polytope/physPolytopeRigidBody.cpp
+  RigidBody/Shape/Polytope/physPolytopeRigidBody.h
   RigidBody/Shape/Polytope/physPolytopeShape.cpp
   RigidBody/Shape/Polytope/physPolytopeShape.h
   RigidBody/Shape/Sphere/physSphereShape.cpp

src/KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeRigidBody.cpp

@@ -0,0 +1 @@
+#include "KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeRigidBody.h"

src/KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeRigidBody.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "KingSystem/Physics/RigidBody/physRigidBodyFromShape.h"
+
+namespace ksys::phys {
+
+class PolytopeRigidBody : public RigidBodyFromShape {
+    SEAD_RTTI_OVERRIDE(PolytopeRigidBody, RigidBodyFromShape)
+public:
+    // TODO
+};
+
+}  // namespace ksys::phys

src/KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeShape.cpp

@@ -1 +1,175 @@
 #include "KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeShape.h"
+#include <Havok/Common/Base/Types/Geometry/hkStridedVertices.h>
+#include <Havok/Physics2012/Collide/Shape/Convex/ConvexTransform/hkpConvexTransformShape.h>
+#include <Havok/Physics2012/Collide/Shape/Convex/ConvexVertices/hkpConvexVerticesShape.h>
+#include <math/seadMathCalcCommon.h>
+#include "KingSystem/Utils/HeapUtil.h"
+#include "KingSystem/Utils/SafeDelete.h"
+
+namespace ksys::phys {
+
+PolytopeShape* PolytopeShape::make(const PolytopeShapeParam& param, sead::Heap* heap) {
+    auto* shape = new (heap) PolytopeShape(param);
+
+    if (!shape->init(param, heap)) {
+        delete shape;
+        return nullptr;
+    }
+
+    return shape;
+}
+
+PolytopeShape* PolytopeShape::clone(sead::Heap* heap) const {
+    PolytopeShapeParam param;
+    param.vertex_num = u16(mVertices.size());
+
+    auto* cloned = make(param, heap);
+
+    for (int i = 0; i < int(param.vertex_num); ++i) {
+        cloned->setVertex(i, getVertex(i));
+    }
+    cloned->setNumVertices(getNumVertices());
+    cloned->updateHavokShape();
+    cloned->setMaterialMask(getMaterialMask());
+    return cloned;
+}
+
+bool PolytopeShape::setVertex(int vertex_idx, const sead::Vector3f& vertex) {
+    if (mFlags.isOn(Flag::HasCustomScale)) {
+        mScale = 1.0;
+        mFlags.reset(Flag::HasCustomScale);
+        mFlags.set(Flag::_4);
+        mFlags.set(Flag::_10);
+    }
+
+    if (vertex != mVertices[vertex_idx]) {
+        mVertices[vertex_idx] = vertex;
+        mFlags.set(Flag::_1 | Flag::InvalidVolume);
+        mVolume = -1.0;
+        return true;
+    }
+
+    return false;
+}
+
+void PolytopeShape::setNumVertices(u16 num) {
+    if (mNumVertices == num)
+        return;
+
+    mNumVertices = num;
+    mVolume = -1.0;
+    mFlags.set(Flag::_1 | Flag::InvalidVolume);
+}
+
+PolytopeShape::PolytopeShape(const PolytopeShapeParam& param)
+    : mMaterialMask(param.common.getMaterialMask()), mNumVertices(param.vertex_num) {
+    if (param.common.item_code_disable_stick)
+        mMaterialMask.getData().setCustomFlag(MaterialMaskData::CustomFlag::_0);
+
+    setMaterialMask(mMaterialMask);
+}
+
+PolytopeShape::~PolytopeShape() {
+    if (mHavokShape) {
+        /// @bug This is not how reference counting is supposed to work.
+        for (int i = 0, n = mHavokShape->getReferenceCount(); i < n; ++i)
+            mHavokShape->removeReference();
+
+        mHavokShape = nullptr;
+    }
+
+    util::deallocateObjectUnsafe(mTransformShape);
+    mVertices.freeBuffer();
+}
+
+hkpShape* PolytopeShape::getHavokShape() {
+    if (mFlags.isOn(Flag::HasCustomScale))
+        return mTransformShape;
+    return mHavokShape;
+}
+
+const hkpShape* PolytopeShape::getHavokShape() const {
+    if (mFlags.isOn(Flag::HasCustomScale))
+        return mTransformShape;
+    return mHavokShape;
+}
+
+const hkpShape* PolytopeShape::updateHavokShape() {
+    // TODO
+    return nullptr;
+}
+
+void PolytopeShape::setScale(float scale) {
+    const bool invalid_volume = mFlags.isOn(Flag::InvalidVolume);
+    const float volume = mVolume;
+
+    mScale *= scale;
+    mFlags.set(Flag::_4);
+    if (sead::Mathf::equalsEpsilon(mScale, 1.0)) {
+        mScale = 1.0;
+    }
+
+    // Rescale all vertices.
+    for (int i = 0, n = int(mNumVertices); i < n; ++i) {
+        mVertices[i] = scale * getVertex(i);
+    }
+
+    const bool had_custom_scale = mFlags.isOn(Flag::HasCustomScale);
+    const bool has_custom_scale = mScale != 1.0;
+    mFlags.change(Flag::HasCustomScale, has_custom_scale);
+    mFlags.change(Flag::_10, had_custom_scale != has_custom_scale);
+
+    if (!invalid_volume) {
+        setVolume(scale * scale * scale * volume);
+    }
+}
+
+void PolytopeShape::setVolume(float volume) {
+    mVolume = volume;
+    mFlags.reset(Flag::InvalidVolume);
+}
+
+float PolytopeShape::getVolume() const {
+    return mVolume;
+}
+
+void PolytopeShape::setMaterialMask(const MaterialMask& mask) {
+    mMaterialMask = mask;
+
+    if (mHavokShape)
+        mHavokShape->setUserData(mask.getRawData());
+
+    if (mTransformShape)
+        mTransformShape->setUserData(mask.getRawData());
+}
+
+bool PolytopeShape::init(const PolytopeShapeParam& param, sead::Heap* heap) {
+    mVertices.allocBufferAssert(param.vertex_num, heap);
+    mNumVertices = param.vertex_num;
+
+    for (int i = 0; i < int(param.vertex_num); ++i) {
+        sead::Mathf::sinCosIdx(&mVertices[i].y, &mVertices[i].z,
+                               (sead::Mathu::maxNumber() / mNumVertices) * i);
+        mVertices[i].x = static_cast<float>(i % 2);
+    }
+
+    // Alloc the vertices shape.
+    hkStridedVertices vertices;
+    vertices.set(mVertices.getBufferPtr(), mNumVertices);
+    mHavokShape = new hkpConvexVerticesShape(vertices);
+    if (!mHavokShape) {
+        return false;
+    }
+
+    // Alloc the transform shape.
+    if (auto* storage = util::allocStorage<hkpConvexTransformShape>(heap)) {
+        mTransformShape =
+            new (storage) hkpConvexTransformShape(mHavokShape, hkQsTransform::IdentityInitializer(),
+                                                  hkpShapeContainer::REFERENCE_POLICY_IGNORE);
+    }
+
+    setMaterialMask(mMaterialMask);
+    return true;
+}
+
+}  // namespace ksys::phys

src/KingSystem/Physics/RigidBody/Shape/Polytope/physPolytopeShape.h

@@ -1,13 +1,84 @@
 #pragma once
 
 #include <basis/seadTypes.h>
+#include <container/seadBuffer.h>
+#include <math/seadVector.h>
+#include <prim/seadTypedBitFlag.h>
+#include <thread/seadAtomic.h>
+#include <thread/seadCriticalSection.h>
 #include "KingSystem/Physics/RigidBody/Shape/physShape.h"
+#include "KingSystem/Physics/RigidBody/physRigidBodyParam.h"
+#include "KingSystem/Physics/System/physMaterialMask.h"
+
+class hkpConvexTransformShape;
+class hkpConvexVerticesShape;
 
 namespace ksys::phys {
 
 struct PolytopeShapeParam {
+    /// Number of vertices.
     u16 vertex_num;
     CommonShapeParam common;
 };
 
+class PolytopeShape : public Shape {
+    SEAD_RTTI_OVERRIDE(PolytopeShape, Shape)
+public:
+    enum class Flag {
+        _1 = 1 << 0,
+        InvalidVolume = 1 << 1,
+        _4 = 1 << 2,
+        HasCustomScale = 1 << 3,
+        _10 = 1 << 4,
+    };
+
+    static PolytopeShape* make(const PolytopeShapeParam& param, sead::Heap* heap);
+    PolytopeShape* clone(sead::Heap* heap) const;
+
+    explicit PolytopeShape(const PolytopeShapeParam& param);
+    ~PolytopeShape() override;
+
+    const sead::Vector3f& getVertex(int vertex_idx) const { return mVertices[vertex_idx]; }
+    bool setVertex(int vertex_idx, const sead::Vector3f& vertex);
+    u16 getNumVertices() const { return mNumVertices; }
+    void setNumVertices(u16 num);
+
+    ShapeType getType() const override { return ShapeType::Polytope; }
+    float getVolume() const override;
+    hkpShape* getHavokShape() override;
+    const hkpShape* getHavokShape() const override;
+    const hkpShape* updateHavokShape() override;
+
+    void setScale(float scale) override;
+    float getScale() const { return mScale; }
+
+    void setVolume(float volume);
+
+    void setMaterialMask(const MaterialMask& mask);
+    const MaterialMask& getMaterialMask() const { return mMaterialMask; }
+
+private:
+    bool init(const PolytopeShapeParam& param, sead::Heap* heap);
+
+    sead::CriticalSection mCS;
+    hkpConvexVerticesShape* mHavokShape{};
+    hkpConvexTransformShape* mTransformShape{};
+    sead::TypedBitFlag<Flag, sead::Atomic<u32>> mFlags;
+    MaterialMask mMaterialMask;
+    float mVolume = -1.0;
+    float mScale = 1.0;
+    sead::Buffer<sead::Vector3f> mVertices;
+    u16 mNumVertices{};
+};
+
+constexpr PolytopeShape::Flag operator|(PolytopeShape::Flag a, PolytopeShape::Flag b) {
+    return PolytopeShape::Flag(u32(a) | u32(b));
+}
+
+class PolytopeParam : public RigidBodyInstanceParam {
+    SEAD_RTTI_OVERRIDE(PolytopeParam, RigidBodyInstanceParam)
+public:
+    PolytopeShapeParam shape;
+};
+
 }  // namespace ksys::phys

src/KingSystem/Physics/RigidBody/physRigidBodyFactory.cpp

@@ -7,6 +7,7 @@
 #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/Polytope/physPolytopeShape.h"
 #include "KingSystem/Physics/RigidBody/Shape/Sphere/physSphereShape.h"
 #include "KingSystem/Physics/RigidBody/physRigidBodyFromShape.h"
 
@@ -55,4 +56,9 @@ BoxWaterRigidBody* RigidBodyFactory::createBoxWater(RigidBodyInstanceParam* para
     return createRigidBody<BoxWaterRigidBody, BoxWaterShape, BoxParam>(params, heap);
 }
 
+PolytopeRigidBody* RigidBodyFactory::createPolytope(RigidBodyInstanceParam* params,
+                                                    sead::Heap* heap) {
+    return createRigidBody<PolytopeRigidBody, PolytopeShape, PolytopeParam>(params, heap);
+}
+
 }  // namespace ksys::phys

src/KingSystem/Physics/RigidBody/physRigidBodyFactory.h

@@ -13,6 +13,7 @@ class BoxWaterRigidBody;
 class CapsuleRigidBody;
 class CylinderRigidBody;
 class CylinderWaterRigidBody;
+class PolytopeRigidBody;
 class RigidBody;
 struct RigidBodyInstanceParam;
 
@@ -25,7 +26,7 @@ public:
                                                        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);
+    static PolytopeRigidBody* createPolytope(RigidBodyInstanceParam* params, sead::Heap* heap);
     static RigidBody* createCollection(RigidBodyInstanceParam* params, sead::Heap* heap);
 };