ksys/phys: Start implementing HavokCylinderWaterShape

data/uking_functions.csv

@@ -83659,12 +83659,12 @@ Address,Quality,Size,Name
 0x0000007100fae4dc,O,000204,_ZNK4ksys4phys18CylinderWaterShape27checkDerivedRuntimeTypeInfoEPKN4sead15RuntimeTypeInfo9InterfaceE
 0x0000007100fae5a8,O,000092,_ZNK4ksys4phys18CylinderWaterShape18getRuntimeTypeInfoEv
 0x0000007100fae604,O,000008,_ZNK4ksys4phys18CylinderWaterShape7getTypeEv
-0x0000007100fae60c,U,000084,
-0x0000007100fae660,U,000140,
+0x0000007100fae60c,O,000084,_ZN4ksys4phys23HavokCylinderWaterShapeD0Ev
+0x0000007100fae660,O,000140,_ZNK4ksys4phys23HavokCylinderWaterShape7getAabbERK12hkTransformffR6hkAabb
 0x0000007100fae6ec,U,001508,
-0x0000007100faecd0,U,000152,
+0x0000007100faecd0,O,000152,_ZNK4ksys4phys23HavokCylinderWaterShape20castRayWithCollectorERK20hkpShapeRayCastInputRK9hkpCdBodyR18hkpRayHitCollector
 0x0000007100faed68,U,000316,
-0x0000007100faeea4,U,000004,nullsub_4238
+0x0000007100faeea4,O,000004,_ZNK4ksys4phys23HavokCylinderWaterShape10castSphereERKN19hkpHeightFieldShape18hkpSphereCastInputERK9hkpCdBodyR18hkpRayHitCollector
 0x0000007100faeea8,U,000276,
 0x0000007100faefbc,U,000520,
 0x0000007100faf1c4,U,000480,

lib/hkStubs/CMakeLists.txt

