Start CollisionUtil

asm/Collision/CCollidableAABox.s

@@ -33,7 +33,7 @@ sTableIndex__16CCollidableAABox:
 .section .bss
 .balign 8
 
-.lcomm lbl_80479498, 0x48, 4
+.lcomm normalTable, 0x48, 4
 
 .section .text, "ax"
 
@@ -820,9 +820,9 @@ SetStaticTableIndex__16CCollidableAABoxFUi:
 .global __sinit_CCollidableAABox_cpp
 __sinit_CCollidableAABox_cpp:
 /* 802CE038 002CAF98  C0 42 C4 88 */	lfs f2, lbl_805AE1A8@sda21(r2)
-/* 802CE03C 002CAF9C  3C 60 80 48 */	lis r3, lbl_80479498@ha
+/* 802CE03C 002CAF9C  3C 60 80 48 */	lis r3, normalTable@ha
 /* 802CE040 002CAFA0  C0 22 C4 80 */	lfs f1, lbl_805AE1A0@sda21(r2)
-/* 802CE044 002CAFA4  D4 43 94 98 */	stfsu f2, lbl_80479498@l(r3)
+/* 802CE044 002CAFA4  D4 43 94 98 */	stfsu f2, normalTable@l(r3)
 /* 802CE048 002CAFA8  C0 02 C4 84 */	lfs f0, lbl_805AE1A4@sda21(r2)
 /* 802CE04C 002CAFAC  D0 23 00 04 */	stfs f1, 4(r3)
 /* 802CE050 002CAFB0  D0 23 00 08 */	stfs f1, 8(r3)

asm/Collision/CCollidableSphere.s

@@ -82,7 +82,7 @@ lbl_805A86F8:
 .section .bss
 .balign 8
 
-.lcomm lbl_80479558, 0x48, 4
+.lcomm normalTable, 0x48, 4
 
 .section .text, "ax"
 
@@ -1527,9 +1527,9 @@ SetStaticTableIndex__17CCollidableSphereFUi:
 .global __sinit_CCollidableSphere_cpp
 __sinit_CCollidableSphere_cpp:
 /* 802D52F0 002D2250  C0 42 C5 3C */	lfs f2, lbl_805AE25C@sda21(r2)
-/* 802D52F4 002D2254  3C 60 80 48 */	lis r3, lbl_80479558@ha
+/* 802D52F4 002D2254  3C 60 80 48 */	lis r3, normalTable@ha
 /* 802D52F8 002D2258  C0 22 C5 30 */	lfs f1, lbl_805AE250@sda21(r2)
-/* 802D52FC 002D225C  D4 43 95 58 */	stfsu f2, lbl_80479558@l(r3)
+/* 802D52FC 002D225C  D4 43 95 58 */	stfsu f2, normalTable@l(r3)
 /* 802D5300 002D2260  C0 02 C5 34 */	lfs f0, lbl_805AE254@sda21(r2)
 /* 802D5304 002D2264  D0 23 00 04 */	stfs f1, 4(r3)
 /* 802D5308 002D2268  D0 23 00 08 */	stfs f1, 8(r3)
@@ -1587,4 +1587,3 @@ lbl_803D6A90:
 	.asciz "CCollidableSphere"
 	.balign 4
 	.4byte 0
-

asm/Collision/CollisionUtil.s

@@ -7,7 +7,7 @@ lbl_ctor:
 .section .bss
 .balign 8
 
-.lcomm lbl_80479510, 0x48, 4
+.lcomm normalTable, 0x48, 4
 
 .section .text, "ax"
 
@@ -3358,11 +3358,11 @@ lbl_802D2BD0:
 /* 802D2CBC 002CFC1C  FC 02 08 40 */	fcmpo cr0, f2, f1
 /* 802D2CC0 002CFC20  4C 40 13 82 */	cror 2, 0, 2
 /* 802D2CC4 002CFC24  7C 00 00 26 */	mfcr r0
