feat: quaternion relative

package.json

@@ -1,6 +1,6 @@
 {
   "name": "computercraft-mutil",
-  "version": "1.0.9",
+  "version": "1.0.10",
   "description": "",
   "license": "ISC",
   "author": "",

src/pid.ts

@@ -2,7 +2,7 @@ export interface PIDParameters {
   kP: number;
   kI: number;
   kD: number;
-  errorWrap?: (error: number) => number;
+  calculateError?: (setpoint: number, measuredValue: number) => number;
 }
 
 export type PIDReport = {
@@ -46,10 +46,9 @@ export class PID {
   public update(measuredValue: number): number {
     const currentTime = os.epoch("utc");
 
-    const rawError = this.setpoint - measuredValue;
-    const error = this.params.errorWrap
-      ? this.params.errorWrap(rawError)
-      : rawError;
+    const error = this.params.calculateError
+      ? this.params.calculateError(this.setpoint, measuredValue)
+      : this.setpoint - measuredValue;
 
     let deltaTime = 0;
     if (this.lastTime !== null) {

src/quaternion.ts

@@ -3,14 +3,14 @@ import { Vec3 } from "./vec";
 const math = globalThis.math;
 
 export class Quaternion {
-  public constructor(
+  constructor(
     public x: number,
     public y: number,
     public z: number,
     public w: number,
   ) { }
 
-  public static fromXYZ({
+  static fromXYZ({
     x,
     y,
     z,
@@ -45,6 +45,62 @@ export class Quaternion {
 
     return new Vec3(roll, pitch, yaw);
   }
+
+  public normalized(): Quaternion {
+    const mag = math.sqrt(
+      this.w * this.w + this.x * this.x + this.y * this.y + this.z * this.z,
+    );
+    return new Quaternion(
+      this.x / mag,
+      this.y / mag,
+      this.z / mag,
+      this.w / mag,
+    );
+  }
+
+  public inverse(): Quaternion {
+    // assuming unit quaternion
+    return new Quaternion(-this.x, -this.y, -this.z, this.w);
+  }
+
+  public multiply(q: Quaternion): Quaternion {
+    const w1 = this.w,
+      x1 = this.x,
+      y1 = this.y,
+      z1 = this.z;
+    const w2 = q.w,
+      x2 = q.x,
+      y2 = q.y,
+      z2 = q.z;
+
+    return new Quaternion(
+      x1 * w2 + w1 * x2 + y1 * z2 - z1 * y2,
+      y1 * w2 + w1 * y2 + z1 * x2 - x1 * z2,
+      z1 * w2 + w1 * z2 + x1 * y2 - y1 * x2,
+      w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2,
+    );
+  }
+
+  /** Returns rotation error to target as axis and angle (in radians) */
+  public getErrorTo(target: Quaternion): { axis: Vec3; angle: number } {
+    // q_error = q_target * inverse(q_current)
+    const q_error = target.multiply(this.inverse()).normalized();
+
+    const angle = 2 * math.acos(math.min(math.max(q_error.w, -1), 1));
+    const s = math.sqrt(1 - q_error.w * q_error.w);
+
+    let axis: Vec3;
+    if (s < 1e-6) {
+      axis = new Vec3(1, 0, 0); // arbitrary
+    } else {
+      axis = new Vec3(q_error.x / s, q_error.y / s, q_error.z / s);
+    }
+
+    // Wrap angle to [-PI, PI]
+    const wrappedAngle = angle > Math.PI ? angle - 2 * Math.PI : angle;
+
+    return { axis, angle: wrappedAngle };
+  }
 }
 
 // fast sign without zero