From 40457bbdc0710531d7e5240b3f290309bb56e4bf Mon Sep 17 00:00:00 2001
From: jordan4ibanez <jordan4ibanez@users.noreply.github.com>
Date: Fri, 19 Apr 2024 04:40:11 -0400
Subject: [PATCH] Circular lerp

---
 source/utility/quaternion.ts | 33 +++++++++++++++++++++++++++++++++
 1 file changed, 33 insertions(+)

diff --git a/source/utility/quaternion.ts b/source/utility/quaternion.ts
index ce0c9837..724f32fb 100644
--- a/source/utility/quaternion.ts
+++ b/source/utility/quaternion.ts
@@ -13,6 +13,10 @@ namespace utility {
     w: number = 1;
 
     constructor(vec: Vec3) {
+      this.fromVec(vec);
+    }
+
+    fromVec(vec: Vec3): void {
       let sx = math.sin(vec.x * 0.5);
       let cx = math.cosFromSin(sx, vec.x * 0.5);
       let sy = math.sin(vec.y * 0.5);
@@ -29,5 +33,34 @@ namespace utility {
       this.y = cx * sycz - sx * cysz;
       this.z = cx * cysz + sx * sycz;
     }
+
+    identity(): void {
+      this.x = 0;
+      this.y = 0;
+      this.z = 0;
+      this.w = 0;
+    }
+
+    slerp(target: Quaternion, alpha: number) {
+      let cosom = math.fma(this.x, target.x, math.fma(this.y, target.y, math.fma(this.z, target.z, this.w * target.w)));
+      let absCosom = math.abs(cosom);
+      let scale0, scale1;
+      if (1.0 - absCosom > 1E-6) {
+        let sinSqr = 1.0 - absCosom * absCosom;
+        let sinom = math.invsqrt(sinSqr);
+        let omega = math.atan2(sinSqr * sinom, absCosom);
+        scale0 = (math.sin((1.0 - alpha) * omega) * sinom);
+        scale1 = (math.sin(alpha * omega) * sinom);
+      } else {
+        scale0 = 1.0 - alpha;
+        scale1 = alpha;
+      }
+      scale1 = cosom >= 0.0 ? scale1 : -scale1;
+      this.x = math.fma(scale0, this.x, scale1 * target.x);
+      this.y = math.fma(scale0, this.y, scale1 * target.y);
+      this.z = math.fma(scale0, this.z, scale1 * target.z);
+      this.w = math.fma(scale0, this.w, scale1 * target.w);
+    }
+
   }
 }
\ No newline at end of file