-/* 802D2CC8 002CFC28  3C 60 80 48 */	lis r3, lbl_80479510@ha
+/* 802D2CC8 002CFC28  3C 60 80 48 */	lis r3, normalTable@ha
 /* 802D2CCC 002CFC2C  54 00 2F 7A */	rlwinm r0, r0, 5, 0x1d, 0x1d
 /* 802D2CD0 002CFC30  3B 81 00 50 */	addi r28, r1, 0x50
 /* 802D2CD4 002CFC34  50 80 37 38 */	rlwimi r0, r4, 6, 0x1c, 0x1c
-/* 802D2CD8 002CFC38  3B 63 95 10 */	addi r27, r3, lbl_80479510@l
+/* 802D2CD8 002CFC38  3B 63 95 10 */	addi r27, r3, normalTable@l
 /* 802D2CDC 002CFC3C  50 A0 27 BC */	rlwimi r0, r5, 4, 0x1e, 0x1e
 /* 802D2CE0 002CFC40  3B 20 00 00 */	li r25, 0
 /* 802D2CE4 002CFC44  50 C0 1F FE */	rlwimi r0, r6, 3, 0x1f, 0x1f
@@ -3385,9 +3385,9 @@ lbl_802D2D18:
 /* 802D2D20 002CFC80  48 00 00 60 */	b lbl_802D2D80
 lbl_802D2D24:
 /* 802D2D24 002CFC84  C0 5B 00 0C */	lfs f2, 0xc(r27)
-/* 802D2D28 002CFC88  3C 60 80 48 */	lis r3, lbl_80479510@ha
+/* 802D2D28 002CFC88  3C 60 80 48 */	lis r3, normalTable@ha
 /* 802D2D2C 002CFC8C  C0 3B 00 10 */	lfs f1, 0x10(r27)
-/* 802D2D30 002CFC90  38 03 95 10 */	addi r0, r3, lbl_80479510@l
+/* 802D2D30 002CFC90  38 03 95 10 */	addi r0, r3, normalTable@l
 /* 802D2D34 002CFC94  C0 1B 00 14 */	lfs f0, 0x14(r27)
 /* 802D2D38 002CFC98  FC 40 10 50 */	fneg f2, f2
 /* 802D2D3C 002CFC9C  FC 20 08 50 */	fneg f1, f1
@@ -3438,9 +3438,9 @@ lbl_802D2DD0:
 /* 802D2DE8 002CFD48  80 1F 00 00 */	lwz r0, 0(r31)
 /* 802D2DEC 002CFD4C  2C 00 00 00 */	cmpwi r0, 0
 /* 802D2DF0 002CFD50  40 82 00 B4 */	bne lbl_802D2EA4
-/* 802D2DF4 002CFD54  3C 60 80 48 */	lis r3, lbl_80479510@ha
+/* 802D2DF4 002CFD54  3C 60 80 48 */	lis r3, normalTable@ha
 /* 802D2DF8 002CFD58  7F A5 EB 78 */	mr r5, r29
-/* 802D2DFC 002CFD5C  38 63 95 10 */	addi r3, r3, lbl_80479510@l
+/* 802D2DFC 002CFD5C  38 63 95 10 */	addi r3, r3, normalTable@l
 /* 802D2E00 002CFD60  7F C6 F3 78 */	mr r6, r30
 /* 802D2E04 002CFD64  C0 23 00 34 */	lfs f1, 0x34(r3)
 /* 802D2E08 002CFD68  38 E3 00 30 */	addi r7, r3, 0x30
@@ -3460,9 +3460,9 @@ lbl_802D2DD0:
 /* 802D2E40 002CFDA0  7F E3 FB 78 */	mr r3, r31
 /* 802D2E44 002CFDA4  38 A0 00 00 */	li r5, 0
 /* 802D2E48 002CFDA8  4B EB 27 D9 */	bl Add__18CCollisionInfoListFRC14CCollisionInfob
-/* 802D2E4C 002CFDAC  3C 60 80 48 */	lis r3, lbl_80479510@ha
+/* 802D2E4C 002CFDAC  3C 60 80 48 */	lis r3, normalTable@ha
 /* 802D2E50 002CFDB0  7F A5 EB 78 */	mr r5, r29
