Fix Bezier tangent calculation

Argh...!!

1 files changed, 15 insertions, 7 deletions

diff --git a/src/RaceTrack.java b/src/RaceTrack.java
index 6662196..3828d54 100644
--- a/src/RaceTrack.java
+++ b/src/RaceTrack.java
@@ -332,17 +332,25 @@ class RaceTrack extends BetterBase {
     public static Vector getCubicBezierTng(double t, Vector P0, Vector P1,
                                                      Vector P2, Vector P3) {
         // The tangent is the derivative of the Bézier curve P(t).
-        // dP(u) / du =  3  (1 - u)^2               . P0 +
-        //              (3  (1 - u)^2 + 6u (1 - u)) . P1 +
+        // dP(u) / du = -3  (1 - u)^2               . P0 +
+        //              (3  (1 - u)^2 - 6u (1 - u)) . P1 +
         //              (6u (1 - u) - 3u^2)         . P2 +
         //               3u^2                       . P3
-        //            =  3   (1 - u)^2  . P0 +
-        //                   (3 - 3u^2) . P1 +
+        //            = -3   (1 - u)^2  . P0 +
+        //               (-9u + 3) (1-u). P1 +
         //                  (6u - 9u^2) . P2 +
         //                        3u^2  . P3
-        return       P0.scale(3 *                 pow(1 - t, 2))
-                .add(P1.scale(3     - 3 * t * t))
-                .add(P2.scale(6 * t + 9 * t * t))
+        return       P0.scale(-3 *                pow(1 - t, 2))
+                .add(P1.scale((-9 * t + 3) * (1 - t)))
+                .add(P2.scale(6 * t - 9 * t * t))
                 .add(P3.scale(3 *         t * t));
     }
+
+    /**
+     * Obtains the tangent on Bézier curve from points P, starting at index i.
+     */
+    public static Vector getCubicBezierTng(double t, Vector[] P, int i) {
+        Vector P4 = P[(i + 3) % P.length];
+        return getCubicBezierTng(t, P[i], P[i + 1], P[i + 2], P4);
+    }
 }