Add method to calculate euler angles from quaternion

packages/redgeometry/src/primitives/quaternion.ts

@@ -199,6 +199,19 @@ export class Quaternion {
         return this.a === q.a && this.b === q.b && this.c === q.c && this.d === q.d;
     }
 
+    public getEulerAngles(): { x: number; y: number; z: number } {
+        const qa = this.a;
+        const qb = this.b;
+        const qc = this.c;
+        const qd = this.d;
+
+        const x = Math.atan2(2 * (qa * qb + qc * qd), qa * qa - qb * qb - qc * qc + qd * qd);
+        const y = Math.asin(2 * (qa * qc - qb * qd));
+        const z = Math.atan2(2 * (qa * qd + qb * qc), qa * qa + qb * qb - qc * qc - qd * qd);
+
+        return { x, y, z };
+    }
+
     public inverse(): Quaternion {
         const d = this.lenSq();
         return new Quaternion(this.a / d, -this.b / d, -this.c / d, -this.d / d);

packages/redgeometry/tests/expect.ts

@@ -37,3 +37,27 @@ export function expectToBeCloseQuaternion(q1: Quaternion, q2: Quaternion, numDig
     expect(q1.c).toBeCloseTo(q2.c, numDigits ?? 15);
     expect(q1.d).toBeCloseTo(q2.d, numDigits ?? 15);
 }
+
+export function expectToBeCloseEuler(
+    eul1: { x: number; y: number; z: number },
+    eul2: { x: number; y: number; z: number },
+    numDigits?: number,
+): void {
+    const tau = 2 * Math.PI;
+
+    const eul1x = (eul1.x + tau) % tau;
+    const eul1y = (eul1.y + tau) % tau;
+    const eul1z = (eul1.z + tau) % tau;
+
+    const eul2x = (eul2.x + tau) % tau;
+    const eul2y = (eul2.y + tau) % tau;
+    const eul2z = (eul2.z + tau) % tau;
+
+    console.log(eul1x, eul2x);
+    console.log(eul1y, eul2y);
+    console.log(eul1z, eul2z);
+
+    expect(eul1x).toBeCloseTo(eul2x, numDigits ?? 15);
+    expect(eul1y).toBeCloseTo(eul2y, numDigits ?? 15);
+    expect(eul1z).toBeCloseTo(eul2z, numDigits ?? 15);
+}

packages/redgeometry/tests/primitives/quaternion.spec.ts

@@ -3,7 +3,12 @@ import { Matrix4A } from "../../src/primitives/matrix.js";
 import { Point3 } from "../../src/primitives/point.js";
 import { Quaternion, RotationOrder } from "../../src/primitives/quaternion.js";
 import { Vector3 } from "../../src/primitives/vector.js";
-import { expectToBeClosePoint3, expectToBeCloseQuaternion, expectToBeCloseVector3 } from "../expect.js";
+import {
+    expectToBeCloseEuler,
+    expectToBeClosePoint3,
+    expectToBeCloseQuaternion,
+    expectToBeCloseVector3,
+} from "../expect.js";
 
 test("Quaternion - fromRotationAngleX", () => {
     const a = 1;
@@ -142,6 +147,16 @@ test("Quaternion - fromRotationEuler", () => {
     expectToBeCloseQuaternion(qf1, qf3);
 });
 
+test("Quaternion - getEulerAngles", () => {
+    // Pitch angle (y) must be in the range of `(-Math.PI / 2, Math.PI / 2)` to avoid gimbal lock
+    const eul1 = { x: 0.5, y: 1, z: 2 };
+
+    const q = Quaternion.fromRotationEuler(eul1.x, eul1.y, eul1.z, RotationOrder.XYZ);
+    const eul2 = q.getEulerAngles();
+
+    expectToBeCloseEuler(eul1, eul2);
+});
+
 test("Quaternion - inverse", () => {
     const q = new Quaternion(1, 2, 3, 4);
     const qInv = q.inverse();