From 10c3ed364eaf4d3f0103f0b23d03ddf155b6501b Mon Sep 17 00:00:00 2001
From: UnlikePaladin <36827970+UnlikePaladin@users.noreply.github.com>
Date: Fri, 5 Jul 2024 21:23:55 -0600
Subject: [PATCH] Change bezier calculations to use newton raphson and catmull
 rom ordering to match the definition

---
 .../org/figuramc/figura/utils/MathUtils.java  | 39 ++++++++++++-------
 1 file changed, 26 insertions(+), 13 deletions(-)

diff --git a/common/src/main/java/org/figuramc/figura/utils/MathUtils.java b/common/src/main/java/org/figuramc/figura/utils/MathUtils.java
index 059051500..288ad91a4 100644
--- a/common/src/main/java/org/figuramc/figura/utils/MathUtils.java
+++ b/common/src/main/java/org/figuramc/figura/utils/MathUtils.java
@@ -170,7 +170,7 @@ public static FiguraVec3 catmullrom(double delta, FiguraVec3 prevA, FiguraVec3 p
     // no idea how it works
     // it is the same function from minecraft, but using doubles instead
     public static double catmullrom(double delta, double prevA, double prevB, double nextA, double nextB) {
-        return 0.5 * (2 * prevB + (nextA - prevA) * delta + (2 * prevA - 5 * prevB + 4 * nextA - nextB) * delta * delta + (3 * prevB - prevA - 3 * nextA + nextB) * delta * delta * delta);
+        return 0.5 * (2 * prevB + (-prevA + nextA) * delta + (2 * prevA - 5 * prevB + 4 * nextA - nextB) * delta * delta + (-prevA + 3 * prevB - 3 * nextA + nextB) * delta * delta * delta);
     }
 
     // bezier function generated by ChatGPT
@@ -179,23 +179,36 @@ public static double bezier(double t, double p0, double p1, double p2, double p3
         return p0 * d * d * d + 3 * p1 * d * d * t + 3 * p2 * d * t * t + p3 * t * t * t;
     }
 
-    // secant method for finding bezier t based on x, also provided by ChatGPT
+    // newton raphson method for finding bezier t based on x, also provided by ChatGPT
+    // this approximation method is more accurate and often requires less iterations than secant based
     public static double bezierFindT(double x, double p0, double p1, double p2, double p3) {
-        double x0 = 0.4;
-        double x1 = 0.6;
-        double tolerance = 0.001;
+        double tolerance = 0.0001;
+        double t = 0.5;
         int iterations = 100;
 
         for (int i = 0; i < iterations; i++) {
-            double fx1 = bezier(x1, p0, p1, p2, p3) - x;
-            double fx0 = bezier(x0, p0, p1, p2, p3) - x;
-            double xNext = x1 - fx1 * (x1 - x0) / (fx1 - fx0);
-            if (Math.abs(xNext - x1) < tolerance)
-                return xNext;
-            x0 = x1;
-            x1 = xNext;
+            double xBezier = bezier(t, p0, p1, p2, p3);
+            double dxBezier = bezierDerivative(t, p0, p1, p2, p3);
+
+            if (dxBezier == 0) {
+                break; // Avoid division by zero
+            }
+
+            double tNext = t - (xBezier - x) / dxBezier;
+
+            if (Math.abs(tNext - t) < tolerance) {
+                return tNext;
+            }
+
+            t = tNext;
         }
 
-        return x1;
+        return t;
     }
+
+    private static double bezierDerivative(double t, double p0, double p1, double p2, double p3) {
+        double d = 1 - t;
+        return 3 * (p1 - p0) * d * d + 6 * (p2 - p1) * d * t + 3 * (p3 - p2) * t * t;
+    }
+
 }