feat: Quaternion

src/quaternion.ts

@@ -0,0 +1,51 @@
+import { Vec3 } from "./vec";
+
+export class Quaternion {
+  public constructor(
+    public x: number,
+    public y: number,
+    public z: number,
+    public w: number,
+  ) {}
+
+  public fromXYZ({
+    x,
+    y,
+    z,
+    w,
+  }: {
+    x: number;
+    y: number;
+    z: number;
+    w: number;
+  }) {
+    return new Quaternion(x, y, z, w);
+  }
+
+  public toEuler(): Vec3 {
+    const { x, y, z, w } = this;
+
+    const sinr_cosp = 2 * (w * x + y * z);
+    const cosr_cosp = 1 - 2 * (x * x + y * y);
+    const roll = math.atan2(sinr_cosp, cosr_cosp);
+
+    const sinp = 2 * (w * y - z * x);
+    let pitch: number;
+    if (math.abs(sinp) >= 1) {
+      pitch = (sign(sinp) * Math.PI) / 2;
+    } else {
+      pitch = math.asin(sinp);
+    }
+
+    const siny_cosp = 2 * (w * z + x * y);
+    const cosy_cosp = 1 - 2 * (y * y + z * z);
+    const yaw = math.atan2(siny_cosp, cosy_cosp);
+
+    return new Vec3(roll, pitch, yaw);
+  }
+}
+
+// fast sign without zero
+function sign(v: number) {
+  return v > 0 ? 1 : -1;
+}