ksys/phys: Add most remaining parts of RayCast

One last function remaining (0x0000007100fc4844) but it looks obnoxious
so I'll leave it for later.

7 files changed, 177 insertions, 33 deletions

diff --git a/src/KingSystem/Physics/CMakeLists.txt b/src/KingSystem/Physics/CMakeLists.txt
index 8abe2816..a3e2aaf1 100644
--- a/src/KingSystem/Physics/CMakeLists.txt
+++ b/src/KingSystem/Physics/CMakeLists.txt
@@ -96,6 +96,8 @@ target_sources(uking PRIVATE
   StaticCompound/physStaticCompoundBodyGroup.h
   StaticCompound/physStaticCompoundInfo.cpp
   StaticCompound/physStaticCompoundInfo.h
+  StaticCompound/physStaticCompoundUtil.cpp
+  StaticCompound/physStaticCompoundUtil.h
 
   SupportBone/physSupportBoneParam.cpp
   SupportBone/physSupportBoneParam.h
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyFromResource.cpp b/src/KingSystem/Physics/RigidBody/physRigidBodyFromResource.cpp
index e828d98f..397cee6b 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodyFromResource.cpp
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyFromResource.cpp
@@ -5,6 +5,7 @@
 #include <Havok/Physics2012/Utilities/Dynamics/ScaleSystem/hkpSystemScalingUtility.h>
 #include <math/seadMathCalcCommon.h>
 #include "KingSystem/Physics/RigidBody/physRigidBodyParam.h"
+#include "KingSystem/Physics/StaticCompound/physStaticCompoundUtil.h"
 #include "KingSystem/Physics/physConversions.h"
 #include "KingSystem/Physics/physMaterialMask.h"
 
@@ -81,18 +82,13 @@ bool RigidBodyFromResource::isMaterial(Material material) const {
     return found_child_shape_with_material;
 }
 
-// FIXME: move this to the appropriate header
-// 0x0000007100fd0a1c
-u32 getCollisionFilterInfoFromCollidable(u32* material_mask, u32* collision_filter_info,
-                                         const hkpCollidable* collidable, const u32* unk);
-
 u32 RigidBodyFromResource::getCollisionMasks(RigidBody::CollisionMasks* masks, const u32* unk,
                                              const sead::Vector3f& contact_point) {
     masks->ignored_layers = ~mContactMask;
     auto* collidable = getHkBody()->getCollidable();
     if (unk != nullptr) {
-        return getCollisionFilterInfoFromCollidable(&masks->material_mask,
-                                                    &masks->collision_filter_info, collidable, unk);
+        return getCollisionFilterInfoFromCollidable(masks, &masks->collision_filter_info,
+                                                    *collidable, unk);
     }
     masks->material_mask = collidable->getShape()->getUserData();
     masks->collision_filter_info = collidable->getCollisionFilterInfo();
diff --git a/src/KingSystem/Physics/StaticCompound/physStaticCompoundUtil.cpp b/src/KingSystem/Physics/StaticCompound/physStaticCompoundUtil.cpp
new file mode 100644
index 00000000..72d1bcbb
--- /dev/null
+++ b/src/KingSystem/Physics/StaticCompound/physStaticCompoundUtil.cpp
@@ -0,0 +1 @@
+#include "KingSystem/Physics/StaticCompound/physStaticCompoundUtil.h"
diff --git a/src/KingSystem/Physics/StaticCompound/physStaticCompoundUtil.h b/src/KingSystem/Physics/StaticCompound/physStaticCompoundUtil.h
new file mode 100644
index 00000000..0aef6607
--- /dev/null
+++ b/src/KingSystem/Physics/StaticCompound/physStaticCompoundUtil.h
@@ -0,0 +1,35 @@
+#pragma once
+
+#include <basis/seadTypes.h>
+#include <math/seadVector.h>
+
+class hkpCollidable;
+class hkpShape;
+
+namespace ksys::map {
+class Object;
+}
+
+namespace ksys::phys {
+
+class BodyGroup;
+struct RigidBodyCollisionMasks;
+
+// 0x0000007100fd0810
+u32 getMaterialMaskFromShape(const hkpShape& shape, const u32* shape_key,
+                             const sead::Vector3f& position);
+
+// 0x0000007100fd086c
+bool getMaterialMaskFromCollidable(RigidBodyCollisionMasks* p_masks, u32* p_collision_filter_info,
+                                   const hkpShape& shape, const u32* shape_key);
+
+// 0x0000007100fd09d0
+void getBodyGroupAndObjectFromSCShape(BodyGroup** p_body_group, map::Object** p_object,
+                                      const hkpShape& shape, const u32* shape_key);
+
+// 0x0000007100fd0a1c
+u32 getCollisionFilterInfoFromCollidable(RigidBodyCollisionMasks* p_masks,
+                                         u32* p_collision_filter_info,
+                                         const hkpCollidable& collidable, const u32* shape_key);
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/System/physRayCast.cpp b/src/KingSystem/Physics/System/physRayCast.cpp
index 0983d15f..338bfe12 100644
--- a/src/KingSystem/Physics/System/physRayCast.cpp
+++ b/src/KingSystem/Physics/System/physRayCast.cpp
@@ -9,11 +9,15 @@
 #include <Havok/Physics2012/Collide/Shape/Query/hkpShapeRayCastInput.h>
 #include <Havok/Physics2012/Dynamics/Entity/hkpRigidBody.h>
 #include <Havok/Physics2012/Dynamics/World/hkpWorld.h>
+#include <Havok/Physics2012/Internal/Collide/BvCompressedMesh/hkpBvCompressedMeshShape.h>
 #include <Havok/Physics2012/Internal/Collide/StaticCompound/hkpStaticCompoundShape.h>
+#include <Havok/Physics2012/Utilities/Collide/ShapeUtils/ShapeKeyPath/hkpShapeKeyPath.h>
 #include <prim/seadScopeGuard.h>
 #include "KingSystem/Physics/RigidBody/physRigidBody.h"
+#include "KingSystem/Physics/StaticCompound/physStaticCompoundUtil.h"
 #include "KingSystem/Physics/System/physEntityContactListener.h"
 #include "KingSystem/Physics/System/physGroupFilter.h"
+#include "KingSystem/Physics/System/physPhantom.h"
 #include "KingSystem/Physics/System/physSystem.h"
 #include "KingSystem/Physics/physConversions.h"
 #include "KingSystem/Physics/physLayerMaskBuilder.h"
@@ -95,8 +99,8 @@ void RayCast::resetCastResult() {
     mHitCollidable = {};
     mHitShapeKey = {};
     mHasHitSpecifiedRigidBody = false;
-    _58 = {};
-    _60 = {};
+    mHitBodyGroup = {};
+    mHitMapObject = {};
     _70 = {};
 }
 
@@ -232,7 +236,7 @@ bool RayCast::postCast(const hkpWorldRayCastOutput& output) {
     mHasHit = output.hasHit();
     if (mHasHit) {
         updateHitInformation(output);
-        getActorInfoMaybe(output);
+        updateStaticCompoundObjectInfo(output);
     }
     _98 = false;
     _70 = 1;
@@ -337,14 +341,113 @@ void RayCast::shapeRayCastImpl(hkpWorldRayCastOutput* output, RigidBody* body) {
     }
 }
 
+bool RayCast::phantomRayCast(Phantom* phantom) {
+    hkpWorldRayCastOutput output;
+    preCast();
+    phantomRayCastImpl(&output, phantom);
+    return postCast(output);
+}
+
+void RayCast::phantomRayCastImpl(hkpWorldRayCastOutput* output, Phantom* phantom) {
+    hkpWorldRayCastInput input;
+    auto layer_type = phantom->getLayerType();
+    fillCastInput(input, layer_type);
+
+    ScopedWorldLock lock{layer_type, "raycast", 0, OnlyLockIfNeeded::Yes};
+
+    if (mNormalCheckingMode == NormalCheckingMode::DoNotCheck) {
+        if (mIgnoredGroups.size() > 0 || int(mIgnoredGroundHit) != GroundHit::Ignore) {
+            RayHitCollector collector{output, layer_type,
+                                      mIgnoredGroups.size() > 0 ? &mIgnoredGroups : nullptr,
+                                      mIgnoredGroundHit};
+            phantom->getHavokPhantom()->castRay(input, collector);
+        } else {
+            phantom->getHavokPhantom()->castRay(input, *output);
+        }
+
+    } else {
+        sead::Vector3f ray_line = mTo - mFrom;
+        ray_line.normalize();
+
+        auto* groups = mIgnoredGroups.size() > 0 ? &mIgnoredGroups : nullptr;
+        NormalCheckingRayHitCollector collector(output, ray_line, mNormalCheckingMode, layer_type,
+                                                groups, mIgnoredGroundHit);
+        phantom->getHavokPhantom()->castRay(input, collector);
+    }
+}
+
+void RayCast::updateStaticCompoundObjectInfo(const hkpWorldRayCastOutput& output) {
+    hkpShapeKeyPath path{output};
+    auto iterator = path.getIterator();
+
+    sead::Vector3f hit_position;
+    getHitPosition(&hit_position);
+
+    if (iterator.isValid()) {
+        const auto& shape = *iterator.getShape();
+        const u32 raw_material = getMaterialMaskFromShape(shape, output.m_shapeKeys, hit_position);
+
+        mMaterialMask.set(raw_material);
+        if (raw_material == u32(-1))
+            mMaterialMask.reset();
+
+        if (_99) {
+            getBodyGroupAndObjectFromSCShape(&mHitBodyGroup, &mHitMapObject, shape,
+                                             output.m_shapeKeys);
+        }
+    } else {
+        mMaterialMask.reset();
+    }
+}
+
 void RayCast::getHitPosition(sead::Vector3f* position) const {
     *position = ((1 - mHitFraction) * mFrom) + (mHitFraction * mTo);
 }
 
+static void calculateTriangleNormal(sead::Vector3f* normal, const hkpTriangleShape* triangle) {
+    const auto va = toVec3(triangle->getVertex<0>());
+    const auto vb = toVec3(triangle->getVertex<1>());
+    const auto vc = toVec3(triangle->getVertex<2>());
+    normal->setCross(vb - va, vc - va);
+}
+
+bool RayCast::getHitTriangleNormal(sead::Vector3f* normal, const hkpShape* hit_shape,
+                                   u32 shape_key) const {
+    if (hit_shape->getType() != hkcdShapeType::STATIC_COMPOUND) {
+        if (hit_shape->getType() != hkcdShapeType::BV_COMPRESSED_MESH)
+            return false;
+
+        auto* bv_mesh = static_cast<const hkpBvCompressedMeshShape*>(hit_shape);
+
+        hkpShapeBuffer buffer;
+        hit_shape = bv_mesh->getChildShape(shape_key, buffer);
+        if (hit_shape->getType() != hkcdShapeType::TRIANGLE)
+            return false;
+
+        calculateTriangleNormal(normal, static_cast<const hkpTriangleShape*>(hit_shape));
+        return true;
+    }
+
+    auto* sc = static_cast<const hkpStaticCompoundShape*>(hit_shape);
+    int instance_id;
+    sc->decomposeShapeKey(shape_key, instance_id, shape_key);
+    return instance_id >= 0 &&
+           getHitTriangleNormal(normal, sc->getInstances()[instance_id].getShape(), shape_key);
+}
+
 void RayCast::getHitNormal(sead::Vector3f* normal) const {
     *normal = mHitNormal;
 }
 
+bool RayCast::getHitTriangleNormal(sead::Vector3f* normal) const {
+    if (!mHasHit)
+        return false;
+
+    const u32 shape_key = mHitShapeKey;
+    const hkpShape* shape = mHitCollidable->getShape();
+    return getHitTriangleNormal(normal, shape, shape_key);
+}
+
 RayHitCollector::~RayHitCollector() = default;
 
 bool RayHitCollector::isIgnoredGroup(const hkpCdBody& cdBody,
@@ -471,9 +574,9 @@ bool NormalCheckingRayHitCollector::checkNormal(
     hkVector4f CminusA;
     CminusA.setSub(vertexC, vertexA);
 
-    hkVector4f triangle_line;
-    triangle_line.setCross(BminusA, CminusA);
-    triangle_line.normalize<3>();
+    hkVector4f triangle_normal;
+    triangle_normal.setCross(BminusA, CminusA);
+    triangle_normal.normalize<3>();
 
     const auto body_rotation = cdBody.getRootCollidable()->getTransform().getRotation();
 
@@ -495,7 +598,7 @@ bool NormalCheckingRayHitCollector::checkNormal(
     hkVector4f rotated_normal;
     rotated_normal.setRotatedDir(rotation, hitInfo.m_normal);
 
-    return checkDot<true>(mMode, triangle_line.dot<3>(rotated_normal));
+    return checkDot<true>(mMode, triangle_normal.dot<3>(rotated_normal));
 }
 
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/System/physRayCast.h b/src/KingSystem/Physics/System/physRayCast.h
index bd11a64f..2557e957 100644
--- a/src/KingSystem/Physics/System/physRayCast.h
+++ b/src/KingSystem/Physics/System/physRayCast.h
@@ -10,15 +10,22 @@
 #include "KingSystem/Physics/physMaterialMask.h"
 
 class hkpCollidable;
+class hkpShape;
 struct hkpShapeRayCastInput;
 struct hkpShapeRayCastOutput;
 struct hkpWorldRayCastInput;
 struct hkpWorldRayCastOutput;
 
+namespace ksys::map {
+class Object;
+}
+
 namespace ksys::phys {
 
 struct ActorInfo;
+class BodyGroup;
 class LayerMaskBuilder;
+class Phantom;
 class RigidBody;
 class SystemGroupHandler;
 
@@ -70,17 +77,16 @@ public:
 
     bool worldRayCast(ContactLayerType layer_type);
     bool shapeRayCast(RigidBody* rigid_body);
-    // 0x0000007100fc4248
-    bool phantomRayCast(void* unk);
+    bool phantomRayCast(Phantom* phantom);
 
     void getHitPosition(sead::Vector3f* position) const;
+    bool getHitTriangleNormal(sead::Vector3f* normal, const hkpShape* hit_shape,
+                              u32 shape_key) const;
     void getHitNormal(sead::Vector3f* normal) const;
     // TODO: rename
     // 0x0000007100fc4844
-    void getUnkVectors(sead::Vector3f* unk1, sead::Vector3f* unk2, void* unk3) const;
-
-    // 0x0000007100fc4bd4
-    bool x_1(sead::Vector3f* out) const;
+    void getUnkVectors(sead::Vector3f* unk1, sead::Vector3f* unk2, sead::Vector3f* unk3) const;
+    bool getHitTriangleNormal(sead::Vector3f* normal) const;
 
 protected:
     auto& getLayerMask(ContactLayerType type) { return mLayerMasks[int(type)]; }
@@ -88,14 +94,9 @@ protected:
 
     void worldRayCastImpl(hkpWorldRayCastOutput* output, ContactLayerType layer_type);
     void shapeRayCastImpl(hkpWorldRayCastOutput* output, RigidBody* body);
-    // 0x0000007100fc43b0
-    void phantomRayCastImpl(hkpWorldRayCastOutput* output);
-
-    // 0x0000007100fc4630
-    const ActorInfo* getActorInfoMaybe(const hkpShapeRayCastOutput& output);
+    void phantomRayCastImpl(hkpWorldRayCastOutput* output, Phantom* phantom);
 
-    // 0x0000007100fc4764
-    bool x_2(sead::Vector3f* out, void* unk1, int unk2) const;
+    void updateStaticCompoundObjectInfo(const hkpWorldRayCastOutput& output);
 
     void fillCastInput(hkpWorldRayCastInput& input, ContactLayerType layer_type);
     void fillCastInput(hkpShapeRayCastInput& input, ContactLayerType layer_type);
@@ -116,8 +117,8 @@ protected:
     const hkpCollidable* mHitCollidable;
     u32 mHitShapeKey;
     bool mHasHitSpecifiedRigidBody;
-    void* _58{};
-    void* _60;
+    BodyGroup* mHitBodyGroup{};
+    map::Object* mHitMapObject;
     sead::SafeArray<sead::BitFlag32, NumContactLayerTypes> mLayerMasks{};
     sead::Atomic<u32> _70;
     NormalCheckingMode mNormalCheckingMode;
diff --git a/src/KingSystem/Physics/physMaterialMask.h b/src/KingSystem/Physics/physMaterialMask.h
index 69e72c11..ba4bedba 100644
--- a/src/KingSystem/Physics/physMaterialMask.h
+++ b/src/KingSystem/Physics/physMaterialMask.h
@@ -68,14 +68,12 @@ public:
     virtual ~MaterialMask();
 
     MaterialMask& operator=(const MaterialMask& other) {
-        mData = other.mData;
-        mSubMaterialNameCache = nullptr;
+        set(other.mData);
         return *this;
     }
 
     MaterialMask& operator=(MaterialMaskData data) {
-        mData = data;
-        mSubMaterialNameCache = nullptr;
+        set(data);
         return *this;
     }
 
@@ -91,6 +89,14 @@ public:
     const char* getMaterialName() const;
     const char* getSubMaterialName() const;
 
+    void set(MaterialMaskData data) {
+        mData = data;
+        mSubMaterialNameCache = nullptr;
+    }
+
+    void set(u32 data) { set(MaterialMaskData{data}); }
+    void reset() { set(0); }
+
     void setMaterial(Material mat);
 
     static int getSubMaterialIdx(Material mat, const sead::SafeString& submat_name);