Introduce Quaternions

docs/api/quaternion.md

@@ -0,0 +1,67 @@
+# Quaternion
+
+A number system used in encantar.js to represent rotations in 3D space.
+
+*Since:* 0.4.0
+
+## Properties
+
+### x
+
+`quaternion.x: number`
+
+The x coordinate of the quaternion (imaginary).
+
+### y
+
+`quaternion.y: number`
+
+The y coordinate of the quaternion (imaginary).
+
+### z
+
+`quaternion.z: number`
+
+The z coordinate of the quaternion (imaginary).
+
+### w
+
+`quaternion.w: number`
+
+The w coordinate of the quaternion (real).
+
+## Methods
+
+### length
+
+`quaternion.length(): number`
+
+Compute the length of the quaternion: `sqrt(x^2 + y^2 + z^2 + w^2)`.
+
+**Returns**
+
+The length of the quaternion.
+
+### equals
+
+`quaternion.equals(q: Quaternion): boolean`
+
+Check if `this` and `q` have the same coordinates.
+
+**Arguments**
+
+* `q: Quaternion`. A quaternion.
+
+**Returns**
+
+`true` if `this` and `q` have the same coordinates.
+
+### toString
+
+`quaternion.toString(): string`
+
+Generate a string representation of the quaternion.
+
+**Returns**
+
+A string representation of the quaternion.

mkdocs.yml

@@ -95,6 +95,7 @@ nav:
       - 'View': 'api/view.md'
       - 'PerspectiveView': 'api/perspective-view.md'
       - 'RigidTransform': 'api/rigid-transform.md'
+      - 'Quaternion': 'api/quaternion.md'
     - 'Visualization':
       - 'Viewport': 'api/viewport.md'
       - 'HUD': 'api/hud.md'

src/geometry/quaternion.ts

