matched Math3D_CylVsLineSeg and fixed a Math_ error (#564)

* matched cylvsline

* slight name adjustment

* format

* and asm

* comment cleanup

* fixed misspelling and added mathfixer tool

* formatting

Co-authored-by: petrie911 <pmontag@DESKTOP-LG8A167.localdomain>

1 files changed, 150 insertions, 186 deletions

diff --git a/src/code/sys_math3d.c b/src/code/sys_math3d.c
index 04a7fa864..6ca814f36 100644
--- a/src/code/sys_math3d.c
+++ b/src/code/sys_math3d.c
@@ -62,6 +62,11 @@ s32 Math3D_LineSegMakePerpLineSeg(Vec3f* lineAPointA, Vec3f* lineAPointB, Vec3f*
     Vec3f lineABPointADiff;
     f32 t;
     f32 t2;
+    f32 tempf1;
+    f32 tempf2;
+    f32 tempf3;
+    f32 tempf4;
+    f32 tempf5;
 
     lineADiff.x = lineAPointB->x - lineAPointA->x;
     lineADiff.y = lineAPointB->y - lineAPointA->y;
@@ -70,21 +75,18 @@ s32 Math3D_LineSegMakePerpLineSeg(Vec3f* lineAPointA, Vec3f* lineAPointB, Vec3f*
     lineBDiff.y = lineBPointB->y - lineBPointA->y;
     lineBDiff.z = lineBPointB->z - lineBPointA->z;
 
-    if (IS_ZERO(SQ(lineBDiff.x) + SQ(lineBDiff.y) + SQ(lineBDiff.z))) {
+    if (IS_ZERO(SQXYZ(lineBDiff))) {
         return false;
     }
-
-    sp5C = ((lineADiff.x * lineBDiff.x) + (lineADiff.y * lineBDiff.y) + (lineADiff.z * lineBDiff.z)) *
-           (1.0f / (SQ(lineBDiff.x) + SQ(lineBDiff.y) + SQ(lineBDiff.z)));
+    tempf1 = 1.0f / SQXYZ(lineBDiff);
+    sp5C = DOTXYZ(lineADiff, lineBDiff) * tempf1;
 
     lineABPointADiff.x = lineAPointA->x - lineBPointA->x;
     lineABPointADiff.y = lineAPointA->y - lineBPointA->y;
     lineABPointADiff.z = lineAPointA->z - lineBPointA->z;
 
     // most reordering is here.
-    temp_f18 =
-        ((lineABPointADiff.x * lineBDiff.x) + (lineABPointADiff.y * lineBDiff.y) + (lineABPointADiff.z * lineBDiff.z)) *
-        (1.0f / (SQ(lineBDiff.x) + SQ(lineBDiff.y) + SQ(lineBDiff.z)));
+    temp_f18 = DOTXYZ(lineABPointADiff, lineBDiff) * tempf1;
 
     sp4C = lineADiff.x - (lineBDiff.x * sp5C);
     sp50 = lineADiff.y - (lineBDiff.y * sp5C);
@@ -92,7 +94,6 @@ s32 Math3D_LineSegMakePerpLineSeg(Vec3f* lineAPointA, Vec3f* lineAPointB, Vec3f*
     if (IS_ZERO(SQ(sp4C) + SQ(sp50) + SQ(sp30))) {
         return false;
     }
-
     t = SQ(sp4C) + SQ(sp50) + SQ(sp30);
     temp_f0_4 = -((sp4C * (lineABPointADiff.x - (lineBDiff.x * temp_f18))) +
                   (sp50 * (lineABPointADiff.y - (lineBDiff.y * temp_f18))) +
@@ -1624,72 +1625,28 @@ s32 Math3D_PointInCyl(Cylinder16* cyl, Vec3f* point) {
     }
 }
 
-#ifdef NON_MATCHING
 s32 Math3D_CylVsLineSeg(Cylinder16* cyl, Vec3f* linePointA, Vec3f* linePointB, Vec3f* intersectA, Vec3f* intersectB) {
-    Vec3f pointACylBottomDiff;
-    Vec3f pointBCylBottomDiff;
-    Vec3f spD4;
-    f32 spD0;
-    f32 spCC;
-    f32 spB8;
-    s32 sp9C;
-    Vec3f sp6C[4];
-    s32 sp68;
-    f32 sp4C;
-    f32 sp2C;
-    f32 sp28;
-    f32* temp_a0;
-    f32* temp_a1;
-    f32* temp_s0;
-    f32 temp_f0;
-    f32 temp_f0_2;
-    f32 temp_f0_3;
-    f32 temp_f0_4;
-    f32 temp_f0_5;
-    f32 temp_f0_6;
-    f32 temp_f10;
-    f32 temp_f12;
-    f32 temp_f12_2;
+    Vec3f cylToPtA;
+    Vec3f cylToPtB;
+    Vec3f ptAToPtB;
+    f32 fracA;
+    f32 fracB;
+    f32 fracBase;
+    f32 zero = 0.0f;
+    f32 pad;
     f32 cylRadiusSq;
-    f32 temp_f14_2;
-    f32 temp_f14_3;
-    f32 temp_f16;
-    f32 temp_f16_2;
-    f32 temp_f18;
-    f32 temp_f2;
-    f32 temp_f2_2;
-    f32 temp_f2_3;
-    f32 temp_f2_4;
-    f32 temp_f2_5;
-    f32 temp_f2_6;
-    f32 temp_f2_7;
-    s32 temp_a0_2;
-    s32 temp_t0;
-    s32 temp_v0;
-    s32 temp_v1;
-    void* temp_t2;
-    s32 phi_a1;
-    s32 phi_a1_2;
-    f32 phi_f2;
-    s32 phi_v0;
-    s32 phi_v0_2;
-    s32 phi_v1;
-    s32 phi_v1_2;
-    s32 phi_a2;
-    s32 phi_a1_3;
-    s32 phi_a2_2;
-    s32 phi_a1_4;
-    s32 phi_v0_3;
-    s32 phi_t0;
-    s32 phi_v1_3;
-    s32 phi_v1_4;
-    s32 phi_a2_3;
-    s32 phi_a2_4;
-    s32 phi_t0_2;
-    s32 phi_v1_5;
-    s32 phi_t0_3;
-
-    sp9C = 0;
+    f32 radSqDiff;
+    f32 distCent2;
+    f32 dot2AB;
+    s32 sideIntA;
+    s32 sideIntB;
+    s32 intBeyondA;
+    s32 intBeyondB;
+    s32 intFlags = 0;
+    Vec3f intPts[4];
+    s32 count;
+    s32 i;
+
     if (Math3D_PointInCyl(cyl, linePointA) && Math3D_PointInCyl(cyl, linePointB)) {
         // both points are in the cylinder
         *intersectA = *linePointA;
@@ -1697,151 +1654,158 @@ s32 Math3D_CylVsLineSeg(Cylinder16* cyl, Vec3f* linePointA, Vec3f* linePointB, V
         return 2;
     }
 
-    pointACylBottomDiff.x = linePointA->x - cyl->pos.x;
-    pointACylBottomDiff.y = linePointA->y - cyl->pos.y - cyl->yShift;
-    pointACylBottomDiff.z = linePointA->z - cyl->pos.z;
-    pointBCylBottomDiff.x = linePointB->x - cyl->pos.x;
-    pointBCylBottomDiff.y = linePointB->y - cyl->pos.y - cyl->yShift;
-    pointBCylBottomDiff.z = linePointB->z - cyl->pos.z;
-    Math_Vec3f_Diff(&pointBCylBottomDiff, &pointACylBottomDiff, &spD4);
+    cylToPtA.x = linePointA->x - cyl->pos.x;
+    cylToPtA.y = linePointA->y - cyl->pos.y - cyl->yShift;
+    cylToPtA.z = linePointA->z - cyl->pos.z;
+    cylToPtB.x = linePointB->x - cyl->pos.x;
+    cylToPtB.y = linePointB->y - cyl->pos.y - cyl->yShift;
+    cylToPtB.z = linePointB->z - cyl->pos.z;
+    Math_Vec3f_Diff(&cylToPtB, &cylToPtA, &ptAToPtB);
     cylRadiusSq = SQ(cyl->radius);
-    if (!IS_ZERO(spD4.y)) {
-        if (1) {}
-        if ((-pointACylBottomDiff.y / spD4.y) >= 0.0f) {
-            if ((-pointACylBottomDiff.y / spD4.y) <= 1.0f) {
-                if ((SQ((spD4.x * (-pointACylBottomDiff.y / spD4.y)) + pointACylBottomDiff.x) +
-                     SQ((spD4.z * (-pointACylBottomDiff.y / spD4.y)) + pointACylBottomDiff.z)) < cylRadiusSq) {
-                    sp6C[0].x =
-                        (f32)cyl->pos.x + ((spD4.x * (-pointACylBottomDiff.y / spD4.y)) + pointACylBottomDiff.x);
-                    sp6C[0].y = (f32)cyl->pos.y + (f32)cyl->yShift;
-                    sp6C[0].z =
-                        (f32)cyl->pos.z + ((spD4.z * (-pointACylBottomDiff.y / spD4.y)) + pointACylBottomDiff.z);
-                    sp9C |= 1;
-                }
+
+    /**
+     * This section checks for intersections with the cylinder's base and top
+     */
+    if (!IS_ZERO(ptAToPtB.y)) {
+        // fraction of length along AB to reach y = 0
+        fracBase = -cylToPtA.y / ptAToPtB.y;
+        if ((0.0f <= fracBase) && (fracBase <= 1.0f)) {
+            f32 baseIntX = (ptAToPtB.x * fracBase) + cylToPtA.x;
+            f32 baseIntZ = (ptAToPtB.z * fracBase) + cylToPtA.z;
+
+            if (SQ(baseIntX) + SQ(baseIntZ) < cylRadiusSq) {
+                // adds base intersection point to intPts and sets its flag
+                intPts[0].x = cyl->pos.x + baseIntX;
+                intPts[0].y = (f32)cyl->pos.y + cyl->yShift;
+                intPts[0].z = cyl->pos.z + baseIntZ;
+                intFlags |= 1;
             }
         }
-
-        phi_f2 = ((cyl->height - pointACylBottomDiff.y) / spD4.y);
-        if (phi_f2 >= 0.0f) {
-            if (phi_f2 <= 1.0f) {
-                if ((SQ(pointACylBottomDiff.x + (spD4.x * phi_f2)) + SQ(pointACylBottomDiff.z + (spD4.z * phi_f2))) <
-                    cylRadiusSq) {
-                    sp6C[1].x = (f32)cyl->pos.x + pointACylBottomDiff.x + (spD4.x * phi_f2);
-                    sp6C[1].y = (f32)cyl->pos.y + cyl->height + cyl->yShift;
-                    sp6C[1].z = (f32)cyl->pos.z + pointACylBottomDiff.z + (spD4.x * phi_f2);
-                    sp9C |= 2;
-                }
+        // fraction of length along AB to reach y = cyl->height
+        fracA = (cyl->height - cylToPtA.y) / ptAToPtB.y;
+        if ((0.0f <= fracA) && (fracA <= 1.0f)) {
+            f32 topIntX = ptAToPtB.x * fracA + cylToPtA.x;
+            f32 topIntZ = ptAToPtB.z * fracA + cylToPtA.z;
+
+            if (SQ(topIntX) + SQ(topIntZ) < cylRadiusSq) {
+                // adds top intersection point to intPts and sets its flag
+                intPts[1].x = cyl->pos.x + topIntX;
+                intPts[1].y = (f32)cyl->pos.y + cyl->yShift + cyl->height;
+                intPts[1].z = cyl->pos.z + topIntZ;
+                intFlags |= 2;
             }
         }
     }
-
-    spB8 = SQ(pointACylBottomDiff.x) + SQ(pointACylBottomDiff.z) - cylRadiusSq; // 498c
-    temp_f12_2 = SQ(spD4.z) + SQ(spD4.x);
-    temp_f18 = temp_f12_2 * 2.0f;
-    if (!IS_ZERO(temp_f18)) {
-        temp_f2_3 = (spD4.x * pointACylBottomDiff.x) + (spD4.z * pointACylBottomDiff.z);
-        temp_f14_2 = temp_f2_3 + temp_f2_3;
-        temp_f0_3 = temp_f14_2 * temp_f14_2;
-        temp_f16_2 = (4.0f * temp_f12_2) * spB8;
-        if (temp_f0_3 < temp_f16_2) {
-            return false;
+    /**
+     * This section finds the points of intersection of the infinite line containing AB with the side of the infinite
+     * cylinder containing cyl. Intersection points beyond the bounds of the segment and cylinder are filtered out
+     * afterward.
+     */
+    radSqDiff = SQXZ(cylToPtA) - cylRadiusSq;
+    if (!IS_ZERO(2.0f * SQXZ(ptAToPtB))) {
+        dot2AB = 2.0f * DOTXZ(ptAToPtB, cylToPtA);
+        if (SQ(dot2AB) < 4.0f * SQXZ(ptAToPtB) * radSqDiff) {
+            // Line's closest xz-approach is outside cylinder. No intersections.
+            return 0;
         }
-
-        phi_a1 = (temp_f0_3 - temp_f16_2) > 0.0f;
-        phi_f2 = (sqrtf((temp_f0_3 - temp_f16_2)) - temp_f14_2) / temp_f18;
-
-        if (phi_a1) {
-            spCC = (-temp_f14_2 - sqrtf((temp_f0_3 - temp_f16_2))) / temp_f18;
+        if (SQ(dot2AB) - (4.0f * SQXZ(ptAToPtB) * radSqDiff) > zero) {
+            sideIntA = sideIntB = 1;
+        } else {
+            // Line is tangent in xz-plane. At most 1 side intersection.
+            sideIntA = 1;
+            sideIntB = 0;
         }
-    } else if (!IS_ZERO(((spD4.x * pointACylBottomDiff.x) + (spD4.z * pointACylBottomDiff.z)) +
-                        ((spD4.x * pointACylBottomDiff.x) + (spD4.z * pointACylBottomDiff.z)))) {
-        phi_f2 = -spB8 / ((spD4.x * pointACylBottomDiff.x) + (spD4.z * pointACylBottomDiff.z)) +
-                 ((spD4.x * pointACylBottomDiff.x) + (spD4.z * pointACylBottomDiff.z));
-        phi_a2 = 1;
-        phi_a1 = 0;
+        distCent2 = sqrtf(SQ(dot2AB) - (4.0f * SQXZ(ptAToPtB) * radSqDiff));
+        if (sideIntA == 1) {
+            // fraction of length along AB for side intersection closer to A
+            fracA = (distCent2 - dot2AB) / (2.0f * SQXZ(ptAToPtB));
+        }
+        if (sideIntB == 1) {
+            // fraction of length along AB for side intersection closer to B
+            fracB = (-dot2AB - distCent2) / (2.0f * SQXZ(ptAToPtB));
+        }
+    } else if (!IS_ZERO(2.0f * DOTXZ(ptAToPtB, cylToPtA))) {
+        // Used if the line segment is nearly vertical. Unclear what it's calculating.
+        fracA = -radSqDiff / (2.0f * DOTXZ(ptAToPtB, cylToPtA));
+        sideIntA = 1;
+        sideIntB = 0;
     } else {
         return 0;
     }
-
-    if (!phi_a1) {
-        if (phi_f2 < 0.0f || phi_f2 > 1.0f) {
-            return false;
+    // checks for intersection points outside the bounds of the segment
+    if (!sideIntB) {
+        if (fracA < 0.0f || 1.0f < fracA) {
+            return 0;
         }
     } else {
-        phi_a2 = phi_f2 < 0.0f || phi_f2 > 1.0f;
-
-        phi_a1 = spCC < 0.0f || spCC > 1.0f;
-
-        if (phi_a1 && phi_a2) {
+        intBeyondA = fracA < 0.0f || 1.0f < fracA;
+        intBeyondB = fracB < 0.0f || 1.0f < fracB;
+        if (intBeyondA && intBeyondB) {
             return 0;
         }
-
-        if (phi_a2) {
-            phi_a2 = 0;
+        if (intBeyondA) {
+            sideIntA = 0;
         }
-
-        if (phi_a1) {
-            phi_a1 = 0;
+        if (intBeyondB) {
+            sideIntB = 0;
         }
     }
-
-    if ((phi_a2 == 1) && (((phi_f2 * spD4.y) + pointACylBottomDiff.y) < 0.0f ||
-                          cyl->height < ((phi_f2 * spD4.y) + pointACylBottomDiff.y))) {
-        phi_a2 = 0;
+    // checks for intersection points outside the bounds of the cylinder
+    if ((sideIntA == 1) &&
+        ((fracA * ptAToPtB.y + cylToPtA.y) < 0.0f || cyl->height < (fracA * ptAToPtB.y + cylToPtA.y))) {
+        sideIntA = 0;
     }
-
-    if ((phi_a1 == 1) &&
-        (((spCC * spD4.y) + pointACylBottomDiff.y) < 0.0f || cyl->height < ((spCC * spD4.y) + pointACylBottomDiff.y))) {
-        phi_a1 = 0;
+    if ((sideIntB == 1) &&
+        ((fracB * ptAToPtB.y + cylToPtA.y) < 0.0f || cyl->height < (fracB * ptAToPtB.y + cylToPtA.y))) {
+        sideIntB = 0;
     }
-    if (phi_a2 == 0 && phi_a1 == 0) {
+    if (sideIntA == 0 && sideIntB == 0) {
         return 0;
     }
-
-    if (phi_a2 == 1 && phi_a1 == 1) {
-        sp6C[2].x = ((phi_f2 * spD4.x) + pointACylBottomDiff.x) + (f32)cyl->pos.x;
-        sp6C[2].y = (((phi_f2 * spD4.y) + pointACylBottomDiff.y) + (f32)cyl->pos.y) + (f32)cyl->yShift;
-        sp6C[2].z = ((phi_f2 * spD4.z) + pointACylBottomDiff.z) + (f32)cyl->pos.z;
-        sp6C[2].x = ((spCC * spD4.x) + pointACylBottomDiff.x) + (f32)cyl->pos.x;
-        sp6C[2].y = (((spCC * spD4.y) + pointACylBottomDiff.y) + (f32)cyl->pos.y) + (f32)cyl->yShift;
-        sp6C[2].z = ((spCC * spD4.z) + pointACylBottomDiff.z) + (f32)cyl->pos.z;
-        sp9C = (sp9C | 4) | 8;
-    } else if (phi_a2 == 1) {
-        sp6C[2].x = ((phi_f2 * spD4.x) + pointACylBottomDiff.x) + (f32)cyl->pos.x;
-        sp6C[2].y = (((phi_f2 * spD4.y) + pointACylBottomDiff.y) + (f32)cyl->pos.y) + (f32)cyl->yShift;
-        sp6C[2].z = ((phi_f2 * spD4.z) + pointACylBottomDiff.z) + (f32)cyl->pos.z;
-        sp9C |= 4;
-    } else if (phi_a1 == 1) {
-        sp6C[2].x = ((spCC * spD4.x) + pointACylBottomDiff.x) + (f32)cyl->pos.x;
-        sp6C[2].y = (((spCC * spD4.y) + pointACylBottomDiff.y) + (f32)cyl->pos.y) + (f32)cyl->yShift;
-        sp6C[2].z = ((spCC * spD4.z) + pointACylBottomDiff.z) + (f32)cyl->pos.z;
-        sp9C |= 4;
-    }
-
-    for (phi_v0_3 = 0, phi_v1_3 = 0; phi_v0_3 < 4; phi_v0_3++) {
-        if (sp9C & (1 << phi_v0_3)) {
-            if (phi_v1_3 == 0) {
-                *intersectA = sp6C[phi_v0_3];
-            } else if (phi_v1_3 == 1) {
-                if (Math3D_Vec3fDistSq(intersectA, linePointA) < Math3D_Vec3fDistSq(intersectA, &sp6C[phi_v0_3])) {
-                    *intersectB = sp6C[phi_v0_3];
+    // Adds intersection points to intPts and sets side A and side B flags
+    if (sideIntA == 1 && sideIntB == 1) {
+        intPts[2].x = (fracA * ptAToPtB.x + cylToPtA.x) + cyl->pos.x;
+        intPts[2].y = (fracA * ptAToPtB.y + cylToPtA.y) + cyl->pos.y + cyl->yShift;
+        intPts[2].z = (fracA * ptAToPtB.z + cylToPtA.z) + cyl->pos.z;
+        intFlags |= 4;
+        intPts[3].x = (fracB * ptAToPtB.x + cylToPtA.x) + cyl->pos.x;
+        intPts[3].y = (fracB * ptAToPtB.y + cylToPtA.y) + cyl->pos.y + cyl->yShift;
+        intPts[3].z = (fracB * ptAToPtB.z + cylToPtA.z) + cyl->pos.z;
+        intFlags |= 8;
+    } else if (sideIntA == 1) {
+        intPts[2].x = (fracA * ptAToPtB.x + cylToPtA.x) + cyl->pos.x;
+        intPts[2].y = (fracA * ptAToPtB.y + cylToPtA.y) + cyl->pos.y + cyl->yShift;
+        intPts[2].z = (fracA * ptAToPtB.z + cylToPtA.z) + cyl->pos.z;
+        intFlags |= 4;
+    } else if (sideIntB == 1) {
+        intPts[2].x = (fracB * ptAToPtB.x + cylToPtA.x) + cyl->pos.x;
+        intPts[2].y = (fracB * ptAToPtB.y + cylToPtA.y) + cyl->pos.y + cyl->yShift;
+        intPts[2].z = (fracB * ptAToPtB.z + cylToPtA.z) + cyl->pos.z;
+        intFlags |= 4;
+    }
+
+    /**
+     * Places the found intersection points into intersectA and intersectB. IntersectA is always closer to point A
+     */
+    for (count = 0, i = 0; i < 4; i++) {
+        if (intFlags & (1 << i)) {
+            if (count == 0) {
+                *intersectA = intPts[i];
+            } else if (count == 1) {
+                if (Math3D_Vec3fDistSq(intersectA, linePointA) < Math3D_Vec3fDistSq(intersectA, &intPts[i])) {
+                    *intersectB = intPts[i];
                 } else {
                     *intersectB = *intersectA;
-                    *intersectA = sp6C[phi_v0_3];
+                    *intersectA = intPts[i];
                 }
                 break;
             }
+            count++;
         }
-        phi_v1_3++;
     }
-
-    return phi_v1_3;
+    return count;
 }
 
-#else
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_math3d/Math3D_CylVsLineSeg.s")
-#endif
-
 /*
  * Determines if `cyl` and `tri` are touching.  The point of intersection
  * is placed in `intersect` Returns 1 if they are touching, 0 otherwise.