-/* 802D2E54 002CFDB4  38 63 95 10 */	addi r3, r3, lbl_80479510@l
+/* 802D2E54 002CFDB4  38 63 95 10 */	addi r3, r3, normalTable@l
 /* 802D2E58 002CFDB8  7F C6 F3 78 */	mr r6, r30
 /* 802D2E5C 002CFDBC  C0 23 00 40 */	lfs f1, 0x40(r3)
 /* 802D2E60 002CFDC0  38 E3 00 3C */	addi r7, r3, 0x3c
@@ -4593,9 +4593,9 @@ lbl_802D3DE0:
 .global __sinit_CollisionUtil_cpp
 __sinit_CollisionUtil_cpp:
 /* 802D3E14 002D0D74  C0 42 C4 C0 */	lfs f2, lbl_805AE1E0@sda21(r2)
-/* 802D3E18 002D0D78  3C 60 80 48 */	lis r3, lbl_80479510@ha
+/* 802D3E18 002D0D78  3C 60 80 48 */	lis r3, normalTable@ha
 /* 802D3E1C 002D0D7C  C0 22 C4 B8 */	lfs f1, lbl_805AE1D8@sda21(r2)
-/* 802D3E20 002D0D80  D4 43 95 10 */	stfsu f2, lbl_80479510@l(r3)
+/* 802D3E20 002D0D80  D4 43 95 10 */	stfsu f2, normalTable@l(r3)
 /* 802D3E24 002D0D84  C0 02 C4 BC */	lfs f0, lbl_805AE1DC@sda21(r2)
 /* 802D3E28 002D0D88  D0 23 00 04 */	stfs f1, 4(r3)
 /* 802D3E2C 002D0D8C  D0 23 00 08 */	stfs f1, 8(r3)
@@ -4775,4 +4775,3 @@ lbl_803D6A78:
 	.4byte 0
 	.4byte 0xBFF00000
 	.4byte 0
-

configure.py

