ksys/phys: Add BoxWaterShape

4 files changed, 251 insertions, 0 deletions

diff --git a/src/KingSystem/Physics/CMakeLists.txt b/src/KingSystem/Physics/CMakeLists.txt
index 77ab1141..8d235b74 100644
--- a/src/KingSystem/Physics/CMakeLists.txt
+++ b/src/KingSystem/Physics/CMakeLists.txt
@@ -42,6 +42,8 @@ target_sources(uking PRIVATE
 
   RigidBody/Shape/physBoxShape.cpp
   RigidBody/Shape/physBoxShape.h
+  RigidBody/Shape/physBoxWaterShape.cpp
+  RigidBody/Shape/physBoxWaterShape.h
   RigidBody/Shape/physBoxRigidBody.cpp
   RigidBody/Shape/physBoxRigidBody.h
   RigidBody/Shape/physCapsuleShape.cpp
diff --git a/src/KingSystem/Physics/RigidBody/Shape/physBoxWaterShape.cpp b/src/KingSystem/Physics/RigidBody/Shape/physBoxWaterShape.cpp
new file mode 100644
index 00000000..787d9801
--- /dev/null
+++ b/src/KingSystem/Physics/RigidBody/Shape/physBoxWaterShape.cpp
@@ -0,0 +1,194 @@
+#include "KingSystem/Physics/RigidBody/Shape/physBoxWaterShape.h"
+#include <Havok/Common/Base/Types/Geometry/Aabb/hkAabb.h>
+#include <Havok/Common/Base/Types/Geometry/Sphere/hkSphere.h>
+#include <Havok/Physics2012/Collide/Shape/HeightField/Plane/hkpPlaneShape.h>
+#include <Havok/Physics2012/Collide/Shape/Query/hkpShapeRayCastOutput.h>
+#include "KingSystem/Physics/RigidBody/Shape/physBoxShape.h"
+#include "KingSystem/Physics/physConversions.h"
+#include "KingSystem/Utils/HeapUtil.h"
+#include "KingSystem/Utils/SafeDelete.h"
+
+namespace ksys::phys {
+
+class HavokPlaneShape : public hkpPlaneShape {
+public:
+    HK_DECLARE_CLASS_ALLOCATOR(HavokPlaneShape)
+
+    HavokPlaneShape(const hkVector4& plane, const hkAabb& aabb) : hkpPlaneShape(plane, aabb) {}
+
+    hkBool castRay(const hkpShapeRayCastInput& input,
+                   hkpShapeRayCastOutput& results) const override {
+        const auto dot_from = getPlane().dot4xyz1(input.m_from);
+        const auto dot_to = getPlane().dot4xyz1(input.m_to);
+        if (dot_from.isGreaterZero() && dot_to.isGreaterZero())
+            return false;
+
+        hkSimdReal hit_fraction;
+        if (dot_from.isLessEqualZero())
+            hit_fraction = 0;
+        else
+            hit_fraction = dot_from * (dot_from - dot_to).reciprocal();
+
+        if (!hkSimdReal(results.m_hitFraction).isGreater(hit_fraction))
+            return false;
+
+        hkVector4f position;
+        position.setInterpolate(input.m_from, input.m_to, hit_fraction.val());
+
+        hkVector4f rel_pos;
+        rel_pos.setSub(position, getAabbCenter());
+        rel_pos.setAbs(rel_pos);
+        rel_pos.sub(0.0001);
+
+        if (!getAabbHalfExtents().greaterEqual(rel_pos).allAreSet<hkVector4fComparison::MASK_XYZ>())
+            return false;
+
+        results.m_hitFraction = hit_fraction;
+        results.m_normal = getPlane();
+        results.setKey(HK_INVALID_SHAPE_KEY);
+        return true;
+    }
+
+    void collideSpheres(const CollideSpheresInput& input,
+                        SphereCollisionOutput* outputArray) const override {
+        auto* sphere = input.m_spheres;
+        for (int i = input.m_numSpheres - 1; i >= 0; --i) {
+            const auto r =
+                getPlane().dot4xyz1(sphere->getPositionAndRadius()) - sphere->getRadiusSimdReal();
+
+            auto w = hkSimdReal::getConstant<HK_QUADREAL_MAX>();
+
+            if (r < input.m_tolerance) {
+                const auto r_ = sphere->getRadiusSimdReal() + hkSimdReal(input.m_tolerance);
+
+                hkVector4f half_extents;
+                half_extents.setAdd(getAabbHalfExtents(), r_);
+
+                hkVector4f rel_pos;
+                rel_pos.setSub(sphere->getPositionAndRadius(), getAabbCenter());
+                rel_pos.setAbs(rel_pos);
+
+                if (!rel_pos.greater(half_extents).anyIsSet(hkVector4fComparison::MASK_XYZ))
+                    w = r;
+            }
+
+            outputArray->setXYZ_W(getPlane(), w);
+            ++sphere;
+            ++outputArray;
+        }
+    }
+};
+
+BoxWaterShape* BoxWaterShape::make(const BoxShapeParam& param, sead::Heap* heap) {
+    void* storage = util::allocStorage<HavokPlaneShape>(heap);
+    if (!storage)
+        return nullptr;
+
+    hkVector4f plane(0, 1, 0);
+    plane.setW(param.translate.y + param.extents.y / 2);
+
+    hkAabb aabb;
+    aabb.m_min.setSub(toHkVec4(param.translate), toHkVec4(param.extents / 2));
+    aabb.m_max.setAdd(toHkVec4(param.translate), toHkVec4(param.extents / 2));
+
+    auto* hk_shape = new (storage) HavokPlaneShape(plane, aabb);
+    return new (heap) BoxWaterShape(param, hk_shape);
+}
+
+BoxWaterShape* BoxWaterShape::clone(sead::Heap* heap) const {
+    BoxShapeParam param;
+    param.extents = mExtents;
+    param.translate = mTranslate;
+    param.rotate = mRotate;
+
+    auto* cloned = make(param, heap);
+    cloned->setMaterialMask(mMaterialMask);
+    return cloned;
+}
+
+BoxWaterShape::BoxWaterShape(const BoxShapeParam& param, hkpPlaneShape* hk_shape)
+    : mExtents(param.extents), mTranslate(param.translate), mRotate(param.rotate),
+      mHavokShape(hk_shape), mMaterialMask(param.common.getMaterialMask()) {
+    mFlags.change(Flag::IsWater, param.common.material == Material::Water);
+
+    const bool no_transform =
+        mTranslate == sead::Vector3f(0, 0, 0) && mRotate == sead::Vector3f(0, 0, 0);
+    mFlags.change(Flag::HasTransform, !no_transform);
+
+    if (param.common.item_code_disable_stick)
+        mMaterialMask.getData().setCustomFlag(MaterialMaskData::CustomFlag::_0);
+
+    setMaterialMask(mMaterialMask);
+}
+
+BoxWaterShape::~BoxWaterShape() {
+    util::deallocateObjectUnsafe(mHavokShape);
+}
+
+void BoxWaterShape::setMaterialMask(const MaterialMask& mask) {
+    mMaterialMask = mask;
+
+    if (mHavokShape)
+        mHavokShape->setUserData(mask.getRawData());
+}
+
+bool BoxWaterShape::setExtents(const sead::Vector3f& extents) {
+    if (mExtents == extents)
+        return false;
+
+    mExtents = extents;
+    mFlags.set(Flag::Dirty);
+    return true;
+}
+
+bool BoxWaterShape::setTranslate(const sead::Vector3f& translate) {
+    if (mTranslate == translate)
+        return false;
+
+    mTranslate = translate;
+
+    mFlags.change(Flag::HasTransform, !hasNoTransform());
+    mFlags.set(Flag::Dirty);
+    return true;
+}
+
+float BoxWaterShape::getVolume() const {
+    return mExtents.x * mExtents.y * mExtents.z;
+}
+
+hkpShape* BoxWaterShape::getHavokShape() {
+    return mHavokShape;
+}
+
+const hkpShape* BoxWaterShape::getHavokShape() const {
+    return mHavokShape;
+}
+
+const hkpShape* BoxWaterShape::updateHavokShape() {
+    if (!mFlags.isOn(Flag::Dirty))
+        return nullptr;
+
+    const auto ref_count = mHavokShape->getReferenceCount();
+
+    hkVector4f plane(0, 1, 0);
+    plane.setW(mTranslate.y + mExtents.y / 2);
+
+    hkAabb aabb;
+    aabb.m_min.setSub(toHkVec4(mTranslate), toHkVec4(mExtents / 2));
+    aabb.m_max.setAdd(toHkVec4(mTranslate), toHkVec4(mExtents / 2));
+
+    mHavokShape = new (mHavokShape) HavokPlaneShape(plane, aabb);
+    mHavokShape->setReferenceCount(ref_count);
+
+    setMaterialMask(mMaterialMask);
+
+    mFlags.reset(Flag::Dirty);
+    return nullptr;
+}
+
+void BoxWaterShape::setScale(float scale) {
+    setExtents(mExtents * scale);
+    setTranslate(mTranslate * scale);
+}
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/RigidBody/Shape/physBoxWaterShape.h b/src/KingSystem/Physics/RigidBody/Shape/physBoxWaterShape.h
new file mode 100644
index 00000000..1512bb5a
--- /dev/null
+++ b/src/KingSystem/Physics/RigidBody/Shape/physBoxWaterShape.h
@@ -0,0 +1,54 @@
+#pragma once
+
+#include <math/seadVector.h>
+#include <prim/seadTypedBitFlag.h>
+#include <thread/seadAtomic.h>
+#include "KingSystem/Physics/RigidBody/Shape/physShape.h"
+#include "KingSystem/Physics/System/physMaterialMask.h"
+
+class hkpPlaneShape;
+
+namespace ksys::phys {
+
+struct BoxShapeParam;
+
+class BoxWaterShape : public Shape {
+    SEAD_RTTI_OVERRIDE(BoxWaterShape, Shape)
+public:
+    enum class Flag {
+        IsWater = 1 << 0,
+        Dirty = 1 << 1,
+        /// Whether we have a transform (translation and/or rotation).
+        HasTransform = 1 << 2,
+    };
+
+    static BoxWaterShape* make(const BoxShapeParam& param, sead::Heap* heap);
+    BoxWaterShape* clone(sead::Heap* heap) const;
+
+    BoxWaterShape(const BoxShapeParam& param, hkpPlaneShape* hk_shape);
+    ~BoxWaterShape() override;
+
+    void setMaterialMask(const MaterialMask& mask);
+    bool setExtents(const sead::Vector3f& extents);
+    bool setTranslate(const sead::Vector3f& translate);
+
+    float getVolume() const override;
+    hkpShape* getHavokShape() override;
+    const hkpShape* getHavokShape() const override;
+    const hkpShape* updateHavokShape() override;
+    void setScale(float scale) override;
+    ShapeType getType() const override { return ShapeType::BoxWater; }
+
+    bool hasNoTransform() const {
+        return mTranslate == sead::Vector3f(0, 0, 0) && mRotate == sead::Vector3f(0, 0, 0);
+    }
+
+    sead::Vector3f mExtents;
+    sead::TypedBitFlag<Flag, sead::Atomic<u32>> mFlags;
+    sead::Vector3f mTranslate;
+    sead::Vector3f mRotate;
+    hkpPlaneShape* mHavokShape{};
+    MaterialMask mMaterialMask{};
+};
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/RigidBody/Shape/physShape.h b/src/KingSystem/Physics/RigidBody/Shape/physShape.h
index f1c47a66..0cd660f5 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/physShape.h
+++ b/src/KingSystem/Physics/RigidBody/Shape/physShape.h
@@ -16,6 +16,7 @@ enum class ShapeType {
     Cylinder = 3,
     Polytope = 4,
     CharacterPrism = 6,
+    BoxWater = 7,
     Unknown = -1,
 };