utils(Orientation): Orientation utils provides a method to create qua…

…ternion from properties (roll, pitch, heading, or omega phi kappa)

jsdoc-config.json

@@ -48,7 +48,8 @@
             "src/Renderer/ThreeExtended/PlanarControls.js",
             "src/Renderer/ThreeExtended/FirstPersonControls.js",
 
-            "src/utils/CameraUtils.js"
+            "src/utils/CameraUtils.js",
+            "src/utils/OrientationUtils.js"
         ]
     }
 }

src/Main.js

@@ -29,3 +29,4 @@ export { default as FeaturesUtils } from './Renderer/ThreeExtended/FeaturesUtils
 export { CONTROL_EVENTS } from './Renderer/ThreeExtended/GlobeControls';
 export { default as DEMUtils } from './utils/DEMUtils';
 export { default as CameraUtils } from './utils/CameraUtils';
+export { default as OrientationUtils } from './utils/OrientationUtils';

src/utils/OrientationUtils.js

@@ -0,0 +1,121 @@
+import * as THREE from 'three';
+
+/** @module OrientationUtils */
+
+const DEG2RAD = THREE.Math.DEG2RAD;
+
+// The transform from world to local is  RotationZ(heading).RotationX(pitch).RotationY(roll)
+// The transform from local to world is (RotationZ(heading).RotationX(pitch).RotationY(roll)).transpose()
+function quaternionFromRollPitchHeading(roll = 0, pitch = 0, heading = 0, target) {
+    roll *= DEG2RAD;
+    pitch *= DEG2RAD;
+    heading *= DEG2RAD;
+    // return this.setFromEuler(new THREE.Euler(pitch, roll, heading , 'ZXY')).conjugate();
+    return target.setFromEuler(new THREE.Euler(-pitch, -roll, -heading, 'YXZ')); // optimized version of above
+}
+
+// From DocMicMac, the transform from local to world is:
+// RotationX(omega).RotationY(phi).RotationZ(kappa).RotationX(PI)
+// RotationX(PI) = Scale(1, -1, -1) converts between the 2 conventions for the camera local frame:
+//  X right, Y bottom, Z front : convention in webGL, threejs and computer vision
+//  X right, Y top,    Z back  : convention in photogrammetry
+function quaternionFromOmegaPhiKappa(omega = 0, phi = 0, kappa = 0, target) {
+    omega *= DEG2RAD;
+    phi *= DEG2RAD;
+    kappa *= DEG2RAD;
+    target.setFromEuler(new THREE.Euler(omega, phi, kappa, 'XYZ'));
+    // target.setFromRotationMatrix(new THREE.Matrix4().makeRotationFromQuaternion(target).scale(new THREE.Vector3(1, -1, -1)));
+    target.set(target.w, target.z, -target.y, -target.x); // optimized version of above
+    return target;
+}
+
+// Set East North Up Orientation from geodesic normal
+// target - the quaternion to set
+// up - the normalized geodetic normal to the ellipsoid (given by Coordinates.geodeticNormal)
+var quaternionENUFromGeodesicNormal = (() => {
+    const matrix = new THREE.Matrix4();
+    const elements = matrix.elements;
+    const north = new THREE.Vector3();
+    const east = new THREE.Vector3();
+    return function setENUFromGeodesicNormal(up, target = new THREE.Quaternion()) {
+        // this is an optimized version of matrix.lookAt(up, new THREE.Vector3(0, 0, 0), new THREE.Vector3(0, 0, 1));
+        east.set(-up.y, up.x, 0);
+        east.normalize();
+        north.crossVectors(up, east);
+        elements[0] = east.x; elements[4] = north.x; elements[8] = up.x;
+        elements[1] = east.y; elements[5] = north.y; elements[9] = up.y;
+        elements[2] = east.z; elements[6] = north.z; elements[10] = up.z;
+        return target.setFromRotationMatrix(matrix);
+    };
+})();
+
+/**
+ *
+ * @typedef Attitude
+ * @type {Object}
+ *
+ * @property {Number} omega - angle in degrees
+ * @property {Number} phi - angle in degrees
+ * @property {Number} kappa - angle in degrees
+ * @property {Number} roll - angle in degrees
+ * @property {Number} pitch - angle in degrees
+ * @property {Number} heading - angle in degrees
+ */
+
+
+const ENUQuat = new THREE.Quaternion();
+
+export default {
+
+    /**
+     * @function localQuaternionFromAttitude
+     * @param {Attitude} attitude - [Attitude]{@link module:OrientationParser~Attitude}
+     * with properties: (omega, phi, kappa), (roll, pitch, heading) or none.
+     * Note that convergence of the meridians is not taken into account.
+     * @param {THREE.Quaternion} target Quaternion to set
+     * @returns {THREE.Quaternion} Quaternion representing the rotation
+     */
+    localQuaternionFromAttitude(attitude, target = new THREE.Quaternion()) {
+        if ((attitude.roll !== undefined) || (attitude.pitch !== undefined) || (attitude.heading !== undefined)) {
+            return quaternionFromRollPitchHeading(attitude.roll, attitude.pitch, attitude.heading, target);
+        }
+        if ((attitude.omega !== undefined) || (attitude.phi !== undefined) || (attitude.kappa !== undefined)) {
+            return quaternionFromOmegaPhiKappa(attitude.omega, attitude.phi, attitude.kappa, target);
+        }
+        return target.set(0, 0, 0, 1);
+    },
+
+    /**
+     * @function globeQuaternionFromAttitude
+     * @param {Attitude} attitude - [Attitude]{@link module:OrientationParser~Attitude}
+     * with properties: (omega, phi, kappa), (roll, pitch, heading) or none.
+     * @param {Coordinates} coordinate position on the globe
+     * @param {THREE.Quaternion} target Quaternion to set
+     * @returns {THREE.Quaternion} Quaternion representing the rotation
+     */
+    globeQuaternionFromAttitude(attitude, coordinate, target = new THREE.Quaternion()) {
+        quaternionENUFromGeodesicNormal(coordinate.geodesicNormal, ENUQuat);
+        this.localQuaternionFromAttitude(attitude, target);
+        target.premultiply(ENUQuat);
+        return target;
+    },
+
+    /** Read rotation information (roll pitch heading or omega phi kappa),
+     * Create a ThreeJs quaternion representing a rotation.
+     *
+     * @function quaternionFromAttitude
+     * @param {Attitude} attitude - [Attitude]{@link module:OrientationParser~Attitude}
+     * @param {Coordinates} coordinate position the oject (used to apply another rotation on Globe CRS)
+     * @param {Boolean} needsENUFromGeodesicNormal should be true on globe CRS.
+     * If true, we will apply another rotation : The rotation use to create ENU local space at coordinate parameter position.
+     * @param {THREE.Quaternion} target Quaternion to set
+     * @returns {THREE.Quaternion} Quaternion representing the rotation
+     */
+    quaternionFromAttitude(attitude, coordinate, needsENUFromGeodesicNormal, target = new THREE.Quaternion()) {
+        if (needsENUFromGeodesicNormal) {
+            return this.globeQuaternionFromAttitude(attitude, coordinate, target);
+        } else {
+            return this.localQuaternionFromAttitude(attitude, target);
+        }
+    },
+};