@@ -0,0 +1,330 @@
+/*
+ * encantar.js
+ * GPU-accelerated Augmented Reality for the web
+ * Copyright (C) 2022-2024 Alexandre Martins <alemartf(at)gmail.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published
+ * by the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ * quaternion.ts
+ * Quaternions
+ */
+
+import Speedy from 'speedy-vision';
+import { SpeedyMatrix } from 'speedy-vision/types/core/speedy-matrix';
+import { IllegalArgumentError } from '../utils/errors';
+
+/** Small number */
+const EPSILON = 1e-6;
+
+// public / non-internal methods do not change the contents of the quaternion
+
+
+
+/**
+ * Quaternion q = x i + y j + z k + w
+ */
+export class Quaternion
+{
+    /** x coordinate (imaginary) */
+    public x: number;
+
+    /** y coordinate (imaginary) */
+    public y: number;
+
+    /** z coordinate (imaginary) */
+    public z: number;
+
+    /** w coordinate (real) */
+    public w: number;
+
+
+
+    /**
+     * Constructor
+     * @param x x coordinate (imaginary)
+     * @param y y coordinate (imaginary)
+     * @param z z coordinate (imaginary)
+     * @param w w coordinate (real)
+     */
+    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;
+    }
+
+    /**
+     * Instantiate an identity quaternion q = 1
+     * @returns a new identity quaternion
+     */
+    static Identity(): Quaternion
+    {
+        return new Quaternion(0, 0, 0, 1);
+    }
+
+    /**
+     * The length of this quaternion
+     * @returns sqrt(x^2 + y^2 + z^2 + w^2)
+     */
+    length(): number
+    {
+        const x = this.x;
+        const y = this.y;
+        const z = this.z;
+        const w = this.w;
+
+        return Math.sqrt(x*x + y*y + z*z + w*w);
+    }
+
+    /**
+     * Check if this and q have the same coordinates
+     * @param q a quaternion
+     * @returns true if this and q have the same coordinates
+     */
+    equals(q: Quaternion): boolean
+    {
+        return this.w === q.w && this.x === q.x && this.y === q.y && this.z === q.z;
+    }
+
+    /**
+     * Convert to string
+     * @returns a string
+     */
+    toString(): string
+    {
+        const x = this.x.toFixed(4);
+        const y = this.y.toFixed(4);
+        const z = this.z.toFixed(4);
+        const w = this.w.toFixed(4);
+
+        return `Quaternion(${x},${y},${z},${w})`;
+    }
+
+    /**
+     * Normalize this quaternion
+     * @returns this quaternion, normalized
+     * @internal
+     */
+    _normalize(): Quaternion
+    {
+        const length = this.length();
+
+        if(length < EPSILON) // zero?
+            return this;
+
+        this.x /= length;
+        this.y /= length;
+        this.z /= length;
+        this.w /= length;
+
+        return this;
+    }
+
+    /**
+     * Copy q to this
+     * @param q a quaternion
+     * @returns this quaternion
+     * @internal
+     */
+    _copyFrom(q: Quaternion): Quaternion
+    {
+        this.x = q.x;
+        this.y = q.y;
+        this.z = q.z;
+        this.w = q.w;
+
+        return this;
+    }
+
+    /**
+     * Convert a quaternion to a 3x3 rotation matrix
+     * @returns a 3x3 rotation matrix
+     * @internal
+     */
+    _toRotationMatrix(): SpeedyMatrix
+    {
+        const length = this.length(); // should be ~ 1
+
+        // sanity check
+        if(length < EPSILON)
+            return Speedy.Matrix.Eye(3);
+
+        // let q = (x,y,z,w) be a unit quaternion
+        const x = this.x / length;
+        const y = this.y / length;
+        const z = this.z / length;
+        const w = this.w / length;
+
+        /*
+
+        Let q = x i + y j + z k + w be a unit quaternion and
+        p = x_p i + y_p j + z_p k be a purely imaginary quaternion (w_p = 0)
+        representing a vector or point P = (x_p, y_p, z_p) in 3D space.
+
+        Let's rewrite q as q = v + w, where v = x i + y j + z k, and then
+        substitute v by the unit vector u = v / |v|, so that q = |v| u + w.
+        Since q is a unit quaternion, it follows that:
+
+        1 = |q|^2 = x^2 + y^2 + z^2 + w^2 = |v|^2 + w^2.
+
+        Given that cos(t~)^2 + sin(t~)^2 = 1 for all real t~, there is a real t
+        such that cos(t) = w and sin(t) = |v|. Let's rewrite q as:
+
+        q = cos(t) + u sin(t)
+
+        (since 0 <= |v| = sin(t), it follows that 0 <= t <= pi)
+
+        A rotation of P, of 2t radians around axis u, can be computed as:
+
+        r_q(p) = q p q*
+
+        where q* is the conjugate of q. (note: since |q| = 1, q q* = q* q = 1)
+
+        ---
+
+        Let h = x_h i + y_h j + z_h k + w_h be a quaternion. The multplication
+        q h can be performed by pre-multiplying h, written as a column vector,
+        by the following L_q matrix:
+
+                      [  w  -z   y   x  ] [ x_h ]
+        q h = L_q h = [  z   w  -x   y  ] [ y_h ]
+                      [ -y   x   w   z  ] [ z_h ]
+                      [ -x  -y  -z   w  ] [ w_h ]
+
+        (expand q h = (x i + y j + z k + w) (x_h i + y_h j + z_h k + w_h) to see)
+
+        Similarly, the product h q* can be expressed by pre-multiplying h by the
+        following R_q* matrix:
+
+                        [  w  -z   y  -x ] [ x_h ]
+        h q* = R_q* h = [  z   w  -x  -y ] [ y_h ]
+                        [ -y   x   w  -z ] [ z_h ]
+                        [  x   y   z   w ] [ w_h ]
+
+        (expand h q* = (x_h i + y_h j + z_h k + w_h) (-x i - y j - z k + w) to see)
+
+        ---
+
+        Although quaternion multiplication is not commutative, it is associative,
+        i.e., r_q(p) = (q p)q* = q(p q*). From the matrix equations above, it
+        follows that r_q(p) can be expressed as R_q* L_q p = L_q R_q* p. If we
+        define M_q = L_q R_q* = R_q* L_q, we can write r_q(p) = M_q p. Matrix M_q
+        has the following form:
+
+              [ w^2 + x^2 - y^2 - z^2  2xy - 2wz              2xz + 2wy              0 ]
+        M_q = [ 2xy + 2wz              w^2 - x^2 + y^2 - z^2  2yz - 2wx              0 ]
+              [ 2xz - 2wy              2yz + 2wx              w^2 - x^2 - y^2 + z^2  0 ]
+              [ 0                      0                      0                      1 ]
+
+        Note: the bottom-right entry is x^2 + y^2 + z^2 + w^2 = |q|^2 = 1.
+
+        Let M be the top-left 3x3 submatrix of M_q. A direct, but boring,
+        computation shows that M'M = M M' = I, where M' is the transpose of M.
+        In addition, det M = |q|^6 = +1. Therefore, M is a 3x3 rotation matrix.
+
+        */
+
+        const x2 = x*x, y2 = y*y, z2 = z*z;//, w2 = w*w;
+        const xy = 2*x*y, xz = 2*x*z, yz = 2*y*z;
+        const wx = 2*w*x, wy = 2*w*y, wz = 2*w*z;
+
+        return Speedy.Matrix(3, 3, [
+            1-2*(y2+z2), xy+wz, xz-wy,
+            xy-wz, 1-2*(x2+z2), yz+wx,
+            xz+wy, yz-wx, 1-2*(x2+y2)
+        ]);
+    }
+
+    /**
+     * Convert a 3x3 rotation matrix to a unit quaternion
+     * @param m a 3x3 rotation matrix. You should ensure that it is a rotation matrix
+     * @returns this quaternion
+     * @internal
+     */
+    _fromRotationMatrix(m: SpeedyMatrix): Quaternion
+    {
+        if(m.rows != 3 || m.columns != 3)
+            throw new IllegalArgumentError();
+
+        /*
+
+        Let M be the rotation matrix defined above. We're going to find a
+        unit quaternion q associated with M.
+
+        Before we begin, note that q and (-q) encode the same rotation, for
+        r_(-q)(p) = (-q)p(-q)* = (-1)q p (-1)q* = (-1)(-1)q p q* = = r_q(p).
+        Quaternion multiplication is commutative when a factor is a scalar, i.e.,
+        d p = p d for a real d and a quaternion p (check: distributive operation).
+
+        The trace of M, denoted as tr M, is 3w^2 - x^2 - y^2 - z^2. Since |q| = 1,
+        it follows that tr M = 3w^2 - (1 - w^2), which means that 4w^2 = 1 + tr M.
+        That being the case, we can write:
+
+        |w| = sqrt(1 + tr M) / 2
+
+        We'll arbitrarily pick w >= 0, for q and (-q) encode the same rotation.
+
+        Let mij denote the element at the i-th row and at the j-th column of M.
+        A direct verification shows that m21 - m12 = 4wz. Since w >= 0, it follows
+        that sign(z) = sign(m21 - m12). Similarly, sign(y) = sign(m13 - m31) and
+        sign(x) = sign(m32 - m23).
+
+        The quantity m11 + m22 is equal to 2w^2 - 2z^2, which means that 4z^2 =
+        4w^2 - 2(m11 + m22) = (1 + tr M) - 2(m11 + m22). Therefore, let's write:
+
+        |z| = sqrt((1 + tr M) - 2(m11 + m22)) / 2
+
+        Of course, z = |z| sign(z). Similarly,
+
+        |y| = sqrt((1 + tr M) - 2(m11 + m33)) / 2
+
+        |x| = sqrt((1 + tr M) - 2(m22 + m33)) / 2
+
+        This gives (x, y, z, w).
+
+        ---
+
+        We quickly verify that (1 + tr M) - 2(m11 + m22) >= 0 if M is (really)
+        a rotation matrix: (1 + tr M) - 2(m11 + m22) = 1 + tr M - 2(tr M - m33) =
+        1 - tr M + 2 m33 = 1 - (3w^2 - x^2 - y^2 - z^2) + 2(w^2 - x^2 - y^2 + z^2) =
+        1 - w^2 + z^2 = (x^2 + y^2 + z^2 + w^2) - w^2 + z^2 = x^2 + y^2 + 2 z^2 >= 0.
+        Similarly, (1 + tr M) - 2(m11 + m33) >= 0 and (1 + tr M) - 2(m22 + m33) >= 0.
+
+        */
+
+        const data = m.read();
+        const m11 = data[0], m21 = data[1], m31 = data[2],
+              m12 = data[3], m22 = data[4], m32 = data[5],
+              m13 = data[6], m23 = data[7], m33 = data[8];
+
+        const tr = 1 + m11 + m22 + m33; // 1 + tr M
+        const sx = +(m32 >= m23) - +(m32 < m23); // sign(x)
+        const sy = +(m13 >= m31) - +(m13 < m31); // sign(y)
+        const sz = +(m21 >= m12) - +(m21 < m12); // sign(z)
+
+        const w = 0.5 * Math.sqrt(Math.max(0, tr)); // |w| = w
+        const x = 0.5 * Math.sqrt(Math.max(0, tr - 2 * (m22 + m33))); // |x|
+        const y = 0.5 * Math.sqrt(Math.max(0, tr - 2 * (m11 + m33))); // |y|
+        const z = 0.5 * Math.sqrt(Math.max(0, tr - 2 * (m11 + m22))); // |z|
+
+        const length = Math.sqrt(x*x + y*y + z*z + w*w); // should be ~ 1
+        this.x = (x * sx) / length;
+        this.y = (y * sy) / length;
+        this.z = (z * sz) / length;
+        this.w = w / length;
+
+        return this;
+    }
+}

