fix: SLERP is now stable

src/02-quaternion/Quaternion.ts

@@ -102,45 +102,46 @@ export class Quaternion {
             return Quaternion.add(Quaternion.multiplyScalar(qa, 1 - t), Quaternion.multiplyScalar(qb, t));
         } else {
             const theta = Math.acos(cosTheta);
-            const thetap = theta * t;
-            const qperp = Quaternion.normalize(Quaternion.subtract(qb, Quaternion.multiplyScalar(qa, cosTheta)));
-            return Quaternion.add(Quaternion.multiplyScalar(qa, Math.cos(thetap)), Quaternion.multiplyScalar(qperp, Math.sin(thetap)));
+            const sinTheta = Math.sin(theta);
+            const sinThetaA = Math.sin(theta * (1 - t));
+            const sinThetaB = Math.sin(theta * t);
+            return Quaternion.add(Quaternion.multiplyScalar(qa, sinThetaA / sinTheta), Quaternion.multiplyScalar(qb, sinThetaB / sinTheta));
         }
     }
 
-    // public static slerp(qa: Quaternion, qb: Quaternion, t: number): Quaternion {
+    public static slerpCG3(qa: Quaternion, qb: Quaternion, t: number): Quaternion {
 
-    //     let cq1 = qa.clone();
-    //     let cq2 = qb.clone();
+        let cq1 = qa.clone();
+        let cq2 = qb.clone();
 
-    //     cq1 = Quaternion.normalize(cq1);
-    //     cq2 = Quaternion.normalize(cq2);
+        cq1 = Quaternion.normalize(cq1);
+        cq2 = Quaternion.normalize(cq2);
 
-    //     let dot = Quaternion.dot(cq1, cq2);
+        let dot = Quaternion.dot(cq1, cq2);
 
-    //     if (dot < 0.0) {
-    //         cq2 = Quaternion.inverse(cq2);
-    //         dot = -dot;
-    //     }
+        if (dot < 0.0) {
+            cq2 = Quaternion.inverse(cq2);
+            dot = -dot;
+        }
 
-    //     if (dot > 0.9995) {
-    //         return Quaternion.add(cq1, Quaternion.multiplyScalar(
-    //             Quaternion.subtract(cq2, cq1), t));
-    //     }
+        if (dot > 0.9995) {
+            return Quaternion.add(cq1, Quaternion.multiplyScalar(
+                Quaternion.subtract(cq2, cq1), t));
+        }
 
-    //     const theta0 = Math.acos(dot);
-    //     const theta = theta0 * t;
+        const theta0 = Math.acos(dot);
+        const theta = theta0 * t;
 
-    //     const sinTheta0 = Math.sin(theta0);
-    //     const sinTheta = Math.sin(theta);
-    //     const s0 = Math.cos(theta0) - dot * sinTheta / sinTheta0;
-    //     const s1 = sinTheta / sinTheta0;
+        const sinTheta0 = Math.sin(theta0);
+        const sinTheta = Math.sin(theta);
+        const s0 = Math.cos(theta0) - dot * sinTheta / sinTheta0;
+        const s1 = sinTheta / sinTheta0;
 
-    //     return Quaternion.add(
-    //         Quaternion.multiplyScalar(cq1, s0),
-    //         Quaternion.multiplyScalar(cq2, s1)
-    //     )
-    // }
+        return Quaternion.add(
+            Quaternion.multiplyScalar(cq1, s0),
+            Quaternion.multiplyScalar(cq2, s1)
+        )
+    }
 
     public static add(qa: Quaternion, qb: Quaternion): Quaternion {
         return new Quaternion(qa._x + qb._x, qa._y + qb._y, qa._z + qb._z, qa._w + qb._w);