chore: Write the Bézier math with vector operations and atan2

curvebuilding-core/src/main/java/me/kous500/curvebuilding/curve/BezierSegment.java

@@ -66,17 +66,17 @@ public Vector3 point(double t) {
             throw new IllegalArgumentException("t must be in the range 0 to 1");
         }
 
+        // The cubic Bernstein polynomials: how much each point pulls the curve at t
         double u = 1 - t;
-        double bernstein0 = u * u * u;
-        double bernstein1 = 3 * u * u * t;
-        double bernstein2 = 3 * u * t * t;
-        double bernstein3 = t * t * t;
-
-        return Vector3.at(
-                bernstein0 * p0.x() + bernstein1 * p1.x() + bernstein2 * p2.x() + bernstein3 * p3.x(),
-                bernstein0 * p0.y() + bernstein1 * p1.y() + bernstein2 * p2.y() + bernstein3 * p3.y(),
-                bernstein0 * p0.z() + bernstein1 * p1.z() + bernstein2 * p2.z() + bernstein3 * p3.z()
-        );
+        double cubic0 = u * u * u;
+        double cubic1 = 3 * u * u * t;
+        double cubic2 = 3 * u * t * t;
+        double cubic3 = t * t * t;
+
+        return p0.multiply(cubic0)
+                .add(p1.multiply(cubic1))
+                .add(p2.multiply(cubic2))
+                .add(p3.multiply(cubic3));
     }
 
     /**
@@ -90,24 +90,31 @@ public Vector3 blockCenter(double t) {
     }
 
     /**
-     * The direction of the curve at {@code t} on the xz plane, as the angle in radians from +z towards +x. 0 when the
-     * curve does not move on the xz plane there.
+     * The direction of the curve at {@code t} on the xz plane, as the angle in radians from +z towards +x, from
+     * {@code -Math.PI} to {@code Math.PI}. 0 when the curve does not move on the xz plane there.
      *
      * @param t where on the segment, from 0 to 1
      * @return the angle in radians
      */
     public double angle(double t) {
-        double dx = 3 * t * t * (-p0.x() + 3 * p1.x() - 3 * p2.x() + p3.x()) + 6 * t * (p0.x() - 2 * p1.x() + p2.x())
-                + 3 * (-p0.x() + p1.x());
-        double dz = 3 * t * t * (-p0.z() + 3 * p1.z() - 3 * p2.z() + p3.z()) + 6 * t * (p0.z() - 2 * p1.z() + p2.z())
-                + 3 * (-p0.z() + p1.z());
-
-        double angle = Math.atan(dx / dz);
-        if (Double.isNaN(angle)) {
-            angle = dx > 0 ? Math.PI / 2 : dx < 0 ? -Math.PI / 2 : 0;
-        }
-        if (dz < 0) angle += Math.PI;
-        return angle;
+        // The direction of the curve, its derivative divided by 3: the steps between the points, weighted by the
+        // quadratic Bernstein polynomials
+        double u = 1 - t;
+        double quadratic0 = u * u;
+        double quadratic1 = 2 * u * t;
+        double quadratic2 = t * t;
+
+        Vector3 step0 = p1.subtract(p0);
+        Vector3 step1 = p2.subtract(p1);
+        Vector3 step2 = p3.subtract(p2);
+
+        Vector3 direction = step0.multiply(quadratic0)
+                .add(step1.multiply(quadratic1))
+                .add(step2.multiply(quadratic2));
+
+        // atan2 gives Math.PI or -Math.PI for a zero vector when z is -0.0
+        if (direction.x() == 0 && direction.z() == 0) return 0;
+        return Math.atan2(direction.x(), direction.z());
     }
 
     /**

curvebuilding-core/src/main/java/me/kous500/curvebuilding/math/Vector3.java

@@ -114,6 +114,16 @@ public Vector3 subtract(double x, double y, double z) {
         return at(this.x - x, this.y - y, this.z - z);
     }
 
+    /**
+     * Perform scalar multiplication and return a new vector.
+     *
+     * @param n the value to multiply by
+     * @return a new vector
+     */
+    public Vector3 multiply(double n) {
+        return at(x * n, y * n, z * n);
+    }
+
     /**
      * Divide this vector by another vector on each component.
      *

curvebuilding-core/src/test/java/me/kous500/curvebuilding/curve/BezierSegmentTest.java

@@ -79,6 +79,7 @@ class Angle {
                 "0,  -1, 3.141592653589793",
                 "-1, 0,  -1.5707963267948966",
                 "1,  1,  0.7853981633974483",
+                "-1, -1, -2.356194490192345",
                 "0,  0,  0",
         })
         void followsTheCurveOnTheXzPlane(double dx, double dz, double expected) {

curvebuilding-core/src/test/java/me/kous500/curvebuilding/math/Vector3Test.java

@@ -23,6 +23,12 @@ void addAndSubtract() {
         assertVector(-3, -3, -3, v.subtract(Vector3.at(4, 5, 6)));
     }
 
+    @Test
+    void multiply() {
+        assertVector(2, -4, 6, Vector3.at(1, -2, 3).multiply(2));
+        assertVector(0, 0, 0, Vector3.at(1, -2, 3).multiply(0));
+    }
+
     @Test
     void withLengthKeepsTheDirection() {
         assertVector(6, 8, 0, Vector3.at(3, 4, 0).withLength(10));