@@ -447,13 +447,13 @@ LIBS = [
         "cflags": "$cflags_retro",
         "host": True,
         "objects": [
-            "Collision/CCollidableAABox",
+            ["Collision/CCollidableAABox", False],
             ["Collision/CCollidableCollisionSurface", True],
             ["Collision/CCollisionInfo", True],
             "Collision/InternalColliders",
             ["Collision/CCollisionPrimitive", False],
             ["Collision/CMaterialList", True],
-            "Collision/CollisionUtil",
+            ["Collision/CollisionUtil", False],
             "Collision/CCollidableSphere",
             ["Collision/CMaterialFilter", True],
             ["Collision/COBBox", False],

include/Collision/CollisionUtil.hpp

@@ -1,9 +1,22 @@
 #ifndef _COLLISIONUTIL
 #define _COLLISIONUTIL
 
+#include "Kyoto/Math/CVector3f.hpp"
+
 class CCollisionInfoList;
 class CVector3f;
 
+/* TODO: Figure out a better place for this, borks CBallFilter here, but is needed for CCollidableAABox/Sphere
+   (Best solution would be to find a Tardis and go back and slap the dev responsible for this)
+static const CVector3f normalTable[6] = {
+  CVector3f(-1.f, 0.f, 0.f),
+  CVector3f(1.f, 0.f, 0.f),
+  CVector3f(0.f, -1.f, 0.f),
+  CVector3f(0.f, 1.f, 0.f),
+  CVector3f(0.f, 0.f, -1.f),
+  CVector3f(0.f, 0.f, 1.f),
+};
+*/
 namespace CollisionUtil {
 
 void AddAverageToFront(const CCollisionInfoList& in, CCollisionInfoList& out);

include/Kyoto/Math/CSphere.hpp

@@ -8,12 +8,14 @@
 
 class CSphere {
 public:
-  CSphere(const CVector3f& pos, float radius) : x0_pos(pos), xc_radius(radius) {}
+  CSphere(const CVector3f& pos, float radius) : x0_center(pos), xc_radius(radius) {}
 
+  CVector3f GetCenter() const { return x0_center; }
+  float GetRadius() const { return xc_radius; }
   CUnitVector3f GetSurfaceNormal(const CVector3f& v) const;
 
 private:
-  CVector3f x0_pos;
+  CVector3f x0_center;
   float xc_radius;
 };
 CHECK_SIZEOF(CSphere, 0x10)

src/Collision/CCollidableAABox.cpp

@@ -0,0 +1,3 @@
+#include "Collision/CCollidableAABox.hpp"
+
+#include "Collision/CollisionUtil.hpp"

src/Collision/CollisionUtil.cpp

@@ -0,0 +1,53 @@
+#include "Collision/CollisionUtil.hpp"
+
+#include "Kyoto/Math/CMath.hpp"
+#include "Kyoto/Math/CPlane.hpp"
+#include "Kyoto/Math/CSphere.hpp"
+
+namespace CollisionUtil {
+bool RayPlaneIntersection(const CVector3f& from, const CVector3f& to, const CPlane& plane,
+                          CVector3f& point) {
+  CVector3f delta = to - from;
+  CUnitVector3f planeNorm = plane.GetNormal();
+  if (CMath::AbsF(CUnitVector3f::Dot(delta.AsNormalized(), planeNorm)) < 0.01f) {
+    return false;
+  }
+
+  float tmp = -plane.PointToPlaneDist(from) / CUnitVector3f::Dot(delta, planeNorm);
+
+  if (tmp < -0.f || tmp > 1.0001f) {
+    return false;
+  }
+
+  point = from + (delta * tmp);
+  return true;
+}
+
+bool RaySphereIntersection(const CSphere& sphere, const CVector3f& pos, const CVector3f& dir,
+                           float mag, float& T, CVector3f& point) {
+  const CVector3f rayToSphere = sphere.GetCenter() - pos;
+  const float magSq = rayToSphere.MagSquared();
+  const float dirDot = CVector3f::Dot(rayToSphere, dir);
+  const float radSq = sphere.GetRadius() * sphere.GetRadius();
+  float intersectSq = (dirDot * dirDot);
+  
+  if (dirDot < 0.f && magSq > radSq) {
+    return false;
+  }
+  intersectSq -= magSq;
+  intersectSq -= radSq;
+
+  if (intersectSq < 0.f) {
+    return false;
+  }
+  intersectSq = CMath::SqrtF(intersectSq);
+  T = magSq > radSq ? dirDot - intersectSq : dirDot + intersectSq;
+
+  if (T < mag || mag == 0.f) {
+    point = pos + T * dir;
+    return true;
+  }
+
+  return false;
+}
+} // namespace CollisionUtil

src/Kyoto/Math/CSphere.cpp

@@ -1,5 +1,5 @@
 #include "Kyoto/Math/CSphere.hpp"
 
 CUnitVector3f CSphere::GetSurfaceNormal(const CVector3f& vec) const {
-  return CUnitVector3f(vec - x0_pos);
+  return CUnitVector3f(vec - x0_center);
 }

src/MetroidPrime/Enemies/CMetaree.cpp

@@ -188,8 +188,8 @@ void CMetaree::Flee(CStateManager& mgr, EStateMsg msg, float) {
     break;
   }
   case kStateMsg_Update: {
-    switch (x5a8_) {
+    switch(x5a8_) {
-    case 0:
+      case 0:
       if (GetBodyCtrl()->GetBodyStateInfo().GetCurrentStateId() == pas::kAS_LieOnGround) {
         x5a8_ = 1;
       } else {
@@ -197,10 +197,10 @@ void CMetaree::Flee(CStateManager& mgr, EStateMsg msg, float) {
             CBCKnockDownCmd(CVector3f(0.f, 1.f, 0.f), pas::kS_Zero));
       }
       break;
-    default:
+      default:
       break;
     }
-
+    
     break;
   }
   case kStateMsg_Deactivate: