Quaternion.ts

import * as THREE from 'three';
export class Quaternion {
    private _x: number;
    private _y: number;
    private _z: number;
    private _w: number;

    constructor(x: number = 0, y: number = 0, z: number = 0, w: number = 1) {
        this._x = x;
        this._y = y;
        this._z = z;
        this._w = w;
    }

    public get x(): number { return this._x; }
    public get y(): number { return this._y; }
    public get z(): number { return this._z; }
    public get w(): number { return this._w; }

    public set x(value: number) { 
        this._x = value;
    }

    public set y(value: number) {
        this._y = value;
    }

    public set z(value: number) { 
        this._z = value;
    }

    public set w(value: number) {
        this._w = value;
    }

    public get eulerMatrix(): THREE.Matrix4 {
        const copy = Quaternion.normalize(this.clone());
        const m = new THREE.Matrix4();
        m.set(
            1 - 2 * (copy._y * copy._y + copy._z * copy._z), 2 * (copy._x * copy._y - copy._z * copy._w), 2 * (copy._x * copy._z + copy._y * copy._w), 0,
            2 * (copy._x * copy._y + copy._z * copy._w), 1 - 2 * (copy._x * copy._x + copy._z * copy._z), 2 * (copy._y * copy._z - copy._x * copy._w), 0,
            2 * (copy._x * copy._z - copy._y * copy._w), 2 * (copy._y * copy._z + copy._x * copy._w), 1 - 2 * (copy._x * copy._x + copy._y * copy._y), 0,
            0, 0, 0, 1
        );
        return m;
    }

    public get array(): number[] {
        return [this._x, this._y, this._z, this._w];
    }

    public fromEuler(euler: THREE.Vector3): this {
        const c1 = Math.cos(euler.x / 2);
        const s1 = Math.sin(euler.x / 2);
        const c2 = Math.cos(euler.y / 2);
        const s2 = Math.sin(euler.y / 2);
        const c3 = Math.cos(euler.z / 2);
        const s3 = Math.sin(euler.z / 2);

        this._x = s1 * c2 * c3 + c1 * s2 * s3;
        this._y = c1 * s2 * c3 - s1 * c2 * s3;
        this._z = c1 * c2 * s3 + s1 * s2 * c3;
        this._w = c1 * c2 * c3 - s1 * s2 * s3;

        this.copy(Quaternion.normalize(this));

        return this;
    }

    public toString(): string {
        return `Quaternion(${this._x}, ${this._y}, ${this._z}, ${this._w})`;
    }

    public set(x: number, y: number, z: number, w: number): this {
        this._x = x;
        this._y = y;
        this._z = z;
        this._w = w;
        return this;
    }

    public copy(q: Quaternion): this {
        this._x = q._x;
        this._y = q._y;
        this._z = q._z;
        this._w = q._w;
        return this;
    }

    public clone(): Quaternion {
        return new Quaternion(this._x, this._y, this._z, this._w);
    }


    public static identity(): Quaternion {
        return new Quaternion(0, 0, 0, 1);
    }

    public static slerp(qa: Quaternion, qb: Quaternion, t: number): Quaternion {
        const cosTheta = Quaternion.dot(qa, qb);
        if (cosTheta > .9995) {
            return Quaternion.add(Quaternion.multiplyScalar(qa, 1 - t), Quaternion.multiplyScalar(qb, t));
        } else {
            const theta = Math.acos(cosTheta);
            const thetap = theta * t;
            const qperp = Quaternion.normalize(Quaternion.subtract(qb, Quaternion.multiplyScalar(qa, cosTheta)));
            return Quaternion.add(Quaternion.multiplyScalar(qa, Math.cos(thetap)), Quaternion.multiplyScalar(qperp, Math.sin(thetap)));
        }
    }

    // public static slerp(qa: Quaternion, qb: Quaternion, t: number): Quaternion {

    //     let cq1 = qa.clone();
    //     let cq2 = qb.clone();

    //     cq1 = Quaternion.normalize(cq1);
    //     cq2 = Quaternion.normalize(cq2);

    //     let dot = Quaternion.dot(cq1, cq2);

    //     if (dot < 0.0) {
    //         cq2 = Quaternion.inverse(cq2);
    //         dot = -dot;
    //     }

    //     if (dot > 0.9995) {
    //         return Quaternion.add(cq1, Quaternion.multiplyScalar(
    //             Quaternion.subtract(cq2, cq1), t));
    //     }

    //     const theta0 = Math.acos(dot);
    //     const theta = theta0 * t;

    //     const sinTheta0 = Math.sin(theta0);
    //     const sinTheta = Math.sin(theta);
    //     const s0 = Math.cos(theta0) - dot * sinTheta / sinTheta0;
    //     const s1 = sinTheta / sinTheta0;

    //     return Quaternion.add(
    //         Quaternion.multiplyScalar(cq1, s0),
    //         Quaternion.multiplyScalar(cq2, s1)
    //     )
    // }

    public static add(qa: Quaternion, qb: Quaternion): Quaternion {
        return new Quaternion(qa._x + qb._x, qa._y + qb._y, qa._z + qb._z, qa._w + qb._w);
    }

    public static subtract(qa: Quaternion, qb: Quaternion): Quaternion {
        return new Quaternion(qa._x - qb._x, qa._y - qb._y, qa._z - qb._z, qa._w - qb._w);
    }

    public static multiply(qa: Quaternion, qb: Quaternion): Quaternion {
        return new Quaternion(
            qa._w * qb._x + qa._x * qb._w + qa._y * qb._z - qa._z * qb._y,
            qa._w * qb._y + qa._y * qb._w + qa._z * qb._x - qa._x * qb._z,
            qa._w * qb._z + qa._z * qb._w + qa._x * qb._y - qa._y * qb._x,
            qa._w * qb._w - qa._x * qb._x - qa._y * qb._y - qa._z * qb._z
        );
    }

    public static multiplyScalar(a: Quaternion | number, b: Quaternion | number): Quaternion {
        if (typeof b === 'number' && a instanceof Quaternion) {
            return new Quaternion(a._x * b, a._y * b, a._z * b, a._w * b);
        } if (typeof a === 'number' && b instanceof Quaternion) {
            return new Quaternion(b._x * a, b._y * a, b._z * a, b._w * a);
        }
        throw new Error("Invalid arguments");
    }

    public static dot(qa: Quaternion, qb: Quaternion): number {
        return qa._x * qb._x + qa._y * qb._y + qa._z * qb._z + qa._w * qb._w;
    }

    public static len(qa: Quaternion): number {
        return Math.sqrt(Quaternion.dot(qa, qa));
    }

    public static normalize(qa: Quaternion): Quaternion {
        const length = Quaternion.len(qa);
        if (length < 0.00001) {
            return Quaternion.identity();
        }
        return Quaternion.multiplyScalar(qa, 1 / length);
    }

    public static inverse(qa: Quaternion): Quaternion {
        return new Quaternion(-qa._x, -qa._y, -qa._z, qa._w);
    }
}