src/main.ts

@@ -28,6 +28,7 @@ import { TrackerFactory } from './trackers/tracker-factory';
 import { SourceFactory } from './sources/source-factory';
 import { Viewport, ViewportSettings } from './core/viewport';
 import { Utils } from './utils/utils';
+
 declare const __AR_VERSION__: string;
 declare const __AR_DEVELOPMENT_MODE__: string;
 declare const __AR_WEBSITE__: string;
@@ -48,65 +49,65 @@ export default class AR
     }
 
     /**
-     * Trackers
+     * Checks if the engine can be run in the browser the client is using
+     * @returns true if the engine is compatible with the browser
      */
-    static get Tracker(): typeof TrackerFactory
+    static isSupported(): boolean
     {
-        return TrackerFactory;
+        return Session.isSupported();
     }
 
     /**
-     * Sources of data
+     * Engine version
      */
-    static get Source(): typeof SourceFactory
+    static get version(): string
     {
-        return SourceFactory;
+        if(__AR_DEVELOPMENT_MODE__)
+            return __AR_VERSION__ + '-dev';
+        else
+            return __AR_VERSION__;
     }
 
     /**
-     * Create a viewport
-     * @param settings
-     * @returns a new viewport with the specified settings
+     * Speedy Vision
      */
-    static Viewport(settings: ViewportSettings): Viewport
+    static get Speedy(): typeof Speedy
     {
-        return new Viewport(settings);
+        return Speedy;
     }
 
     /**
-     * Global Settings
+     * Trackers
      */
-    static get Settings(): typeof Settings
+    static get Tracker(): typeof TrackerFactory
     {
-        return Settings;
+        return TrackerFactory;
     }
 
     /**
-     * Engine version
+     * Sources of data
      */
-    static get version(): string
+    static get Source(): typeof SourceFactory
     {
-        if(__AR_DEVELOPMENT_MODE__)
-            return __AR_VERSION__ + '-dev';
-        else
-            return __AR_VERSION__;
+        return SourceFactory;
     }
 
     /**
-     * Speedy Vision
+     * Create a viewport
+     * @param settings
+     * @returns a new viewport with the specified settings
      */
-    static get Speedy(): typeof Speedy
+    static Viewport(settings: ViewportSettings): Viewport
     {
-        return Speedy;
+        return new Viewport(settings);
     }
 
     /**
-     * Checks if the engine can be run in the browser the client is using
-     * @returns true if the engine is compatible with the browser
+     * Global Settings
      */
-    static isSupported(): boolean
+    static get Settings(): typeof Settings
     {
-        return Session.isSupported();
+        return Settings;
     }
 }