Merge pull request #4048 from kaktus40/adding_AircraftTransformations

adding method to convert heading pitch roll in quaternion defined in …
CesiumGS · Jun 28, 2016 · 1ffee8b · 1ffee8b
2 parents 191a93b + 00cdda8
commit 1ffee8b
Show file tree

Hide file tree

Showing 3 changed files with 194 additions and 1 deletion.
diff --git a/CHANGES.md b/CHANGES.md
@@ -20,10 +20,10 @@ Change Log
 * Added the hot air balloon sample model.
 * Fixed handling of sampled Rectangle coordinates in CZML. [#4033](https://github.com/AnalyticalGraphicsInc/cesium/pull/4033)
 * Fix "Cannot read property 'x' of undefined" error when calling SceneTransforms.wgs84ToWindowCoordinates in certain cases. [#4022](https://github.com/AnalyticalGraphicsInc/cesium/pull/4022)
+* Added transform functions to generate 4X4 matrix or quaternion from heading, pitch, roll of an aircraft (use of North East Down local reference)
 * Exposed a parametric ray-triangle intersection test to the API as `IntersectionTests.rayTriangleParametric`.
 * Added `unsupportedNodeEvent` to `KmlDataSource` that is fired whenever an unsupported node is encountered.
 
-
 ### 1.22.2 - 2016-06-14
 * This is an npm only release to fix the improperly published 1.22.1. There were no code changes.
 

diff --git a/Source/Core/Transforms.js b/Source/Core/Transforms.js
@@ -381,6 +381,36 @@ define([
         return Matrix4.multiply(result, hprMatrix, result);
     };
 
+    /**
+     * Computes a 4x4 transformation matrix from a reference frame with axes computed from the heading-pitch-roll angles
+     * centered at the provided origin to the provided ellipsoid's fixed reference frame. Heading is the rotation from the local north
+     * direction where a positive angle is increasing eastward. Pitch is the rotation from the local east-north plane. Positive pitch angles
+     * are above the plane. Negative pitch angles are below the plane. Roll is the first rotation applied about the local east axis.
+     *
+     * @param {Cartesian3} origin The center point of the local reference frame.
+     * @param {Number} heading The heading angle in radians.
+     * @param {Number} pitch The pitch angle in radians.
+     * @param {Number} roll The roll angle in radians.
+     * @param {Ellipsoid} [ellipsoid=Ellipsoid.WGS84] The ellipsoid whose fixed frame is used in the transformation.
+     * @param {Matrix4} [result] The object onto which to store the result.
+     * @returns {Matrix4} The modified result parameter or a new Matrix4 instance if none was provided.
+     *
+     * @example
+     * // Get the transform from local heading-pitch-roll at cartographic (0.0, 0.0) to Earth's fixed frame.
+     * var center = Cesium.Cartesian3.fromDegrees(0.0, 0.0);
+     * var heading = -Cesium.Math.PI_OVER_TWO;
+     * var pitch = Cesium.Math.PI_OVER_FOUR;
+     * var roll = 0.0;
+     * var transform = Cesium.Transforms.aircraftHeadingPitchRollToFixedFrame(center, heading, pitch, roll);
+     */
+    Transforms.aircraftHeadingPitchRollToFixedFrame = function(origin, heading, pitch, roll, ellipsoid, result) {
+        // checks for required parameters happen in the called functions
+        var hprQuaternion = Quaternion.fromHeadingPitchRoll(heading, pitch, roll, scratchHPRQuaternion);
+        var hprMatrix = Matrix4.fromTranslationQuaternionRotationScale(Cartesian3.ZERO, hprQuaternion, scratchScale, scratchHPRMatrix4);
+        result = Transforms.northEastDownToFixedFrame(origin, ellipsoid, result);
+        return Matrix4.multiply(result, hprMatrix, result);
+    };
+
     var scratchENUMatrix4 = new Matrix4();
     var scratchHPRMatrix3 = new Matrix3();
 
@@ -413,6 +443,35 @@ define([
         return Quaternion.fromRotationMatrix(rotation, result);
     };
 
+    /**
+     * Computes a quaternion from a reference frame with axes computed from the heading-pitch-roll angles
+     * centered at the provided origin. Heading is the rotation from the local north
+     * direction where a positive angle is increasing eastward. Pitch is the rotation from the local east-north plane. Positive pitch angles
+     * are above the plane. Negative pitch angles are below the plane. Roll is the first rotation applied about the local east axis.
+     *
+     * @param {Cartesian3} origin The center point of the local reference frame.
+     * @param {Number} heading The heading angle in radians.
+     * @param {Number} pitch The pitch angle in radians.
+     * @param {Number} roll The roll angle in radians.
+     * @param {Ellipsoid} [ellipsoid=Ellipsoid.WGS84] The ellipsoid whose fixed frame is used in the transformation.
+     * @param {Quaternion} [result] The object onto which to store the result.
+     * @returns {Quaternion} The modified result parameter or a new Quaternion instance if none was provided.
+     *
+     * @example
+     * // Get the quaternion from local heading-pitch-roll at cartographic (0.0, 0.0) to Earth's fixed frame.
+     * var center = Cesium.Cartesian3.fromDegrees(0.0, 0.0);
+     * var heading = -Cesium.Math.PI_OVER_TWO;
+     * var pitch = Cesium.Math.PI_OVER_FOUR;
+     * var roll = 0.0;
+     * var quaternion = Cesium.Transforms.aircraftHeadingPitchRollQuaternion(center, heading, pitch, roll);
+     */
+    Transforms.aircraftHeadingPitchRollQuaternion = function(origin, heading, pitch, roll, ellipsoid, result) {
+        // checks for required parameters happen in the called functions
+        var transform = Transforms.aircraftHeadingPitchRollToFixedFrame(origin, heading, pitch, roll, ellipsoid, scratchENUMatrix4);
+        var rotation = Matrix4.getRotation(transform, scratchHPRMatrix3);
+        return Quaternion.fromRotationMatrix(rotation, result);
+    };
+
 
     var gmstConstant0 = 6 * 3600 + 41 * 60 + 50.54841;
     var gmstConstant1 = 8640184.812866;

diff --git a/Specs/Core/TransformsSpec.js b/Specs/Core/TransformsSpec.js
@@ -243,6 +243,62 @@ defineSuite([
         expect(actualTranslation).toEqual(origin);
     });
 
+    it('aircraftHeadingPitchRollToFixedFrame works without a result parameter', function() {
+        var origin = new Cartesian3(1.0, 0.0, 0.0);
+        var heading = CesiumMath.toRadians(20.0);
+        var pitch = CesiumMath.toRadians(30.0);
+        var roll = CesiumMath.toRadians(40.0);
+
+        var expectedRotation = Matrix3.fromQuaternion(Quaternion.fromHeadingPitchRoll(heading, pitch, roll));
+        var expectedX = Matrix3.getColumn(expectedRotation, 0, new Cartesian3());
+        var expectedY = Matrix3.getColumn(expectedRotation, 1, new Cartesian3());
+        var expectedZ = Matrix3.getColumn(expectedRotation, 2, new Cartesian3());
+
+        Cartesian3.fromElements(-expectedX.z, expectedX.y, expectedX.x, expectedX);
+        Cartesian3.fromElements(-expectedY.z, expectedY.y, expectedY.x, expectedY);
+        Cartesian3.fromElements(-expectedZ.z, expectedZ.y, expectedZ.x, expectedZ);
+
+        var returnedResult = Transforms.aircraftHeadingPitchRollToFixedFrame(origin, heading, pitch, roll, Ellipsoid.UNIT_SPHERE);
+        var actualX = Cartesian3.fromCartesian4(Matrix4.getColumn(returnedResult, 0, new Cartesian4()));
+        var actualY = Cartesian3.fromCartesian4(Matrix4.getColumn(returnedResult, 1, new Cartesian4()));
+        var actualZ = Cartesian3.fromCartesian4(Matrix4.getColumn(returnedResult, 2, new Cartesian4()));
+        var actualTranslation = Cartesian3.fromCartesian4(Matrix4.getColumn(returnedResult, 3, new Cartesian4()));
+
+        expect(actualX).toEqual(expectedX);
+        expect(actualY).toEqual(expectedY);
+        expect(actualZ).toEqual(expectedZ);
+        expect(actualTranslation).toEqual(origin);
+    });
+
+    it('aircraftHeadingPitchRollToFixedFrame works with a result parameter', function() {
+        var origin = new Cartesian3(1.0, 0.0, 0.0);
+        var heading = CesiumMath.toRadians(20.0);
+        var pitch = CesiumMath.toRadians(30.0);
+        var roll = CesiumMath.toRadians(40.0);
+
+        var expectedRotation = Matrix3.fromQuaternion(Quaternion.fromHeadingPitchRoll(heading, pitch, roll));
+        var expectedX = Matrix3.getColumn(expectedRotation, 0, new Cartesian3());
+        var expectedY = Matrix3.getColumn(expectedRotation, 1, new Cartesian3());
+        var expectedZ = Matrix3.getColumn(expectedRotation, 2, new Cartesian3());
+
+        Cartesian3.fromElements(-expectedX.z, expectedX.y, expectedX.x, expectedX);
+        Cartesian3.fromElements(-expectedY.z, expectedY.y, expectedY.x, expectedY);
+        Cartesian3.fromElements(-expectedZ.z, expectedZ.y, expectedZ.x, expectedZ);
+
+        var result = new Matrix4();
+        var returnedResult = Transforms.aircraftHeadingPitchRollToFixedFrame(origin, heading, pitch, roll, Ellipsoid.UNIT_SPHERE, result);
+        var actualX = Cartesian3.fromCartesian4(Matrix4.getColumn(returnedResult, 0, new Cartesian4()));
+        var actualY = Cartesian3.fromCartesian4(Matrix4.getColumn(returnedResult, 1, new Cartesian4()));
+        var actualZ = Cartesian3.fromCartesian4(Matrix4.getColumn(returnedResult, 2, new Cartesian4()));
+        var actualTranslation = Cartesian3.fromCartesian4(Matrix4.getColumn(returnedResult, 3, new Cartesian4()));
+
+        expect(returnedResult).toBe(result);
+        expect(actualX).toEqual(expectedX);
+        expect(actualY).toEqual(expectedY);
+        expect(actualZ).toEqual(expectedZ);
+        expect(actualTranslation).toEqual(origin);
+    });
+
     it('headingPitchRollQuaternion works without a result parameter', function() {
         var origin = new Cartesian3(1.0, 0.0, 0.0);
         var heading = CesiumMath.toRadians(20.0);
@@ -273,6 +329,36 @@ defineSuite([
         expect(actual).toEqualEpsilon(expected, CesiumMath.EPSILON11);
     });
 
+    it('aircraftHeadingPitchRollQuaternion works without a result parameter', function() {
+        var origin = new Cartesian3(1.0, 0.0, 0.0);
+        var heading = CesiumMath.toRadians(20.0);
+        var pitch = CesiumMath.toRadians(30.0);
+        var roll = CesiumMath.toRadians(40.0);
+
+        var transform = Transforms.aircraftHeadingPitchRollToFixedFrame(origin, heading, pitch, roll, Ellipsoid.UNIT_SPHERE);
+        var expected = Matrix4.getRotation(transform, new Matrix3());
+
+        var quaternion = Transforms.aircraftHeadingPitchRollQuaternion(origin, heading, pitch, roll, Ellipsoid.UNIT_SPHERE);
+        var actual = Matrix3.fromQuaternion(quaternion);
+        expect(actual).toEqualEpsilon(expected, CesiumMath.EPSILON11);
+    });
+
+    it('aircraftHeadingPitchRollQuaternion works with a result parameter', function() {
+        var origin = new Cartesian3(1.0, 0.0, 0.0);
+        var heading = CesiumMath.toRadians(20.0);
+        var pitch = CesiumMath.toRadians(30.0);
+        var roll = CesiumMath.toRadians(40.0);
+
+        var transform = Transforms.aircraftHeadingPitchRollToFixedFrame(origin, heading, pitch, roll, Ellipsoid.UNIT_SPHERE);
+        var expected = Matrix4.getRotation(transform, new Matrix3());
+
+        var result = new Quaternion();
+        var quaternion = Transforms.aircraftHeadingPitchRollQuaternion(origin, heading, pitch, roll, Ellipsoid.UNIT_SPHERE, result);
+        var actual = Matrix3.fromQuaternion(quaternion);
+        expect(quaternion).toBe(result);
+        expect(actual).toEqualEpsilon(expected, CesiumMath.EPSILON11);
+    });
+
     it('computeTemeToPseudoFixedMatrix works before noon', function() {
         var time = JulianDate.fromDate(new Date('June 29, 2015 12:00:00 UTC'));
         var t = Transforms.computeTemeToPseudoFixedMatrix(time);
@@ -795,6 +881,54 @@ defineSuite([
         }).toThrowDeveloperError();
     });
 
+    it('aircraftHeadingPitchRollToFixedFrame throws without an origin', function() {
+        expect(function() {
+            Transforms.aircraftHeadingPitchRollToFixedFrame(undefined, 0.0, 0.0, 0.0);
+        }).toThrowDeveloperError();
+    });
+
+    it('aircraftHeadingPitchRollToFixedFrame throws without an heading', function() {
+        expect(function() {
+            Transforms.aircraftHeadingPitchRollToFixedFrame(Cartesian3.ZERO, undefined, 0.0, 0.0);
+        }).toThrowDeveloperError();
+    });
+
+    it('aircraftHeadingPitchRollToFixedFrame throws without an pitch', function() {
+        expect(function() {
+            Transforms.aircraftHeadingPitchRollToFixedFrame(Cartesian3.ZERO, 0.0, undefined, 0.0);
+        }).toThrowDeveloperError();
+    });
+
+    it('aircraftHeadingPitchRollToFixedFrame throws without an roll', function() {
+        expect(function() {
+            Transforms.aircraftHeadingPitchRollToFixedFrame(Cartesian3.ZERO, 0.0, 0.0, undefined);
+        }).toThrowDeveloperError();
+    });
+
+    it('aircraftHeadingPitchRollQuaternion throws without an origin', function() {
+        expect(function() {
+            Transforms.aircraftHeadingPitchRollQuaternion(undefined, 0.0, 0.0, 0.0);
+        }).toThrowDeveloperError();
+    });
+
+    it('aircraftHeadingPitchRollQuaternion throws without an heading', function() {
+        expect(function() {
+            Transforms.aircraftHeadingPitchRollQuaternion(Cartesian3.ZERO, undefined, 0.0, 0.0);
+        }).toThrowDeveloperError();
+    });
+
+    it('aircraftHeadingPitchRollQuaternion throws without an pitch', function() {
+        expect(function() {
+            Transforms.aircraftHeadingPitchRollQuaternion(Cartesian3.ZERO, 0.0, undefined, 0.0);
+        }).toThrowDeveloperError();
+    });
+
+    it('aircraftHeadingPitchRollQuaternion throws without an roll', function() {
+        expect(function() {
+            Transforms.aircraftHeadingPitchRollQuaternion(Cartesian3.ZERO, 0.0, 0.0, undefined);
+        }).toThrowDeveloperError();
+    });
+
     it('computeTemeToPseudoFixedMatrix throws without a date', function() {
         expect(function() {
             Transforms.computeTemeToPseudoFixedMatrix(undefined);