@vyr/engine 0.0.1

package/src/math/Quaternion.ts

@@ -0,0 +1,737 @@
+import { Euler } from './Euler';
+import { Matrix4 } from './Matrix4';
+import { clamp } from './utils';
+import { Vector3 } from './Vector3';
+
+
+/**
+ * Class for representing a Quaternion. Quaternions are used in three.js to represent rotations.
+ *
+ * Iterating through a vector instance will yield its components `(x, y, z, w)` in
+ * the corresponding order.
+ *
+ * Note that three.js expects Quaternions to be normalized.
+ * ```js
+ * const quaternion = new Quaternion();
+ * quaternion.setFromAxisAngle( new Vector3( 0, 1, 0 ), Math.PI / 2 );
+ *
+ * const vector = new Vector3( 1, 0, 0 );
+ * vector.applyQuaternion( quaternion );
+ * ```
+ */
+class Quaternion {
+    x: number;
+    y: number;
+    z: number;
+    w: number;
+    /**
+     * Constructs a new quaternion.
+     *
+     * @param x - The x value of this quaternion.
+     * @param y - The y value of this quaternion.
+     * @param z - The z value of this quaternion.
+     * @param w - The w value of this quaternion.
+     */
+    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;
+    }
+
+    /**
+     * Interpolates between two quaternions via SLERP. This implementation assumes the
+     * quaternion data are managed in flat arrays.
+     *
+     * @param dst - The destination array.
+     * @param dstOffset - An offset into the destination array.
+     * @param src0 - The source array of the first quaternion.
+     * @param srcOffset0 - An offset into the first source array.
+     * @param src1 - The source array of the second quaternion.
+     * @param srcOffset1 - An offset into the second source array.
+     * @param t - The interpolation factor in the range `[0,1]`.
+     * @see {@link Quaternion#slerp}
+     */
+    static slerpFlat(
+        dst: number[],
+        dstOffset: number,
+        src0: number[],
+        srcOffset0: number,
+        src1: number[],
+        srcOffset1: number,
+        t: number
+    ): void {
+        // fuzz-free, array-based Quaternion SLERP operation
+        let x0 = src0[srcOffset0 + 0],
+            y0 = src0[srcOffset0 + 1],
+            z0 = src0[srcOffset0 + 2],
+            w0 = src0[srcOffset0 + 3];
+
+        const x1 = src1[srcOffset1 + 0],
+            y1 = src1[srcOffset1 + 1],
+            z1 = src1[srcOffset1 + 2],
+            w1 = src1[srcOffset1 + 3];
+
+        if (t === 0) {
+            dst[dstOffset + 0] = x0;
+            dst[dstOffset + 1] = y0;
+            dst[dstOffset + 2] = z0;
+            dst[dstOffset + 3] = w0;
+            return;
+        }
+
+        if (t === 1) {
+            dst[dstOffset + 0] = x1;
+            dst[dstOffset + 1] = y1;
+            dst[dstOffset + 2] = z1;
+            dst[dstOffset + 3] = w1;
+            return;
+        }
+
+        if (w0 !== w1 || x0 !== x1 || y0 !== y1 || z0 !== z1) {
+            let s = 1 - t;
+            const cos = x0 * x1 + y0 * y1 + z0 * z1 + w0 * w1,
+                dir = cos >= 0 ? 1 : -1,
+                sqrSin = 1 - cos * cos;
+
+            // Skip the Slerp for tiny steps to avoid numeric problems:
+            if (sqrSin > Number.EPSILON) {
+                const sin = Math.sqrt(sqrSin),
+                    len = Math.atan2(sin, cos * dir);
+
+                s = Math.sin(s * len) / sin;
+                t = Math.sin(t * len) / sin;
+            }
+
+            const tDir = t * dir;
+
+            x0 = x0 * s + x1 * tDir;
+            y0 = y0 * s + y1 * tDir;
+            z0 = z0 * s + z1 * tDir;
+            w0 = w0 * s + w1 * tDir;
+
+            // Normalize in case we just did a lerp:
+            if (s === 1 - t) {
+                const f = 1 / Math.sqrt(x0 * x0 + y0 * y0 + z0 * z0 + w0 * w0);
+
+                x0 *= f;
+                y0 *= f;
+                z0 *= f;
+                w0 *= f;
+            }
+        }
+
+        dst[dstOffset] = x0;
+        dst[dstOffset + 1] = y0;
+        dst[dstOffset + 2] = z0;
+        dst[dstOffset + 3] = w0;
+    }
+
+    /**
+     * Multiplies two quaternions. This implementation assumes the quaternion data are managed
+     * in flat arrays.
+     *
+     * @param dst - The destination array.
+     * @param dstOffset - An offset into the destination array.
+     * @param src0 - The source array of the first quaternion.
+     * @param srcOffset0 - An offset into the first source array.
+     * @param src1 - The source array of the second quaternion.
+     * @param srcOffset1 - An offset into the second source array.
+     * @return The destination array.
+     * @see {@link Quaternion#multiplyQuaternions}.
+     */
+    static multiplyQuaternionsFlat(
+        dst: number[],
+        dstOffset: number,
+        src0: number[],
+        srcOffset0: number,
+        src1: number[],
+        srcOffset1: number
+    ): number[] {
+        const x0 = src0[srcOffset0];
+        const y0 = src0[srcOffset0 + 1];
+        const z0 = src0[srcOffset0 + 2];
+        const w0 = src0[srcOffset0 + 3];
+
+        const x1 = src1[srcOffset1];
+        const y1 = src1[srcOffset1 + 1];
+        const z1 = src1[srcOffset1 + 2];
+        const w1 = src1[srcOffset1 + 3];
+
+        dst[dstOffset] = x0 * w1 + w0 * x1 + y0 * z1 - z0 * y1;
+        dst[dstOffset + 1] = y0 * w1 + w0 * y1 + z0 * x1 - x0 * z1;
+        dst[dstOffset + 2] = z0 * w1 + w0 * z1 + x0 * y1 - y0 * x1;
+        dst[dstOffset + 3] = w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1;
+
+        return dst;
+    }
+
+    /**
+     * Sets the quaternion components.
+     *
+     * @param x - The x value of this quaternion.
+     * @param y - The y value of this quaternion.
+     * @param z - The z value of this quaternion.
+     * @param w - The w value of this quaternion.
+     * @return A reference to this quaternion.
+     */
+    set(x: number, y: number, z: number, w: number): Quaternion {
+        this.x = x;
+        this.y = y;
+        this.z = z;
+        this.w = w;
+
+
+        return this;
+    }
+
+    /**
+     * Returns a new quaternion with copied values from this instance.
+     *
+     * @return A clone of this instance.
+     */
+    clone(): Quaternion {
+        return new (this.constructor as typeof Quaternion)(this.x, this.y, this.z, this.w);
+    }
+
+    /**
+     * Copies the values of the given quaternion to this instance.
+     *
+     * @param quaternion - The quaternion to copy.
+     * @return A reference to this quaternion.
+     */
+    copy(quaternion: Quaternion): Quaternion {
+        this.x = quaternion.x;
+        this.y = quaternion.y;
+        this.z = quaternion.z;
+        this.w = quaternion.w;
+
+
+        return this;
+    }
+
+    /**
+     * Sets this quaternion from the rotation specified by the given
+     * Euler angles.
+     *
+     * @param euler - The Euler angles.
+     * @return A reference to this quaternion.
+     */
+    setFromEuler(euler: Euler): Quaternion {
+        const x = euler.x,
+            y = euler.y,
+            z = euler.z,
+            order = euler.order;
+
+        // http://www.mathworks.com/matlabcentral/fileexchange/
+        // 	20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors/
+        //	content/SpinCalc.m
+
+        const cos = Math.cos;
+        const sin = Math.sin;
+
+        const c1 = cos(x / 2);
+        const c2 = cos(y / 2);
+        const c3 = cos(z / 2);
+
+        const s1 = sin(x / 2);
+        const s2 = sin(y / 2);
+        const s3 = sin(z / 2);
+
+        switch (order) {
+            case 'XYZ':
+                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;
+                break;
+
+            case 'YXZ':
+                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;
+                break;
+
+            case 'ZXY':
+                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;
+                break;
+
+            case 'ZYX':
+                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;
+                break;
+
+            case 'YZX':
+                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;
+                break;
+
+            case 'XZY':
+                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;
+                break;
+
+            default:
+                console.warn('Quaternion: .setFromEuler() encountered an unknown order: ' + order);
+        }
+
+        return this;
+    }
+
+    /**
+     * Sets this quaternion from the given axis and angle.
+     *
+     * @param axis - The normalized axis.
+     * @param angle - The angle in radians.
+     * @return A reference to this quaternion.
+     */
+    setFromAxisAngle(axis: Vector3, angle: number): Quaternion {
+        // http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/index.htm
+        const halfAngle = angle / 2,
+            s = Math.sin(halfAngle);
+
+        this.x = axis.x * s;
+        this.y = axis.y * s;
+        this.z = axis.z * s;
+        this.w = Math.cos(halfAngle);
+
+
+        return this;
+    }
+
+    /**
+     * Sets this quaternion from the given rotation matrix.
+     *
+     * @param m - A 4x4 matrix of which the upper 3x3 of matrix is a pure rotation matrix (i.e. unscaled).
+     * @return A reference to this quaternion.
+     */
+    setFromRotationMatrix(m: Matrix4): Quaternion {
+        // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
+        // assumes the upper 3x3 of m is a pure rotation matrix (i.e, unscaled)
+        const te = m.elements,
+            m11 = te[0],
+            m12 = te[4],
+            m13 = te[8],
+            m21 = te[1],
+            m22 = te[5],
+            m23 = te[9],
+            m31 = te[2],
+            m32 = te[6],
+            m33 = te[10],
+            trace = m11 + m22 + m33;
+
+        if (trace > 0) {
+            const s = 0.5 / Math.sqrt(trace + 1.0);
+
+            this.w = 0.25 / s;
+            this.x = (m32 - m23) * s;
+            this.y = (m13 - m31) * s;
+            this.z = (m21 - m12) * s;
+        } else if (m11 > m22 && m11 > m33) {
+            const s = 2.0 * Math.sqrt(1.0 + m11 - m22 - m33);
+
+            this.w = (m32 - m23) / s;
+            this.x = 0.25 * s;
+            this.y = (m12 + m21) / s;
+            this.z = (m13 + m31) / s;
+        } else if (m22 > m33) {
+            const s = 2.0 * Math.sqrt(1.0 + m22 - m11 - m33);
+
+            this.w = (m13 - m31) / s;
+            this.x = (m12 + m21) / s;
+            this.y = 0.25 * s;
+            this.z = (m23 + m32) / s;
+        } else {
+            const s = 2.0 * Math.sqrt(1.0 + m33 - m11 - m22);
+
+            this.w = (m21 - m12) / s;
+            this.x = (m13 + m31) / s;
+            this.y = (m23 + m32) / s;
+            this.z = 0.25 * s;
+        }
+
+
+        return this;
+    }
+
+    /**
+     * Sets this quaternion to the rotation required to rotate the direction vector
+     * `vFrom` to the direction vector `vTo`.
+     *
+     * @param vFrom - The first (normalized) direction vector.
+     * @param vTo - The second (normalized) direction vector.
+     * @return A reference to this quaternion.
+     */
+    setFromUnitVectors(vFrom: Vector3, vTo: Vector3): Quaternion {
+        // assumes direction vectors vFrom and vTo are normalized
+        let r = vFrom.dot(vTo) + 1;
+
+        if (r < Number.EPSILON) {
+            // vFrom and vTo point in opposite directions
+            r = 0;
+
+            if (Math.abs(vFrom.x) > Math.abs(vFrom.z)) {
+                this.x = -vFrom.y;
+                this.y = vFrom.x;
+                this.z = 0;
+                this.w = r;
+            } else {
+                this.x = 0;
+                this.y = -vFrom.z;
+                this.z = vFrom.y;
+                this.w = r;
+            }
+        } else {
+            // crossVectors( vFrom, vTo ); // inlined to avoid cyclic dependency on Vector3
+            this.x = vFrom.y * vTo.z - vFrom.z * vTo.y;
+            this.y = vFrom.z * vTo.x - vFrom.x * vTo.z;
+            this.z = vFrom.x * vTo.y - vFrom.y * vTo.x;
+            this.w = r;
+        }
+
+        return this.normalize();
+    }
+
+    /**
+     * Returns the angle between this quaternion and the given one in radians.
+     *
+     * @param q - The quaternion to compute the angle with.
+     * @return The angle in radians.
+     */
+    angleTo(q: Quaternion): number {
+        return 2 * Math.acos(Math.abs(clamp(this.dot(q), -1, 1)));
+    }
+
+    /**
+     * Rotates this quaternion by a given angular step to the given quaternion.
+     * The method ensures that the final quaternion will not overshoot `q`.
+     *
+     * @param q - The target quaternion.
+     * @param step - The angular step in radians.
+     * @return A reference to this quaternion.
+     */
+    rotateTowards(q: Quaternion, step: number): Quaternion {
+        const angle = this.angleTo(q);
+
+        if (angle === 0) return this;
+
+        const t = Math.min(1, step / angle);
+
+        this.slerp(q, t);
+
+        return this;
+    }
+
+    /**
+     * Sets this quaternion to the identity quaternion; that is, to the
+     * quaternion that represents "no rotation".
+     *
+     * @return A reference to this quaternion.
+     */
+    identity(): Quaternion {
+        return this.set(0, 0, 0, 1);
+    }
+
+    /**
+     * Inverts this quaternion via {@link Quaternion#conjugate}. The
+     * quaternion is assumed to have unit length.
+     *
+     * @return A reference to this quaternion.
+     */
+    invert(): Quaternion {
+        return this.conjugate();
+    }
+
+    /**
+     * Returns the rotational conjugate of this quaternion. The conjugate of a
+     * quaternion represents the same rotation in the opposite direction about
+     * the rotational axis.
+     *
+     * @return A reference to this quaternion.
+     */
+    conjugate(): Quaternion {
+        this.x *= -1;
+        this.y *= -1;
+        this.z *= -1;
+
+
+        return this;
+    }
+
+    /**
+     * Calculates the dot product of this quaternion and the given one.
+     *
+     * @param v - The quaternion to compute the dot product with.
+     * @return The result of the dot product.
+     */
+    dot(v: Quaternion): number {
+        return this.x * v.x + this.y * v.y + this.z * v.z + this.w * v.w;
+    }
+
+    /**
+     * Computes the squared Euclidean length (straight-line length) of this quaternion,
+     * considered as a 4 dimensional vector. This can be useful if you are comparing the
+     * lengths of two quaternions, as this is a slightly more efficient calculation than
+     * {@link Quaternion#length}.
+     *
+     * @return The squared Euclidean length.
+     */
+    lengthSq(): number {
+        return this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w;
+    }
+
+    /**
+     * Computes the Euclidean length (straight-line length) of this quaternion,
+     * considered as a 4 dimensional vector.
+     *
+     * @return The Euclidean length.
+     */
+    length(): number {
+        return Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w);
+    }
+
+    /**
+     * Normalizes this quaternion - that is, calculated the quaternion that performs
+     * the same rotation as this one, but has a length equal to `1`.
+     *
+     * @return A reference to this quaternion.
+     */
+    normalize(): Quaternion {
+        let l = this.length();
+
+        if (l === 0) {
+            this.x = 0;
+            this.y = 0;
+            this.z = 0;
+            this.w = 1;
+        } else {
+            l = 1 / l;
+
+            this.x = this.x * l;
+            this.y = this.y * l;
+            this.z = this.z * l;
+            this.w = this.w * l;
+        }
+
+
+        return this;
+    }
+
+    /**
+     * Multiplies this quaternion by the given one.
+     *
+     * @param q - The quaternion.
+     * @return A reference to this quaternion.
+     */
+    multiply(q: Quaternion): Quaternion {
+        return this.multiplyQuaternions(this, q);
+    }
+
+    /**
+     * Pre-multiplies this quaternion by the given one.
+     *
+     * @param q - The quaternion.
+     * @return A reference to this quaternion.
+     */
+    premultiply(q: Quaternion): Quaternion {
+        return this.multiplyQuaternions(q, this);
+    }
+
+    /**
+     * Multiplies the given quaternions and stores the result in this instance.
+     *
+     * @param a - The first quaternion.
+     * @param b - The second quaternion.
+     * @return A reference to this quaternion.
+     */
+    multiplyQuaternions(a: Quaternion, b: Quaternion): Quaternion {
+        // from http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm
+        const qax = a.x,
+            qay = a.y,
+            qaz = a.z,
+            qaw = a.w;
+        const qbx = b.x,
+            qby = b.y,
+            qbz = b.z,
+            qbw = b.w;
+
+        this.x = qax * qbw + qaw * qbx + qay * qbz - qaz * qby;
+        this.y = qay * qbw + qaw * qby + qaz * qbx - qax * qbz;
+        this.z = qaz * qbw + qaw * qbz + qax * qby - qay * qbx;
+        this.w = qaw * qbw - qax * qbx - qay * qby - qaz * qbz;
+
+
+        return this;
+    }
+
+    /**
+     * Performs a spherical linear interpolation between quaternions.
+     *
+     * @param qb - The target quaternion.
+     * @param t - The interpolation factor in the closed interval `[0, 1]`.
+     * @return A reference to this quaternion.
+     */
+    slerp(qb: Quaternion, t: number): Quaternion {
+        if (t === 0) return this;
+        if (t === 1) return this.copy(qb);
+
+        const x = this.x,
+            y = this.y,
+            z = this.z,
+            w = this.w;
+
+        // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/
+        let cosHalfTheta = w * qb.w + x * qb.x + y * qb.y + z * qb.z;
+
+        if (cosHalfTheta < 0) {
+            this.w = -qb.w;
+            this.x = -qb.x;
+            this.y = -qb.y;
+            this.z = -qb.z;
+
+            cosHalfTheta = -cosHalfTheta;
+        } else {
+            this.copy(qb);
+        }
+
+        if (cosHalfTheta >= 1.0) {
+            this.w = w;
+            this.x = x;
+            this.y = y;
+            this.z = z;
+
+            return this;
+        }
+
+        const sqrSinHalfTheta = 1.0 - cosHalfTheta * cosHalfTheta;
+
+        if (sqrSinHalfTheta <= Number.EPSILON) {
+            const s = 1 - t;
+            this.w = s * w + t * this.w;
+            this.x = s * x + t * this.x;
+            this.y = s * y + t * this.y;
+            this.z = s * z + t * this.z;
+
+            this.normalize(); // normalize calls _onChangeCallback()
+            return this;
+        }
+
+        const sinHalfTheta = Math.sqrt(sqrSinHalfTheta);
+        const halfTheta = Math.atan2(sinHalfTheta, cosHalfTheta);
+        const ratioA = Math.sin((1 - t) * halfTheta) / sinHalfTheta,
+            ratioB = Math.sin(t * halfTheta) / sinHalfTheta;
+
+        this.w = w * ratioA + this.w * ratioB;
+        this.x = x * ratioA + this.x * ratioB;
+        this.y = y * ratioA + this.y * ratioB;
+        this.z = z * ratioA + this.z * ratioB;
+
+
+        return this;
+    }
+
+    /**
+     * Performs a spherical linear interpolation between the given quaternions
+     * and stores the result in this quaternion.
+     *
+     * @param qa - The source quaternion.
+     * @param qb - The target quaternion.
+     * @param t - The interpolation factor in the closed interval `[0, 1]`.
+     * @return A reference to this quaternion.
+     */
+    slerpQuaternions(qa: Quaternion, qb: Quaternion, t: number): Quaternion {
+        return this.copy(qa).slerp(qb, t);
+    }
+
+    /**
+     * Sets this quaternion to a uniformly random, normalized quaternion.
+     *
+     * @return A reference to this quaternion.
+     */
+    random(): Quaternion {
+        // Ken Shoemake
+        // Uniform random rotations
+        // D. Kirk, editor, Graphics Gems III, pages 124-132. Academic Press, New York, 1992.
+        const theta1 = 2 * Math.PI * Math.random();
+        const theta2 = 2 * Math.PI * Math.random();
+
+        const x0 = Math.random();
+        const r1 = Math.sqrt(1 - x0);
+        const r2 = Math.sqrt(x0);
+
+        return this.set(r1 * Math.sin(theta1), r1 * Math.cos(theta1), r2 * Math.sin(theta2), r2 * Math.cos(theta2));
+    }
+
+    /**
+     * Returns `true` if this quaternion is equal with the given one.
+     *
+     * @param quaternion - The quaternion to test for equality.
+     * @return Whether this quaternion is equal with the given one.
+     */
+    equals(quaternion: Quaternion): boolean {
+        return quaternion.x === this.x && quaternion.y === this.y && quaternion.z === this.z && quaternion.w === this.w;
+    }
+
+    /**
+     * Sets this quaternion's components from the given array.
+     *
+     * @param array - An array holding the quaternion component values.
+     * @param offset - The offset into the array.
+     * @return A reference to this quaternion.
+     */
+    fromArray(array: number[], offset: number = 0): Quaternion {
+        this.x = array[offset];
+        this.y = array[offset + 1];
+        this.z = array[offset + 2];
+        this.w = array[offset + 3];
+
+
+        return this;
+    }
+
+    /**
+     * Writes the components of this quaternion to the given array. If no array is provided,
+     * the method returns a new instance.
+     *
+     * @param array - The target array holding the quaternion components.
+     * @param offset - Index of the first element in the array.
+     * @return The quaternion components.
+     */
+    toArray(array: number[] = [], offset: number = 0): number[] {
+        array[offset] = this.x;
+        array[offset + 1] = this.y;
+        array[offset + 2] = this.z;
+        array[offset + 3] = this.w;
+
+        return array;
+    }
+
+    /**
+     * This methods defines the serialization result of this class. Returns the
+     * numerical elements of this quaternion in an array of format `[x, y, z, w]`.
+     *
+     * @return The serialized quaternion.
+     */
+    toJSON(): number[] {
+        return this.toArray();
+    }
+
+    *[Symbol.iterator](): Generator<number> {
+        yield this.x;
+        yield this.y;
+        yield this.z;
+        yield this.w;
+    }
+}
+
+export { Quaternion };