diff --git a/configure.py b/configure.py
index 20330dfac..808a00213 100755
--- a/configure.py
+++ b/configure.py
@@ -493,7 +493,7 @@ config.libs = [
             Object(Matching,    "d/d_bg_s_movebg_actor.cpp"),
             Object(Matching,    "d/d_bg_s_spl_grp_chk.cpp"),
             Object(Matching,    "d/d_bg_s_wtr_chk.cpp"),
-            Object(NonMatching, "d/d_bg_w.cpp"),
+            Object(Matching,    "d/d_bg_w.cpp"),
             Object(Matching,    "d/d_bg_w_deform.cpp"),
             Object(Matching,    "d/d_bg_w_hf.cpp"),
             Object(Matching,    "d/d_bg_w_sv.cpp"),
@@ -643,7 +643,7 @@ config.libs = [
             Object(Matching,    "SSystem/SComponent/c_bg_w.cpp"),
             Object(Matching,    "SSystem/SComponent/c_m2d.cpp"),
             Object(Matching,    "SSystem/SComponent/c_m2d_g_box.cpp"),
-            Object(NonMatching, "SSystem/SComponent/c_m3d.cpp"),
+            Object(Matching,    "SSystem/SComponent/c_m3d.cpp"),
             Object(Matching,    "SSystem/SComponent/c_m3d_g_aab.cpp"),
             Object(Matching,    "SSystem/SComponent/c_m3d_g_cyl.cpp"),
             Object(Matching,    "SSystem/SComponent/c_m3d_g_pla.cpp"),
diff --git a/include/SSystem/SComponent/c_bg_s_shdw_draw.h b/include/SSystem/SComponent/c_bg_s_shdw_draw.h
index 8899412e6..fe40006b0 100644
--- a/include/SSystem/SComponent/c_bg_s_shdw_draw.h
+++ b/include/SSystem/SComponent/c_bg_s_shdw_draw.h
@@ -3,7 +3,6 @@
 
 #include "SSystem/SComponent/c_bg_s_chk.h"
 #include "SSystem/SComponent/c_m3d_g_aab.h"
-#include "dolphin/types.h"
 
 struct cBgD_Vtx_t;
 class cM3dGPla;
diff --git a/include/SSystem/SComponent/c_m3d.h b/include/SSystem/SComponent/c_m3d.h
index 50d57bcbc..1c668728a 100644
--- a/include/SSystem/SComponent/c_m3d.h
+++ b/include/SSystem/SComponent/c_m3d.h
@@ -73,6 +73,7 @@ bool cM3d_Cross_LinPla(const cM3dGLin*, const cM3dGPla*, Vec*, bool, bool);
 bool cM3d_Cross_MinMaxBoxLine(const Vec*, const Vec*, const Vec*, const Vec*);
 bool cM3d_InclusionCheckPosIn3PosBox3d(const Vec*, const Vec*, const Vec*, const Vec*, f32);
 inline static bool cM3d_InclusionCheckPosIn3PosBox2d(f32, f32, f32, f32, f32, f32, f32, f32);
+bool cM3d_CrossX_Tri(const cM3dGTri*, const Vec*, f32*);
 bool cM3d_CrossX_Tri(const cM3dGTri*, const Vec*, f32);
 bool cM3d_CrossX_Tri(const cM3dGTri*, const Vec*);
 bool cM3d_CrossX_LinTri_proc(const cM3dGTri*, const Vec*);
@@ -83,6 +84,7 @@ bool cM3d_CrossY_Tri_Front(const Vec&, const Vec&, const Vec&, const Vec*);
 bool cM3d_CrossY_Tri(const cM3dGTri*, const Vec*, f32*);
 bool cM3d_CrossY_Tri(const cM3dGTri*, const Vec*, f32);
 bool cM3d_CrossY_Tri(const cM3dGTri*, const Vec*, const cM3d_Range*, f32*);
+bool cM3d_CrossZ_Tri(const cM3dGTri*, const Vec*, f32*);
 bool cM3d_CrossZ_Tri(const cM3dGTri*, const Vec*, f32);
 bool cM3d_CrossZ_Tri(const cM3dGTri*, const Vec*);
 bool cM3d_CrossZ_LinTri_proc(const cM3dGTri*, const Vec*);
@@ -109,7 +111,7 @@ int cM3d_Cross_CylPntPnt(const cM3dGCyl*, const Vec*, const Vec*, Vec*, Vec*);
 bool cM3d_Cross_CylPnt(const cM3dGCyl*, const Vec*);
 bool cM3d_Cross_CpsCps(const cM3dGCps&, const cM3dGCps&, Vec*);
 bool cM3d_Cross_CpsCyl(const cM3dGCps&, const cM3dGCyl&, Vec*);
-bool cM3d_Cross_CpsSph_CrossPos(const cM3dGCps&, const cM3dGSph&, const Vec&, Vec*);
+void cM3d_Cross_CpsSph_CrossPos(const cM3dGCps&, const cM3dGSph&, const Vec&, Vec*);
 bool cM3d_Cross_CpsSph(const cM3dGCps&, const cM3dGSph&, Vec*);
 bool cM3d_Cross_TriTri(const cM3dGTri&, const cM3dGTri&, Vec*);
 bool cM3d_Cross_CpsTri(const cM3dGCps&, cM3dGTri, Vec*);
diff --git a/include/SSystem/SComponent/c_m3d_g_pla.h b/include/SSystem/SComponent/c_m3d_g_pla.h
index 11ddc95ff..fea7b34a9 100644
--- a/include/SSystem/SComponent/c_m3d_g_pla.h
+++ b/include/SSystem/SComponent/c_m3d_g_pla.h
@@ -42,8 +42,8 @@ public:
         *i_value = (-mNormal.x * i_axis.x - mNormal.z * i_axis.z) / mNormal.y;
         return true;
     }
-    f32 getSignedLenPos(const cXyz* param_1) const {
-        return cM3d_SignedLenPlaAndPos(this, param_1);
+    f32 getSignedLenPos(const cXyz* pos) const {
+        return cM3d_SignedLenPlaAndPos(this, pos);
     }
     void Set(const cM3dGPla* pla) {
         mNormal = *pla->GetNP();
diff --git a/include/SSystem/SComponent/c_m3d_g_tri.h b/include/SSystem/SComponent/c_m3d_g_tri.h
index bbce3a148..29282c4be 100644
--- a/include/SSystem/SComponent/c_m3d_g_tri.h
+++ b/include/SSystem/SComponent/c_m3d_g_tri.h
@@ -33,6 +33,10 @@ public:
     bool Cross(const cM3dGSph& sph, Vec* out) const { return cM3d_Cross_SphTri(&sph, this, out); }
     bool Cross(const cM3dGTri& tri, Vec* out) const { return cM3d_Cross_TriTri(tri, *this, out); }
 
+    bool crossX(const Vec* pos, f32* pcross_len) const { return cM3d_CrossX_Tri(this, pos, pcross_len); }
+    bool crossY(const Vec* pos) const { return cM3d_CrossY_Tri(this, pos); }
+    bool crossZ(const Vec* pos, f32* pcross_len) const { return cM3d_CrossZ_Tri(this, pos, pcross_len); }
+
     void Up(f32 dist) {
         Vec delta;
         VECScale(&mNormal, &delta, dist);
@@ -42,10 +46,6 @@ public:
         mD -= dist;
     }
 
-    // TODO
-    void crossX(const Vec*, f32*) const {}
-    void crossY(const Vec*) const {}
-    void crossZ(const Vec*, f32*) const {}
     void setBg(const Vec* a, const Vec* b, const Vec* c, const cM3dGPla* pla) {
         mA = *a;
         mB = *b;
@@ -61,6 +61,7 @@ public:
     void setUp() {
         SetupFrom3Vtx(&mA, &mB, &mC);
     }
+
     virtual ~cM3dGTri() {}
 };  // Size: 0x38
 
diff --git a/include/d/d_bg_s.h b/include/d/d_bg_s.h
index 3fbeb322d..3c84d252f 100644
--- a/include/d/d_bg_s.h
+++ b/include/d/d_bg_s.h
@@ -157,7 +157,7 @@ public:
         mGndChk.OffWall();
         mFlag &= ~8;
     }
-    void ClrPosVec() { field_0x4c.x = field_0x4c.y = field_0x4c.z = 0.0f; }
+    void ClrPosVec() { m_pos_vec.x = m_pos_vec.y = m_pos_vec.z = 0.0f; }
 
     f32 GetWallH() const { return mWallHeight; }
     f32 GetWallR() const { return mWallRadius; }
@@ -167,9 +167,9 @@ public:
     }
     f32 GetGroundH() const { return mGroundH; }
 
-    cXyz* GetOldPos() const { return pm_old_pos; }
     cXyz* GetPos() const { return pm_pos; }
-    cXyz& GetPosVec() { return field_0x4c; }
+    cXyz* GetOldPos() const { return pm_old_pos; }
+    cXyz& GetPosVec() { return m_pos_vec; }
 
     const cM3dGCyl* GetCylP() const { return &mCyl; }
     void SetOldCyl() { mOldCyl.Set(*pm_old_pos, mWallRadius, mWallHeight * 2.0f); }
@@ -179,14 +179,19 @@ public:
     void SetWallActorInfo(int bg_index, void* bgw, fpc_ProcID actor_id) {
         SetActorInfo(bg_index, bgw, actor_id);
     }
-    void SetWallPolyIndex(int) {} // TODO
+    void SetWallPolyIndex(int poly_index) {
+        SetPolyIndex(poly_index);
+    }
 
+    /* 0x000 */ /* cBgS_PolyInfo */
+    /* 0x010 */ /* dBgS_Chk */
+    /* 0x024 */ /* cBgS_Chk */
     /* 0x038 */ u32 mFlag;
     /* 0x03C */ f32 mWallHeight;
     /* 0x040 */ f32 mWallRadius;
     /* 0x044 */ cXyz* pm_pos;
     /* 0x048 */ cXyz* pm_old_pos;
-    /* 0x04C */ cXyz field_0x4c;
+    /* 0x04C */ cXyz m_pos_vec;
     /* 0x058 */ cXyz* field_0x58;
     /* 0x05C */ f32 mGroundH;
     /* 0x060 */ u8 field_0x60;
diff --git a/src/SSystem/SComponent/c_m3d.cpp b/src/SSystem/SComponent/c_m3d.cpp
index 0a5b251d8..ce5c88c5f 100644
--- a/src/SSystem/SComponent/c_m3d.cpp
+++ b/src/SSystem/SComponent/c_m3d.cpp
@@ -42,8 +42,7 @@ void cM3d_InDivPos2(const Vec* v0, const Vec* v1, f32 scale, Vec* pDst) {
 }
 
 /* 8024A4B4-8024A56C       .text cM3d_Len2dSqPntAndSegLine__FffffffPfPfPf */
-bool cM3d_Len2dSqPntAndSegLine(f32 xp, f32 yp, f32 x0, f32 y0, f32 x1, f32 y1, f32* outx, f32* outy,
-                               f32* seg) {
+bool cM3d_Len2dSqPntAndSegLine(f32 xp, f32 yp, f32 x0, f32 y0, f32 x1, f32 y1, f32* outx, f32* outy, f32* seg) {
     bool ret = false;
     f32 xd = x1 - x0;
     f32 yd = y1 - y0;
@@ -578,46 +577,47 @@ bool cM3d_InclusionCheckPosIn3PosBox3d(const Vec* pVecA, const Vec* pVecB, const
     return true;
 }
 
-inline static bool cM3d_InclusionCheckPosIn3PosBox2d(f32 param_1, f32 param_2, f32 param_3,
-                                                     f32 param_4, f32 param_5, f32 param_6,
-                                                     f32 param_7, f32 param_8) {
-    f32 dVar7;
-    f32 temp;
+inline static bool cM3d_InclusionCheckPosIn3PosBox2d(
+    f32 param_1, f32 param_2,
+    f32 param_3, f32 param_4,
+    f32 param_5, f32 param_6,
+    f32 param_7, f32 param_8
+) {
+    f32 f31;
+    f32 f30;
     if (param_1 < param_3) {
-        dVar7 = param_1;
-        temp = param_3;
+        f31 = param_1;
+        f30 = param_3;
     } else {
-        dVar7 = param_3;
-        temp = param_1;
+        f31 = param_3;
+        f30 = param_1;
     }
 
-    if (dVar7 > param_5) {
-        dVar7 = param_5;   
-    } else if (temp < param_5) {
-        temp = param_5;
+    if (f31 > param_5) {
+        f31 = param_5;   
+    } else if (f30 < param_5) {
+        f30 = param_5;
     }
 
-    if (dVar7 > param_7 || temp < param_7) {
+    if (f31 > param_7 || f30 < param_7) {
         return false;
     } 
 
-    f32 fVar1;
     if (param_2 < param_4) {
-        fVar1 = param_2;
-        param_4 = param_4;
+        f31 = param_2;
+        f30 = param_4;
     } else {
-        fVar1 = param_4;
-        param_4 = param_2;
+        f31 = param_4;
+        f30 = param_2;
     }
 
-    if (fVar1 > param_6) {
-        fVar1 = param_6;
-    } else if (param_4 < param_6) {
-        param_4 = param_6;
+    if (f31 > param_6) {
+        f31 = param_6;
+    } else if (f30 < param_6) {
+        f30 = param_6;
     }
     
-    if (fVar1 > param_8 || param_4 < param_8)
-    {
+    if (f31 > param_8 || f30 < param_8) {
         return false;
     }
     return true;
@@ -768,9 +768,9 @@ bool cM3d_CrossX_Tri(const cM3dGTri* tri, const Vec* pos) {
 }
 
 /* 8024BF1C-8024BFA0       .text cM3d_CrossX_Tri__FPC8cM3dGTriPC3VecPf */
-bool cM3d_CrossX_Tri(const cM3dGTri* tri, const Vec* r30, f32* r31) {
+bool cM3d_CrossX_Tri(const cM3dGTri* tri, const Vec* r30, f32* pcross_len) {
     if (cM3d_CrossX_Tri(tri, r30)) {
-        *r31 = ((r30->y * -tri->GetNP()->y) - (r30->z * tri->GetNP()->z) - tri->GetD()) / tri->GetNP()->x;
+        *pcross_len = ((r30->y * -tri->GetNP()->y) - (r30->z * tri->GetNP()->z) - tri->GetD()) / tri->GetNP()->x;
         return true;
     } else {
         return false;
@@ -969,9 +969,9 @@ bool cM3d_CrossY_Tri_Front(const Vec& r3, const Vec& r4, const Vec& r5, const Ve
 }
 
 /* 8024C4D0-8024C554       .text cM3d_CrossY_Tri__FPC8cM3dGTriPC3VecPf */
-bool cM3d_CrossY_Tri(const cM3dGTri* tri, const Vec* r30, f32* r31) {
+bool cM3d_CrossY_Tri(const cM3dGTri* tri, const Vec* r30, f32* pcross_len) {
     if (cM3d_CrossY_Tri(tri, r30)) {
-        *r31 = ((r30->x * -tri->GetNP()->x) - (r30->z * tri->GetNP()->z) - tri->GetD()) / tri->GetNP()->y;
+        *pcross_len = ((r30->x * -tri->GetNP()->x) - (r30->z * tri->GetNP()->z) - tri->GetD()) / tri->GetNP()->y;
         return true;
     } else {
         return false;
@@ -1051,7 +1051,7 @@ bool cM3d_CrossY_Tri(const cM3dGTri* tri, const Vec* pos, f32 f1) {
 }
 
 /* 8024C738-8024C828       .text cM3d_CrossY_Tri__FPC8cM3dGTriPC3VecPC10cM3d_RangePf */
-bool cM3d_CrossY_Tri(const cM3dGTri* tri, const Vec* pos, const cM3d_Range* range, f32* r31) {
+bool cM3d_CrossY_Tri(const cM3dGTri* tri, const Vec* pos, const cM3d_Range* range, f32* pcross_len) {
     if (cM3d_IsZero(tri->GetNP()->y)) {
         return false;
     }
@@ -1065,7 +1065,7 @@ bool cM3d_CrossY_Tri(const cM3dGTri* tri, const Vec* pos, const cM3d_Range* rang
     if ((f31 > 0.0f && f1 > 0.0f) || (f31 < 0.0f && f1 < 0.0f)) {
         return false;
     }
-    return cM3d_CrossY_Tri(tri, pos, r31);
+    return cM3d_CrossY_Tri(tri, pos, pcross_len);
 }
 
 /* 8024C828-8024CA0C       .text cM3d_CrossZ_Tri__FPC8cM3dGTriPC3Vecf */
@@ -1213,9 +1213,9 @@ bool cM3d_CrossZ_Tri(const cM3dGTri* tri, const Vec* pos) {
 }
 
 /* 8024CBF4-8024CC78       .text cM3d_CrossZ_Tri__FPC8cM3dGTriPC3VecPf */
-bool cM3d_CrossZ_Tri(const cM3dGTri* tri, const Vec* r30, f32* r31) {
+bool cM3d_CrossZ_Tri(const cM3dGTri* tri, const Vec* r30, f32* pcross_len) {
     if (cM3d_CrossZ_Tri(tri, r30)) {
-        *r31 = ((r30->x * -tri->GetNP()->x) - (r30->y * tri->GetNP()->y) - tri->GetD()) / tri->GetNP()->z;
+        *pcross_len = ((r30->x * -tri->GetNP()->x) - (r30->y * tri->GetNP()->y) - tri->GetD()) / tri->GetNP()->z;
         return true;
     } else {
         return false;
@@ -1683,10 +1683,25 @@ bool cM3d_Cross_CylTri(const cM3dGCyl* cyl, const cM3dGTri* tri, Vec* param_2) {
 
 /* 8024F410-8024FA90       .text cM3d_Cross_CylLin__FPC8cM3dGCylPC8cM3dGLinP3VecP3Vec */
 int cM3d_Cross_CylLin(const cM3dGCyl* cyl, const cM3dGLin* line, Vec* param_2, Vec* param_3) {
-    /* Nonmatching - regalloc */
-    f32 f1 = 0.0f;
-    f32 f2 = 0.0f;
-    u32 uVar11 = 0;
+    f32 ratio;
+    f32 f2;
+    f32 fVar5;
+    f32 fVar2;
+    f32 fVar1;
+    f32 fVar6;
+    f32 fVar4;
+    BOOL bVar4;
+    BOOL bVar3;
+    BOOL bVar6;
+    BOOL bVar5;
+    u32 uVar11;
+    f32 sp28;
+    f32 r_sq;
+    int count;
+
+    ratio = 0.0f;
+    f2 = 0.0f;
+    uVar11 = 0;
 
     if (cM3d_Cross_CylPnt(cyl, line->GetStartP()) && cM3d_Cross_CylPnt(cyl, line->GetEndP())) {
         *param_2 = *line->GetStartP();
@@ -1700,10 +1715,10 @@ int cM3d_Cross_CylLin(const cM3dGCyl* cyl, const cM3dGLin* line, Vec* param_2, V
     VECSubtract(line->GetStartP(), center, &vec1);
     VECSubtract(line->GetEndP(), center, &vec2);
     VECSubtract(&vec2, &vec1, &vec3);
-    f32 r_sq = cyl->GetR() * cyl->GetR();
+    r_sq = cyl->GetR() * cyl->GetR();
 
     if (!cM3d_IsZero(vec3.y)) {
-        f32 ratio = -vec1.y / vec3.y;
+        ratio = -vec1.y / vec3.y;
         if (ratio >= 0.0f && ratio <= 1.0f) {
             f32 x = vec1.x + vec3.x * ratio;
             f32 z = vec1.z + vec3.z * ratio;
@@ -1728,56 +1743,48 @@ int cM3d_Cross_CylLin(const cM3dGCyl* cyl, const cM3dGLin* line, Vec* param_2, V
         }
     }
 
-    f32 fVar5 = vec3.x * vec3.x + vec3.z * vec3.z;
-    f32 fVar2 = (vec3.x * vec1.x + vec3.z * vec1.z) * 2.0f;
-    f32 fVar1 = vec1.x * vec1.x + vec1.z * vec1.z - r_sq;
+    bVar4 = false;
+    bVar3 = false;
+    fVar5 = vec3.x * vec3.x + vec3.z * vec3.z;
+    fVar2 = (vec3.x * vec1.x + vec3.z * vec1.z) * 2.0f;
+    fVar1 = vec1.x * vec1.x + vec1.z * vec1.z - r_sq;
 
-    BOOL bVar4, bVar3;
-    if (!cM3d_IsZero(fVar5 * 2.0f)) {
-        f32 fVar6 = fVar2 * fVar2 - fVar5 * 4.0f * fVar1;
+    f32 sp0C = fVar5 * 2.0f;
+    if (!cM3d_IsZero(sp0C)) {
+        fVar6 = fVar2 * fVar2 - fVar5 * 4.0f * fVar1;
         if (fVar6 < 0.0f) {
             return 0;
         }
         if (fVar6 > 0.0f) {
-            bVar3 = true;
-            bVar4 = true;
+            bVar4 = bVar3 = true;
         } else {
             bVar4 = true;
             bVar3 = false;
         }
-        f32 fVar4 = std::sqrtf(fVar6);
+        fVar4 = std::sqrtf(fVar6);
         if (bVar4) {
-            f1 = (-fVar2 + fVar4) / (fVar5 * 2.0f);
+            ratio = (-fVar2 + fVar4) / sp0C;
         }
         if (bVar3) {
-            f2 = (-fVar2 - fVar4) / (fVar5 * 2.0f);
+            f2 = (-fVar2 - fVar4) / sp0C;
         }
     } else {
         if (!cM3d_IsZero(fVar2)) {
             bVar4 = true;
             bVar3 = false;
-            f1 = -fVar1 / fVar2;
+            ratio = -fVar1 / fVar2;
         } else {
             return 0;
         }
     }
 
-    BOOL bVar6, bVar5;
     if (bVar4 && !bVar3) {
-        if (0.0f > f1 || f1 > 1.0f) {
+        if (0.0f > ratio || ratio > 1.0f) {
             return 0;
         }
     } else {
-        bool tmp = false;
-        if (0.0f > f1 || f1 > 1.0f) {
-            tmp = true;
-        }
-        bVar6 = tmp;
-        tmp = false;
-        if (0.0f > f2 || f2 > 1.0f) {
-            tmp = true;
-        }
-        bVar5 = tmp;
+        bVar6 = 0.0f > ratio || ratio > 1.0f;
+        bVar5 = 0.0f > f2 || f2 > 1.0f;
         if (bVar6 && bVar5) {
             return 0;
         }
@@ -1790,14 +1797,14 @@ int cM3d_Cross_CylLin(const cM3dGCyl* cyl, const cM3dGLin* line, Vec* param_2, V
     }
 
     if (bVar4) {
-        f32 tmp = vec1.y + f1 * vec3.y;
-        if (tmp < 0.0f || tmp > cyl->GetH()) {
+        sp28 = vec1.y + ratio * vec3.y;
+        if (sp28 < 0.0f || sp28 > cyl->GetH()) {
             bVar4 = false;
         }
     }
     if (bVar3) {
-        f32 tmp = vec1.y + f2 * vec3.y;
-        if (tmp < 0.0f || tmp > cyl->GetH()) {
+        sp28 = vec1.y + f2 * vec3.y;
+        if (sp28 < 0.0f || sp28 > cyl->GetH()) {
             bVar3 = false;
         }
     }
@@ -1809,15 +1816,16 @@ int cM3d_Cross_CylLin(const cM3dGCyl* cyl, const cM3dGLin* line, Vec* param_2, V
     if (bVar4 && bVar3) {
         Vec vec5, vec6;
         VECAdd(&vec1, center, &vec5);
-        VECScale(&vec3, &vec6, f1);
+        uVar11 |= 4;
+        VECScale(&vec3, &vec6, ratio);
         VECAdd(&vec6, &vec5, &vec[2]);
-        uVar11 |= 0xC;
+        uVar11 |= 8;
         VECScale(&vec3, &vec6, f2);
         VECAdd(&vec6, &vec5, &vec[3]);
     } else if (bVar4) {
         uVar11 |= 4;
         Vec vec5, vec6;
-        VECScale(&vec3, &vec5, f1);
+        VECScale(&vec3, &vec5, ratio);
         VECAdd(&vec5, &vec1, &vec6);
         VECAdd(&vec6, center, &vec[2]);
     } else if (bVar3) {
@@ -1828,13 +1836,15 @@ int cM3d_Cross_CylLin(const cM3dGCyl* cyl, const cM3dGLin* line, Vec* param_2, V
         VECAdd(&vec6, center, &vec[2]);
     }
 
-    int count = 0;
+    count = 0;
     for (int i = 0; i < 4; i++) {
         if (uVar11 & (1 << i)) {
             if (count == 0) {
                 *param_2 = vec[i];
             } else if (count == 1) {
-                if (cM3d_LenSq(line->GetStartP(), &vec[i]) < cM3d_LenSq(line->GetStartP(), param_2)) {
+                // Bug? This check was changed to the following for TP:
+                // if (cM3d_LenSq(line->GetStartP(), param_2) < cM3d_LenSq(line->GetStartP(), &vec[i])) {
+                if (cM3d_LenSq(param_2, line->GetStartP()) < cM3d_LenSq(param_2, &vec[i])) {
                     *param_3 = vec[i];
                 } else {
                     *param_3 = *param_2;
@@ -1857,12 +1867,12 @@ int cM3d_Cross_CylPntPnt(const cM3dGCyl* cyl, const Vec* a, const Vec* b, Vec* d
 
 /* 8024FB04-8024FB68       .text cM3d_Cross_CylPnt__FPC8cM3dGCylPC3Vec */
 bool cM3d_Cross_CylPnt(const cM3dGCyl* cyl, const Vec* pt) {
-    /* Nonmatching - regalloc */
     const cXyz& center = cyl->GetC();
     f32 dX = center.x - pt->x;
     f32 dZ = center.z - pt->z;
-    f32 maxY = center.y + cyl->GetH();
-    if (dX * dX + dZ * dZ < cyl->GetR() * cyl->GetR() && center.y < pt->y && maxY > pt->y) {
+    f32 y = center.y;
+    f32 maxY = y + cyl->GetH();
+    if (dX * dX + dZ * dZ < cyl->GetR() * cyl->GetR() && y < pt->y && maxY > pt->y) {
         return true;
     } else {
         return false;
@@ -2049,7 +2059,7 @@ bool cM3d_Cross_CpsCyl(const cM3dGCps& cps, const cM3dGCyl& cyl, Vec* param_2) {
 }
 
 /* 802506D4-80250840       .text cM3d_Cross_CpsSph_CrossPos__FRC8cM3dGCpsRC8cM3dGSphRC3VecP3Vec */
-bool cM3d_Cross_CpsSph_CrossPos(const cM3dGCps& param_1, const cM3dGSph& param_2, const Vec& param_3, Vec* param_4) {
+void cM3d_Cross_CpsSph_CrossPos(const cM3dGCps& param_1, const cM3dGSph& param_2, const Vec& param_3, Vec* param_4) {
     Vec aVStack_70;
     Vec VStack_7c;
     int iVar5 = cM3d_Cross_LinSph_CrossPos(param_2, param_1, &aVStack_70, &VStack_7c);
diff --git a/src/SSystem/SComponent/c_m3d_g_cyl.cpp b/src/SSystem/SComponent/c_m3d_g_cyl.cpp
index 1f06f76cd..259c5e56a 100644
--- a/src/SSystem/SComponent/c_m3d_g_cyl.cpp
+++ b/src/SSystem/SComponent/c_m3d_g_cyl.cpp
@@ -6,7 +6,6 @@
 #include "SSystem/SComponent/c_m3d_g_cyl.h"
 #include "SSystem/SComponent/c_m3d.h"
 #include "JSystem/JUtility/JUTAssert.h"
-#include "float.h"
 
 #define CHECK_FLOAT_CLASS(line, x) JUT_ASSERT(line, !(fpclassify(x) == 1));
 #define CHECK_FLOAT_RANGE(line, x) JUT_ASSERT(line, -1.0e32f < x && x < 1.0e32f);
diff --git a/src/SSystem/SComponent/c_m3d_g_tri.cpp b/src/SSystem/SComponent/c_m3d_g_tri.cpp
index 5228eece2..5cf7cafd4 100644
--- a/src/SSystem/SComponent/c_m3d_g_tri.cpp
+++ b/src/SSystem/SComponent/c_m3d_g_tri.cpp
@@ -5,7 +5,6 @@
 
 #include "SSystem/SComponent/c_m3d_g_tri.h"
 #include "SSystem/SComponent/c_m3d.h"
-#include "dolphin/types.h"
 
 /* 802527D4-80252878       .text __ct__8cM3dGTriFPC3VecPC3VecPC3Vec */
 cM3dGTri::cM3dGTri(const Vec* pA, const Vec* pB, const Vec* pC) {
diff --git a/src/d/d_bg_w.cpp b/src/d/d_bg_w.cpp
index e90ceadb9..805d999bd 100644
--- a/src/d/d_bg_w.cpp
+++ b/src/d/d_bg_w.cpp
@@ -597,14 +597,203 @@ void dBgW::positionWallCrrPos(cM3dGTri& plane, dBgS_CrrPos* crr, cXyz* pos, f32
 }
 
 /* 800A819C-800A8964       .text RwgWallCrrPos__4dBgWFUsP11dBgS_CrrPos */
-bool dBgW::RwgWallCrrPos(u16, dBgS_CrrPos*) {
-    /* Nonmatching */
+bool dBgW::RwgWallCrrPos(u16 i_poly_index, dBgS_CrrPos* crr) {
+    cBgW_RwgElm* rwg_elm;
+    cBgD_Tri_t* tri_t;
+    cM3dGPla* plane;
+    bool ret = false;
+    cM3dGTri tri;
+    cXyz wall_top_pos = *crr->GetPos();
+    wall_top_pos.y += crr->GetWallH();
+    cXyz old_wall_top_pos = *crr->GetOldPos();
+    old_wall_top_pos.y += crr->GetWallH();
+    u32 poly_index_z = i_poly_index;
+    u32 poly_index_x = i_poly_index;
+
+    // Z
+    while (true) {
+        rwg_elm = &pm_rwg[poly_index_z];
+        if (!ChkPolyThrough(poly_index_z, crr->GetPolyPassChk())) {
+            tri_t = &pm_bgd->m_t_tbl[poly_index_z];
+            plane = &pm_tri[poly_index_z].m_plane;
+            tri.setBg(
+                &pm_vtx_tbl[tri_t->vtx0],
+                &pm_vtx_tbl[tri_t->vtx1],
+                &pm_vtx_tbl[tri_t->vtx2],
+                plane
+            );
+            
+            f32 dist = tri.getSignedLenPos(&wall_top_pos);
+            if (std::fabsf(dist) > crr->GetWallR()) {
+                if (rwg_elm->next == 0xFFFF) {
+                    break;
+                }
+                poly_index_z = rwg_elm->next;
+                continue;
+            }
+            
+            f32 nx = plane->GetNP()->x;
+            f32 nz = plane->GetNP()->z;
+            f32 f4 = std::sqrtf(nx*nx + nz*nz);
+            if (cM3d_IsZero(f4)) {
+                if (rwg_elm->next == 0xFFFF) {
+                    break;
+                }
+                poly_index_z = rwg_elm->next;
+                continue;
+            }
+            
+            f32 f29 = 1.0f / f4;
+            f32 f28 = f29 * std::fabsf(nz);
+            if (f28 < 0.4f) {
+                if (rwg_elm->next == 0xFFFF) {
+                    break;
+                }
+                poly_index_z = rwg_elm->next;
+                continue;
+            }
+            
+            // Is this an inline? 
+            f32 min_z = tri.mA.z;
+            f32 max_z = min_z;
+            if (min_z > tri.mB.z) {
+                min_z = tri.mB.z;
+            } else if (max_z < tri.mB.z) {
+                max_z = tri.mB.z;
+            }
+            if (min_z > tri.mC.z) {
+                min_z = tri.mC.z;
+            } else if (max_z < tri.mC.z) {
+                max_z = tri.mC.z;
+            }
+            min_z -= crr->GetWallR();
+            max_z += crr->GetWallR();
+            if (min_z > wall_top_pos.z || max_z < wall_top_pos.z) {
+                if (rwg_elm->next == 0xFFFF) {
+                    break;
+                }
+                poly_index_z = rwg_elm->next;
+                continue;
+            }
+            
+            f32 cross_len;
+            if (tri.crossZ(&wall_top_pos, &cross_len)) {
+                f32 f2 = crr->GetWallR() / f28;
+                f32 f1 = cross_len - wall_top_pos.z;
+                f1 *= nz;
+                if (std::fabsf(cross_len - wall_top_pos.z) <= f2 && f1 <= 4.0f) {
+                    positionWallCrrPos(tri, crr, &crr->GetPosVec(), f29, dist);
+                    crr->SetWallPolyIndex(poly_index_z);
+                    ret = true;
+                    crr->SetZCrr();
+                }
+            }
+        }
+
+        if (rwg_elm->next == 0xFFFF)
+            break;
+        poly_index_z = rwg_elm->next;
+    }
+
+    // X
+    while (true) {
+        rwg_elm = &pm_rwg[poly_index_x];
+        if (!ChkPolyThrough(poly_index_x, crr->GetPolyPassChk())) {
+            tri_t = &pm_bgd->m_t_tbl[poly_index_x];
+            plane = &pm_tri[poly_index_x].m_plane;
+            tri.setBg(
+                &pm_vtx_tbl[tri_t->vtx0],
+                &pm_vtx_tbl[tri_t->vtx1],
+                &pm_vtx_tbl[tri_t->vtx2],
+                plane
+            );
+            
+            f32 dist = tri.getSignedLenPos(&wall_top_pos);
+            if (std::fabsf(dist) > crr->GetWallR()) {
+                if (rwg_elm->next == 0xFFFF) {
+                    break;
+                }
+                poly_index_x = rwg_elm->next;
+                continue;
+            }
+            
+            f32 nx = plane->GetNP()->x;
+            f32 nz = plane->GetNP()->z;
+            f32 f4 = std::sqrtf(nx*nx + nz*nz);
+            if (cM3d_IsZero(f4)) {
+                if (rwg_elm->next == 0xFFFF) {
+                    break;
+                }
+                poly_index_x = rwg_elm->next;
+                continue;
+            }
+            
+            f32 f28 = 1.0f / f4;
+            f32 f29 = f28 * std::fabsf(nx);
+            if (f29 < 0.4f) {
+                if (rwg_elm->next == 0xFFFF) {
+                    break;
+                }
+                poly_index_x = rwg_elm->next;
+                continue;
+            }
+            
+            // Is this an inline? 
+            f32 min_x = tri.mA.x;
+            f32 max_x = min_x;
+            if (min_x > tri.mB.x) {
+                min_x = tri.mB.x;
+            } else if (max_x < tri.mB.x) {
+                max_x = tri.mB.x;
+            }
+            if (min_x > tri.mC.x) {
+                min_x = tri.mC.x;
+            } else if (max_x < tri.mC.x) {
+                max_x = tri.mC.x;
+            }
+            min_x -= crr->GetWallR();
+            max_x += crr->GetWallR();
+            if (min_x > wall_top_pos.x || max_x < wall_top_pos.x) {
+                if (rwg_elm->next == 0xFFFF) {
+                    break;
+                }
+                poly_index_x = rwg_elm->next;
+                continue;
+            }
+            
+            f32 cross_len;
+            if (tri.crossX(&wall_top_pos, &cross_len)) {
+                f32 f2 = crr->GetWallR() / f29;
+                f32 f1 = cross_len - wall_top_pos.x;
+                f1 *= nx;
+                if (std::fabsf(cross_len - wall_top_pos.x) <= f2 && f1 <= 4.0f) {
+                    positionWallCrrPos(tri, crr, &crr->GetPosVec(), f28, dist);
+                    crr->SetWallPolyIndex(poly_index_x);
+                    ret = true;
+                    crr->SetXCrr();
+                }
+            }
+        }
+
+        poly_index_x = rwg_elm->next;
+        if (poly_index_x == 0xFFFF) {
+            break;
+        }
+        poly_index_x = rwg_elm->next;
+    }
+
+    if (ret) {
+        crr->SetCyl();
+        crr->SetLin();
+    }
+
+    return ret;
 }
 
 void dBgW::dummyfunc() {
     // TODO: find where this assert actually comes from
     // cBgW::RwgShdwDraw ?
-    int index;
+    int index = 0;
     JUT_ASSERT(0, 0 <= index && index < pm_bgd->m_t_num);
 }