From 68c97bf2db12ee232f1427395e4112307f3148ad Mon Sep 17 00:00:00 2001
From: Franklyn Tackitt <git@frank.af>
Date: Mon, 26 Feb 2024 16:12:02 -0700
Subject: [PATCH] Implement cM3d_Len3dSqPntAndSegLine

Fix cM3d_Len2dSqPntAndSegLine
---
 include/SSystem/SComponent/c_m3d.h       |  1 +
 include/SSystem/SComponent/c_m3d_g_cps.h |  4 ++-
 include/SSystem/SComponent/c_m3d_g_sph.h |  5 ++-
 src/SSystem/SComponent/c_m3d.cpp         | 43 ++++++++++++++++++------
 4 files changed, 41 insertions(+), 12 deletions(-)

diff --git a/include/SSystem/SComponent/c_m3d.h b/include/SSystem/SComponent/c_m3d.h
index 584354b9b..921904c0f 100644
--- a/include/SSystem/SComponent/c_m3d.h
+++ b/include/SSystem/SComponent/c_m3d.h
@@ -78,6 +78,7 @@ bool cM3d_Cross_TriTri(const cM3dGTri&, const cM3dGTri&, Vec*);
 bool cM3d_Cross_CpsTri(const cM3dGCps&, cM3dGTri, Vec*);
 void cM3d_CalcVecAngle(const Vec&, short*, short*);
 void cM3d_CalcVecZAngle(const Vec&, csXyz*);
+bool cM3d_NearPos_Cps(const cM3dGCps&, const Vec&, Vec*);
 void cM3d_PlaneCrossLineProcWork(f32, f32, f32, f32, f32, f32, f32, f32*, f32*);
 int cM3d_2PlaneCrossLine(const cM3dGPla&, const cM3dGPla&, cM3dGLin*);
 bool cM3d_3PlaneCrossPos(const cM3dGPla&, const cM3dGPla&, const cM3dGPla&, Vec*);
diff --git a/include/SSystem/SComponent/c_m3d_g_cps.h b/include/SSystem/SComponent/c_m3d_g_cps.h
index c25d05a92..48b785d5c 100644
--- a/include/SSystem/SComponent/c_m3d_g_cps.h
+++ b/include/SSystem/SComponent/c_m3d_g_cps.h
@@ -36,7 +36,9 @@ public:
     f32 GetR() const { return mRadius; }
     void SetR(f32 r) { mRadius = r; }
 
-    void NearPos(cXyz, cXyz*) const {}
+    bool NearPos(const cXyz& pos, cXyz* outPos) {
+        return cM3d_NearPos_Cps(*this, pos, outPos);
+    }
 };  // Size = 0x20
 
 STATIC_ASSERT(0x20 == sizeof(cM3dGCps));
diff --git a/include/SSystem/SComponent/c_m3d_g_sph.h b/include/SSystem/SComponent/c_m3d_g_sph.h
index a4b01893b..99f877402 100644
--- a/include/SSystem/SComponent/c_m3d_g_sph.h
+++ b/include/SSystem/SComponent/c_m3d_g_sph.h
@@ -28,7 +28,10 @@ public:
         SetC(src.mCenter);
         SetR(src.mRadius);
     }
-    void Set(const cXyz&, f32) {}
+    void Set(const cXyz& srcCenter, f32 srcRadius) {
+        SetC(srcCenter);
+        SetR(srcRadius);
+    }
     bool Cross(const cM3dGCps* cps, cXyz* dst) const {
         return cM3d_Cross_CpsSph(*cps, *this, dst);
     }
diff --git a/src/SSystem/SComponent/c_m3d.cpp b/src/SSystem/SComponent/c_m3d.cpp
index 468646766..3a80d7176 100644
--- a/src/SSystem/SComponent/c_m3d.cpp
+++ b/src/SSystem/SComponent/c_m3d.cpp
@@ -65,31 +65,54 @@ inline f32 cM3d_Len2dSq(f32 x0, f32 y0, f32 x1, f32 y1) {
 }
 
 /* 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) {
-    /* Nonmatching */
+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;
-    f32 dot = (xd*xd + yd*yd);
+    f32 dot = (xd * xd + yd * yd);
     if (cM3d_IsZero(dot)) {
         *seg = 0.0f;
-        return ret;
+        return false;
     }
 
-    f32 t = (x1 * (xp - x0) + y1 * (yp - y0)) / dot;
-    if (t >= 0.0f && t <= 1.0f) {
+    f32 mag = (xd * (xp - x0) + yd * (yp - y0)) / dot;
+    if (mag >= 0.0f && mag <= 1.0f) {
         ret = true;
     }
 
-    *outx = x0 + xd * t;
-    *outy = y0 + yd * t;
+    *outx = x0 + xd * mag;
+    *outy = y0 + yd * mag;
     *seg = cM3d_Len2dSq(*outx, *outy, xp, yp);
     return ret;
 }
 
 /* 8024A56C-8024A670       .text cM3d_Len3dSqPntAndSegLine__FPC8cM3dGLinPC3VecP3VecPf */
-bool cM3d_Len3dSqPntAndSegLine(const cM3dGLin*, const Vec*, Vec*, f32*) {
-    /* Nonmatching */
+bool cM3d_Len3dSqPntAndSegLine(const cM3dGLin* line, const Vec* pos, Vec* pDst, f32* pT) {
+    Vec work2;
+    Vec work1;
+
+    PSVECSubtract(&line->mEnd, &line->mStart, &work2);
+    f32 midPoint = PSVECDotProduct(&work2, &work2);
+    if ((cM3d_IsZero(midPoint))) {
+        *pT = 0.0f;
+        return false;
+    }
+
+    PSVECSubtract(pos, &line->mStart, &work1);
+    midPoint = PSVECDotProduct(&work1, &work2) / midPoint;
+
+    bool success;
+    if (midPoint < 0.0f || midPoint > 1.0f) {
+        success = false;
+    } else {
+        success = true;
+    };
+
+    PSVECScale(&work2, &work2, midPoint);
+    PSVECAdd(&work2, &line->mStart, pDst);
+    *pT = PSVECSquareDistance(pDst, pos);
+    return success;
 }
 
 /* 8024A670-8024A6F0       .text cM3d_SignedLenPlaAndPos__FPC8cM3dGPlaPC3Vec */