test/unit/orientationUtils.js

@@ -0,0 +1,154 @@
+import * as THREE from 'three';
+import assert from 'assert';
+import OrientationUtils from '../../src/utils/OrientationUtils';
+import Coordinates from '../../src/Core/Geographic/Coordinates';
+
+// Asster two float number are equals, with 5 digits precision.
+function assertFloatEqual(float1, float2, msg, precision = 15) {
+    assert.equal(Number(float1).toFixed(precision), Number(float2).toFixed(precision), msg);
+}
+function quaternionToString(q) {
+    return `quaternion : _x: ${q._x}, _y: ${q._y}, _z: ${q._z}, _w: ${q._w}`;
+}
+// Assert two quaternion objects are equals.
+function assertQuatEqual(q1, q2, message = 'Quaternion comparaison') {
+    try {
+        assertFloatEqual(q1._x, q2._x, '_x not equal');
+        assertFloatEqual(q1._y, q2._y, '_y not equal');
+        assertFloatEqual(q1._z, q2._z, '_z not equal');
+        assertFloatEqual(q1._w, q2._w, '_w not equal');
+    } catch (e) {
+        if (e instanceof assert.AssertionError) {
+            assert.fail(`${message}\n${e}\nExpected : ${quaternionToString(q1)}\nActual : ${quaternionToString(q2)}`);
+        } else {
+            assert.fail(e);
+        }
+    }
+}
+
+function testQuaternionFromAttitude(input, expected) {
+    var actual = OrientationUtils.localQuaternionFromAttitude(input);
+    var message = `Input should be parsed properly : ${input}`;
+
+    assertQuatEqual(expected, actual, message);
+}
+
+function RollPitchHeadingToString() {
+    return `roll: ${this.roll}, pitch: ${this.pitch}, heading: ${this.heading}}`;
+}
+
+function OmegaPhiKappaToString() {
+    return `omega: ${this.omega}, phi: ${this.phi}, kappa: ${this.kappa}`;
+}
+
+describe('OrientationUtils localQuaternionFromAttitude', function () {
+    it('should parse empty input', function () {
+        var input = {};
+        var expected = new THREE.Quaternion();
+        testQuaternionFromAttitude(input, expected);
+    });
+
+    it('should parse roll pitch heading unity', function () {
+        var input = {
+            roll: 0,
+            toString: RollPitchHeadingToString,
+        };
+        var expected = new THREE.Quaternion();
+
+        testQuaternionFromAttitude(input, expected);
+    });
+
+    it('should parse roll', function () {
+        var input = {
+            roll: -180,
+            toString: RollPitchHeadingToString,
+        };
+        var expected = new THREE.Quaternion();
+        expected.setFromAxisAngle(new THREE.Vector3(0, 1, 0), Math.PI);
+
+        testQuaternionFromAttitude(input, expected);
+    });
+
+    it('should parse pitch', function () {
+        var input = {
+            pitch: -180,
+            toString: RollPitchHeadingToString,
+        };
+        var expected = new THREE.Quaternion();
+        expected.setFromAxisAngle(new THREE.Vector3(1, 0, 0), Math.PI);
+
+        testQuaternionFromAttitude(input, expected);
+    });
+
+    it('should parse heading', function () {
+        var input = {
+            heading: -180,
+            toString: RollPitchHeadingToString,
+        };
+
+        var expected = new THREE.Quaternion();
+        expected.setFromAxisAngle(new THREE.Vector3(0, 0, 1), Math.PI);
+
+        testQuaternionFromAttitude(input, expected);
+    });
+
+    it('should parse omega phi kappa', function () {
+        var input = {
+            omega: 0,
+            phi: 0,
+            kappa: 0,
+            toString: OmegaPhiKappaToString,
+        };
+
+        var expected = new THREE.Quaternion();
+        expected.setFromAxisAngle(new THREE.Vector3(1, 0, 0), Math.PI);
+
+        testQuaternionFromAttitude(input, expected);
+    });
+});
+
+
+describe('OrientationUtils globeQuaternionFromAttitude', function () {
+    it('should set ENU quaternion from greenwich on ecuador', function () {
+        var coord = new Coordinates('EPSG:4326', 0, 0);
+        var input = {
+            roll: 0,
+            pitch: 0,
+            heading: 0,
+            toString() { return `roll: ${this.roll}, pitch: ${this.pitch}, heading: ${this.heading}`; },
+        };
+
+        var actual = OrientationUtils.globeQuaternionFromAttitude(input, coord);
+
+        var expected = new THREE.Quaternion();
+        expected.setFromEuler(new THREE.Euler(0, Math.PI / 2, Math.PI / 2, 'YZX'));
+
+        assertQuatEqual(expected, actual);
+    });
+});
+
+describe('OrientationUtils parser', function () {
+    it('should parse most simple empty data', function () {
+        var properties = {};
+        var coord; // coord is undefined because it's not used when applyRotationForGlobe is false.
+        var applyRotationForGlobeView = false;
+
+        var actual = OrientationUtils.quaternionFromAttitude(properties, coord, applyRotationForGlobeView);
+
+        var expected = new THREE.Quaternion();
+        assertQuatEqual(expected, actual);
+    });
+
+    it('should parse simple data in globe crs', function () {
+        var properties = {};
+        var coord = new Coordinates('EPSG:4326', 0, 0);
+        var applyRotationForGlobeView = true;
+
+        var actual = OrientationUtils.quaternionFromAttitude(properties, coord, applyRotationForGlobeView);
+
+        var expected = new THREE.Quaternion();
+        expected.setFromEuler(new THREE.Euler(0, Math.PI / 2, Math.PI / 2, 'YZX'));
+        assertQuatEqual(expected, actual);
+    });
+});
+