@@ -63,6 +63,7 @@ add_library(hkStubs OBJECT
   Havok/Common/Base/Types/hkRefPtr.h
   Havok/Common/Base/Types/hkRefVariant.h
   Havok/Common/Base/Types/Geometry/Aabb/hkAabb.h
+  Havok/Common/Base/Types/Geometry/Aabb/hkAabbUtil.h
   Havok/Common/Base/Types/Geometry/Sphere/hkSphere.h
   Havok/Common/Base/Types/Physics/hkStepInfo.h
   Havok/Common/Base/Types/Physics/ContactPoint/hkContactPointMaterial.h
@@ -103,6 +104,7 @@ add_library(hkStubs OBJECT
   Havok/Physics2012/Collide/Filter/hkpShapeCollectionFilter.h
   Havok/Physics2012/Collide/Filter/Group/hkpGroupFilter.cpp
   Havok/Physics2012/Collide/Filter/Group/hkpGroupFilter.h
+  Havok/Physics2012/Collide/Query/hkpRayHitCollector.h
   Havok/Physics2012/Collide/Query/CastUtil/hkpWorldRayCastInput.h
   Havok/Physics2012/Collide/Shape/hkpShape.h
   Havok/Physics2012/Collide/Shape/hkpShapeBase.h

lib/hkStubs/Havok/Common/Base/Types/Geometry/Aabb/hkAabbUtil.h

@@ -0,0 +1,36 @@
+#pragma once
+
+#include <Havok/Common/Base/Types/Geometry/Aabb/hkAabb.h>
+
+namespace hkAabbUtil {
+
+void calcAabb(const hkTransform& localToWorld, const hkVector4& halfExtents,
+              hkSimdRealParameter extraRadius, hkAabb& aabbOut);
+
+inline void calcAabb(const hkTransform& localToWorld, const hkVector4& halfExtents,
+                     hkSimdRealParameter extraRadius, hkAabb& aabbOut) {
+    hkVector4 x, y, z;
+    x.setMul(halfExtents.getComponent<0>(), localToWorld.getRotation().getColumn<0>());
+    y.setMul(halfExtents.getComponent<1>(), localToWorld.getRotation().getColumn<1>());
+    z.setMul(halfExtents.getComponent<2>(), localToWorld.getRotation().getColumn<2>());
+
+    x.setAbs(x);
+    y.setAbs(y);
+    z.setAbs(z);
+
+    hkVector4 max;
+    z.setAdd(z, extraRadius);
+    max.setAdd(x, y);
+    max.add(z);
+
+    hkVector4 min;
+    min.setNeg<4>(max);
+
+    max.add(localToWorld.getTranslation());
+    min.add(localToWorld.getTranslation());
+
+    aabbOut.m_max = max;
+    aabbOut.m_min = min;
+}
+
+}  // namespace hkAabbUtil

lib/hkStubs/Havok/Physics2012/Collide/Query/hkpRayHitCollector.h

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <Havok/Common/Base/hkBase.h>
+#include <Havok/Physics2012/Collide/Shape/hkpShapeBase.h>
+
+class hkpCdBody;
+struct hkpShapeRayCastCollectorOutput;
+
+class hkpRayHitCollector {
+public:
+    inline hkpRayHitCollector();
+
+    virtual void addRayHit(const hkpCdBody& cdBody,
+                           const hkpShapeRayCastCollectorOutput& hitInfo) = 0;
+
+    inline void reset();
+
+    virtual ~hkpRayHitCollector() = default;
+
+    static int shapeKeysFromCdBody(hkpShapeKey* buf, int maxKeys, const hkpCdBody& body);
+
+    hkReal m_earlyOutHitFraction;
+};
+
+inline hkpRayHitCollector::hkpRayHitCollector() {
+    reset();
+}
+
+inline void hkpRayHitCollector::reset() {
+    m_earlyOutHitFraction = 1.f;
+}

src/KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterShape.cpp

@@ -1,10 +1,29 @@
 #include "KingSystem/Physics/RigidBody/Shape/CylinderWater/physCylinderWaterShape.h"
+#include <Havok/Common/Base/Types/Geometry/Aabb/hkAabb.h>
+#include <Havok/Common/Base/Types/Geometry/Aabb/hkAabbUtil.h>
+#include <Havok/Common/Base/Types/Geometry/Sphere/hkSphere.h>
+#include <Havok/Physics2012/Collide/Agent/Collidable/hkpCdBody.h>
+#include <Havok/Physics2012/Collide/Query/hkpRayHitCollector.h>
 #include <Havok/Physics2012/Collide/Shape/HeightField/hkpHeightFieldShape.h>
+#include <Havok/Physics2012/Collide/Shape/Query/hkpShapeRayCastOutput.h>
 #include "KingSystem/Utils/HeapUtil.h"
 #include "KingSystem/Utils/SafeDelete.h"
 
 namespace ksys::phys {
 
+static void addAabb(const hkTransform localToWorld, const hkVector4& halfExtents, hkAabb& aabbOut) {
+    hkVector4f x, y, z;
+    x.setMul(halfExtents.getComponent<0>(), localToWorld.getRotation().getColumn<0>());
+    y.setMul(halfExtents.getComponent<1>(), localToWorld.getRotation().getColumn<1>());
+    z.setMul(halfExtents.getComponent<2>(), localToWorld.getRotation().getColumn<2>());
+
+    hkVector4f max;
+    max.setAdd(x, y);
+    max.add(z);
+    aabbOut.m_min.add(max);
+    aabbOut.m_max.add(max);
+}
+
 class alignas(16) HavokCylinderWaterShape : public hkpHeightFieldShape {
 public:
     HK_DECLARE_CLASS_ALLOCATOR(HavokCylinderWaterShape)
@@ -13,15 +32,46 @@ public:
 
     void setRadius(float radius) { m_radius = radius; }
 
-    void getAabb(const hkTransform& localToWorld, hkReal tolerance, hkAabb& aabbOut) const override;
+    void getAabb(const hkTransform& localToWorld, hkReal tolerance,
+                 hkAabb& aabbOut) const override {
+        hkVector4 half_extents(m_height / 2, m_radius / 2, m_radius / 2);
+        hkAabbUtil::calcAabb(localToWorld, half_extents, tolerance, aabbOut);
 
-    hkBool castRay(const hkpShapeRayCastInput& input, hkpShapeRayCastOutput& output) const override;
+        hkVector4 half_extents2(0, -m_radius / 2, m_radius / 2);
+        addAabb(localToWorld, half_extents2, aabbOut);
+    }
+
+    hkBool castRay(const hkpShapeRayCastInput& input,
+                   hkpShapeRayCastOutput& output) const override {
+        // FIXME: implement
+        return false;
+    }
 
     void castRayWithCollector(const hkpShapeRayCastInput& input, const hkpCdBody& cdBody,
-                              hkpRayHitCollector& collector) const override;
+                              hkpRayHitCollector& collector) const override {
+        hkpShapeRayCastOutput output;
+        output.m_hitFraction = collector.m_earlyOutHitFraction;
+        if (castRay(input, output)) {
+            output.m_normal._setRotatedDir(cdBody.getTransform().getRotation(), output.m_normal);
+            collector.addRayHit(cdBody, output);
+        }
+    }
 
     void collideSpheres(const CollideSpheresInput& input,
-                        SphereCollisionOutput* outputArray) const override;
+                        SphereCollisionOutput* outputArray) const override {
+        auto* sphere = input.m_spheres;
+        const hkSphere dummy({0, 1, 0}, hkSimdReal::getConstant<HK_QUADREAL_MAX>());
+
+        for (int i = input.m_numSpheres - 1; i >= 0; --i) {
+            hkSphere out = dummy;
+
+            // FIXME: implement the rest of this function
+
+            *outputArray = out.getPositionAndRadius();
+            ++sphere;
+            ++outputArray;
+        }
+    }
 
     void castSphere(const hkpSphereCastInput& input, const hkpCdBody& cdBody,
                     hkpRayHitCollector& collector) const override {}