Add quaternion to euler utility to user scripts utilties (#4009)

Co-authored-by: ljburtz <ljburtz@gmail.com>

packages/studio-base/src/players/UserNodePlayer/nodeTransformerWorker/typescript/rawUserUtils.ts

@@ -4,13 +4,15 @@
 
 import markers from "./userUtils/markers.ts?raw";
 import pointClouds from "./userUtils/pointClouds.ts?raw";
+import quaternions from "./userUtils/quaternions.ts?raw";
 import readers from "./userUtils/readers.ts?raw";
 import time from "./userUtils/time.ts?raw";
 import types from "./userUtils/types.ts?raw";
 import vectors from "./userUtils/vectors.ts?raw";
 
 export default [
   { fileName: "pointClouds.ts", sourceCode: pointClouds },
+  { fileName: "quaternions.ts", sourceCode: quaternions },
   { fileName: "readers.ts", sourceCode: readers },
   { fileName: "time.ts", sourceCode: time },
   { fileName: "types.ts", sourceCode: types },

packages/studio-base/src/players/UserNodePlayer/nodeTransformerWorker/typescript/userUtils/quaternions.test.ts

@@ -0,0 +1,20 @@
+import { quaternionToEuler } from "./quaternions";
+
+describe("quaternions", () => {
+  it("converts quaternions to euler angles", () => {
+    const quaternion = {
+      x: 0.03813457647485015,
+      y: 0.189307857412,
+      z: 0.2392983377447303,
+      w: 0.9515485246437886,
+    };
+
+    const euler = quaternionToEuler(quaternion);
+    expect(euler.x).toBeCloseTo(10);
+    expect(euler.y).toBeCloseTo(20);
+    expect(euler.z).toBeCloseTo(30);
+    expect(euler.roll).toBeCloseTo(10);
+    expect(euler.pitch).toBeCloseTo(20);
+    expect(euler.yaw).toBeCloseTo(30);
+  });
+});

packages/studio-base/src/players/UserNodePlayer/nodeTransformerWorker/typescript/userUtils/quaternions.ts

@@ -0,0 +1,47 @@
+// This Source Code Form is subject to the terms of the Mozilla Public
+// License, v2.0. If a copy of the MPL was not distributed with this
+// file, You can obtain one at http://mozilla.org/MPL/2.0/
+
+export type Quaternion = {
+  x: number;
+  y: number;
+  z: number;
+  w: number;
+};
+
+export type Euler = {
+  x: number;
+  y: number;
+  z: number;
+  roll: number;
+  pitch: number;
+  yaw: number;
+};
+
+/**
+ * Converts a quaternion to a Euler roll, pitch, yaw representation, in degrees.
+ *
+ * @param quaternion Input quaternion.
+ * @returns Converted Euler angle roll, pitch, yaw representation, in degrees.
+ */
+export function quaternionToEuler(quaternion: Quaternion): Euler {
+  const { x, y, z, w } = quaternion;
+
+  const toDegrees = 180 / Math.PI;
+  const dcm00 = w * w + x * x - y * y - z * z;
+  const dcm10 = 2 * (x * y + w * z);
+  const dcm20 = 2 * (x * z - w * y);
+  const dcm21 = 2 * (w * x + y * z);
+  const dcm22 = w * w - x * x - y * y + z * z;
+  const roll = toDegrees * Math.atan2(dcm21, dcm22);
+  const pitch = toDegrees * Math.asin(-dcm20);
+  const yaw = toDegrees * Math.atan2(dcm10, dcm00);
+  return {
+    x: roll,
+    y: pitch,
+    z: yaw,
+    roll,
+    pitch,
+    yaw,
+  };
+}