diff --git a/src/quaternion.ts b/src/quaternion.ts new file mode 100644 index 0000000..718cef6 --- /dev/null +++ b/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; +}