@dronedeploy/rocos-js-sdk 1.0.0-alpha-2 → 2.6.1

package/utils/localtransform.js

@@ -0,0 +1,50 @@
+"use strict";
+Object.defineProperty(exports, "__esModule", { value: true });
+exports.LocalTransform = void 0;
+const transformationmatrix_1 = require("./transformationmatrix");
+const helperfunctions_1 = require("./helperfunctions");
+// LocalTransform is the implementation of Transform locally within the script
+class LocalTransform {
+    constructor(origin, basis, frameID) {
+        this.origin = origin;
+        this.basis = basis;
+        this.frameID = frameID;
+    }
+    ID() { return this.frameID; }
+    ParentID(tree) { return tree.getParent(this.ID()); }
+    GetRotation() { return this.basis; }
+    GetPosition() { return this.origin; }
+    GetTransformationMatrix() {
+        return new transformationmatrix_1.TransformationMatrix([[this.basis.elements[0][0], this.basis.elements[0][1], this.basis.elements[0][2], this.origin.elements[0][0]],
+            [this.basis.elements[1][0], this.basis.elements[1][1], this.basis.elements[1][2], this.origin.elements[1][0]],
+            [this.basis.elements[2][0], this.basis.elements[2][1], this.basis.elements[2][2], this.origin.elements[2][0]],
+            [0, 0, 0, 1]]);
+    }
+    // ToFrame returns the transformation matrix relative to the new specified frame
+    ToFrame(tree, resultFrameID) {
+        var originalFrameID = this.ID();
+        if (originalFrameID == resultFrameID) {
+            return this.GetTransformationMatrix();
+        }
+        else if (tree.isDescendant(originalFrameID, resultFrameID)) {
+            return helperfunctions_1.frameToFrameTransformationMatrixSeq(tree, tree.getSeqDown(originalFrameID, resultFrameID), false);
+        }
+        else if (tree.isAncestor(originalFrameID, resultFrameID)) {
+            return helperfunctions_1.frameToFrameTransformationMatrixSeq(tree, tree.getSeqDown(resultFrameID, originalFrameID), true);
+        }
+        else { // Frames are on different branches
+            var commonNode = tree.getCommonFrameID(originalFrameID, resultFrameID);
+            // Calculate the transformation matrix of each frame relative to the common node
+            var totalTM1 = helperfunctions_1.frameToFrameTransformationMatrixSeq(tree, tree.getSeqDown(originalFrameID, commonNode), false); // calculate down tree matrix
+            var totalTM2 = helperfunctions_1.frameToFrameTransformationMatrixSeq(tree, tree.getSeqDown(resultFrameID, commonNode), true); // calculate up the tree (inverses)
+            var totalTM = totalTM2.Multiply(totalTM1);
+            if (totalTM instanceof transformationmatrix_1.TransformationMatrix) {
+                return new transformationmatrix_1.TransformationMatrix(totalTM.elements);
+            }
+            else {
+                throw "Erorr occurred";
+            }
+        }
+    }
+}
+exports.LocalTransform = LocalTransform;

package/utils/localtransformtree.d.ts

@@ -0,0 +1,17 @@
+import { FrameID } from './misc';
+import { LocalTransform } from './localtransform';
+export declare class LocalTransformTree {
+    frames: Map<FrameID, LocalTransform>;
+    parents: Map<FrameID, FrameID>;
+    children: Map<FrameID, FrameID[]>;
+    constructor(frames: Map<FrameID, LocalTransform>, parents: Map<FrameID, FrameID>, children: Map<FrameID, FrameID[]>);
+    getFrame(searchID: FrameID): LocalTransform;
+    getSeqDown(start: FrameID, end: FrameID): FrameID[];
+    getCommonFrameID(frame1ID: FrameID, frame2ID: FrameID): FrameID;
+    getParent(searchID: FrameID): FrameID;
+    getChildren(searchID: FrameID): FrameID[];
+    isChild(originalFrameID: FrameID, resultFrameID: FrameID): boolean;
+    isParent(originalFrameID: FrameID, resultFrameID: FrameID): boolean;
+    isAncestor(originalFrameID: FrameID, resultFrameID: FrameID): boolean;
+    isDescendant(originalFrameID: FrameID, resultFrameID: FrameID): boolean;
+}

package/utils/localtransformtree.js

@@ -0,0 +1,88 @@
+"use strict";
+Object.defineProperty(exports, "__esModule", { value: true });
+exports.LocalTransformTree = void 0;
+class LocalTransformTree {
+    constructor(frames, parents, children) {
+        this.frames = frames;
+        this.parents = parents;
+        this.children = children;
+    }
+    // getFrame returns the specified frame
+    getFrame(searchID) {
+        let frame = this.frames.get(searchID);
+        if (frame == null) {
+            throw "Frame does not exist with given ID";
+        }
+        return frame;
+    }
+    // getSeqDown returns the sequence of frame IDs from start to end. If end isn't found, SeqDown returns seq to root
+    getSeqDown(start, end) {
+        if (start == end) {
+            return [start];
+        }
+        var currentFrame = this.getFrame(start);
+        var seq = [];
+        // traverse down branch from end to start and record sequence
+        while (true) {
+            seq.push(currentFrame.frameID);
+            // if end is found
+            if (this.getParent(currentFrame.ID()) == end) {
+                seq.push(end);
+                break;
+            }
+            // Reached root node (frame without parent)
+            if (this.getParent(currentFrame.ID()) == "") {
+                break;
+            }
+            currentFrame = this.getFrame(this.getParent(currentFrame.ID()));
+        }
+        return seq;
+    }
+    // getCommonFrameID returns the common frame ID between the specified frames
+    getCommonFrameID(frame1ID, frame2ID) {
+        var frame1 = this.getFrame(frame1ID);
+        var frame2 = this.getFrame(frame2ID);
+        var frame1Seq = this.getSeqDown(frame1.frameID, "R");
+        var frame2Seq = this.getSeqDown(frame2.frameID, "R");
+        //  Return first common element (if any) --> O(n^2)
+        for (var i = 0; i < frame1Seq.length; i++) {
+            for (var j = 0; j < frame2Seq.length; j++) {
+                if (frame1Seq[i] == frame2Seq[j]) {
+                    return frame1Seq[i];
+                }
+            }
+        }
+        throw "No common node between specified frames";
+    }
+    // Parent returns the parent ID of the specified frame
+    getParent(searchID) {
+        let parents = this.parents.get(searchID);
+        if (parents == null) {
+            throw "Frame does not exist with given ID, cannot retrieve children";
+        }
+        return parents;
+    }
+    // getChildren returns the children of the specified frame
+    getChildren(searchID) {
+        let children = this.children.get(searchID);
+        if (children == null) {
+            throw "Frame does not exist with given ID, cannot retrieve children";
+        }
+        return children;
+    }
+    // isChild determines if originalFrameID is a child of resultFrameID
+    isChild(originalFrameID, resultFrameID) {
+        return this.getChildren(resultFrameID).includes(originalFrameID);
+    }
+    // isParent determines if originalFrameID is the parent of resultFrameID
+    isParent(originalFrameID, resultFrameID) {
+        return this.getParent(resultFrameID) == originalFrameID;
+    }
+    isAncestor(originalFrameID, resultFrameID) {
+        return this.getSeqDown(resultFrameID, originalFrameID).includes(originalFrameID);
+    }
+    isDescendant(originalFrameID, resultFrameID) {
+        return this.getSeqDown(originalFrameID, resultFrameID).includes(resultFrameID);
+    }
+}
+exports.LocalTransformTree = LocalTransformTree;

package/utils/misc.d.ts

@@ -0,0 +1,17 @@
+export declare class FrameID extends String {
+}
+export declare enum Seq {
+    XZX = 0,
+    XYX = 1,
+    YXY = 2,
+    YZY = 3,
+    ZYZ = 4,
+    ZXZ = 5,
+    XZY = 6,
+    XYZ = 7,
+    YXZ = 8,
+    YZX = 9,
+    ZYX = 10,
+    ZXY = 11,
+    NUM = 12
+}

package/utils/misc.js

@@ -0,0 +1,23 @@
+"use strict";
+Object.defineProperty(exports, "__esModule", { value: true });
+exports.Seq = exports.FrameID = void 0;
+class FrameID extends String {
+}
+exports.FrameID = FrameID;
+// Seq type for EulerAngles
+var Seq;
+(function (Seq) {
+    Seq[Seq["XZX"] = 0] = "XZX";
+    Seq[Seq["XYX"] = 1] = "XYX";
+    Seq[Seq["YXY"] = 2] = "YXY";
+    Seq[Seq["YZY"] = 3] = "YZY";
+    Seq[Seq["ZYZ"] = 4] = "ZYZ";
+    Seq[Seq["ZXZ"] = 5] = "ZXZ";
+    Seq[Seq["XZY"] = 6] = "XZY";
+    Seq[Seq["XYZ"] = 7] = "XYZ";
+    Seq[Seq["YXZ"] = 8] = "YXZ";
+    Seq[Seq["YZX"] = 9] = "YZX";
+    Seq[Seq["ZYX"] = 10] = "ZYX";
+    Seq[Seq["ZXY"] = 11] = "ZXY";
+    Seq[Seq["NUM"] = 12] = "NUM";
+})(Seq = exports.Seq || (exports.Seq = {}));

package/utils/positionvector.d.ts

@@ -0,0 +1,13 @@
+import { FrameID } from './misc';
+import { LocalTransformTree } from './localtransformtree';
+export declare class PositionVector {
+    elements: number[][];
+    constructor(elements: number[][]);
+    X(): number;
+    Y(): number;
+    Z(): number;
+    magnitude(): number;
+    unitVector(): PositionVector;
+    Multiply(mB: PositionVector): PositionVector;
+    ToFrame(tree: LocalTransformTree, thisID: FrameID, thatID: FrameID): PositionVector;
+}

package/utils/positionvector.js

@@ -0,0 +1,79 @@
+"use strict";
+Object.defineProperty(exports, "__esModule", { value: true });
+exports.PositionVector = void 0;
+// PositionVector is a 3X1 matrix that contains a vector
+class PositionVector {
+    constructor(elements) {
+        this.elements = elements;
+    }
+    X() { return this.elements[0][0]; }
+    Y() { return this.elements[1][0]; }
+    Z() { return this.elements[2][0]; }
+    // magnitude returns the magnitude of the vector
+    magnitude() {
+        var vec = this.elements;
+        var sum = 0;
+        for (var i = 0; i < vec.length; i++) {
+            sum += vec[i][0] ** 2;
+        }
+        return sum ** 0.5;
+    }
+    // unitVector returns the unit vector of the vector
+    unitVector() {
+        var mag = this.magnitude();
+        var e = this.elements;
+        return new PositionVector([[e[0][0] / mag], [e[1][0] / mag], [e[2][0] / mag]]);
+    }
+    // Multiply multipies with a given matrix
+    Multiply(mB) {
+        var matrixA = this.elements;
+        if (mB instanceof PositionVector) {
+            var e = mB.elements;
+            var matrixB = [[e[0][0]], [e[1][0]], [e[2][0]], [1]];
+        }
+        else {
+            throw "Error occurred";
+        }
+        var aNumRows = matrixA.length, aNumCols = matrixA[0].length, bNumCols = matrixB[0].length, m = new Array(aNumRows);
+        for (var r = 0; r < aNumRows; ++r) {
+            m[r] = new Array(bNumCols);
+            for (var c = 0; c < bNumCols; ++c) {
+                m[r][c] = 0;
+                for (var i = 0; i < aNumCols; ++i) {
+                    m[r][c] += matrixA[r][i] * matrixB[i][c];
+                }
+            }
+        }
+        var rows = m.length;
+        var cols = m[0].length;
+        if (rows == 3 && cols == 1) {
+            return new PositionVector([[m[0][0]], [m[1][0]], [m[2][0]]]);
+        }
+        else {
+            throw "Error occurred";
+        }
+    }
+    // ToFrame converts the position vector to the specified frame
+    ToFrame(tree, thisID, thatID) {
+        var frame1 = tree.getFrame(thisID);
+        var frame2 = tree.getFrame(thatID);
+        // Return vector if already in terms of frame
+        if (frame1.ID() == frame2.ID()) {
+            return this;
+        }
+        // find transformation matrix
+        var totalTransformationMatrix = frame1.ToFrame(tree, frame2.ID());
+        var e = this.elements;
+        // Find new position vector relative to frame 2
+        var newPosVec = totalTransformationMatrix.Multiply(new PositionVector([[e[0][0]], [e[1][0]], [e[2][0]]]));
+        // console.log("newPosVec")
+        // console.log(newPosVec)
+        if (newPosVec instanceof PositionVector) {
+            return new PositionVector([[newPosVec.elements[0][0]], [newPosVec.elements[1][0]], [newPosVec.elements[2][0]]]);
+        }
+        else {
+            throw "Error occured";
+        }
+    }
+}
+exports.PositionVector = PositionVector;

package/utils/quaternion.d.ts

@@ -0,0 +1,20 @@
+import { FrameID } from './misc';
+import { LocalTransformTree } from './localtransformtree';
+import { RotationMatrix } from './rotationmatrix';
+import { EulerAngles } from './eulerangles';
+export declare class Quaternion {
+    x: number;
+    y: number;
+    z: number;
+    w: number;
+    constructor(x: number, y: number, z: number, w: number);
+    X(): number;
+    Y(): number;
+    Z(): number;
+    W(): number;
+    ToRotm(): RotationMatrix;
+    ToEul(): EulerAngles;
+    ToQuat(): Quaternion;
+    ToAxAng(): import("./axisangle").AxisAngle;
+    ToFrame(tree: LocalTransformTree, thisID: FrameID, thatID: FrameID): Quaternion;
+}

package/utils/quaternion.js

@@ -0,0 +1,62 @@
+"use strict";
+Object.defineProperty(exports, "__esModule", { value: true });
+exports.Quaternion = void 0;
+const misc_1 = require("./misc");
+const helperfunctions_1 = require("./helperfunctions");
+const rotationmatrix_1 = require("./rotationmatrix");
+const eulerangles_1 = require("./eulerangles");
+const mathjs_1 = require("mathjs");
+// Quaternion contains the quaternion representation
+class Quaternion {
+    constructor(x, y, z, w) {
+        this.x = x;
+        this.y = y;
+        this.z = z;
+        this.w = w;
+    }
+    X() { return this.x; }
+    Y() { return this.y; }
+    Z() { return this.z; }
+    W() { return this.w; }
+    // ToRotm returns the RotationMatrix representation of the rotation
+    ToRotm() {
+        return new rotationmatrix_1.RotationMatrix([[1 - 2 * this.y ** 2 - 2 * this.z ** 2, 2 * this.x * this.y - 2 * this.z * this.w, 2 * this.x * this.z + 2 * this.y * this.w],
+            [2 * this.x * this.y + 2 * this.z * this.w, 1 - 2 * this.x ** 2 - 2 * this.z ** 2, 2 * this.y * this.z - 2 * this.x * this.w],
+            [2 * this.x * this.z - 2 * this.y * this.w, 2 * this.y * this.z + 2 * this.x * this.w, 1 - 2 * this.x ** 2 - 2 * this.y ** 2]]);
+    }
+    // ToEul returns the EulerAngles representation of the rotation
+    ToEul() {
+        var angles = new eulerangles_1.EulerAngles(0, 0, 0, misc_1.Seq.ZYX);
+        // roll (x-axis rotation)
+        var sinr_cosp = 2 * (this.w * this.x + this.y * this.z);
+        var cosr_cosp = 1 - 2 * (this.x * this.x + this.y * this.y);
+        angles.gamma = mathjs_1.atan2(sinr_cosp, cosr_cosp);
+        // pitch (y-axis rotation)
+        var sinp = 2 * (this.w * this.y - this.z * this.x);
+        if (mathjs_1.abs(sinp) >= 1)
+            angles.beta = helperfunctions_1.copysign(mathjs_1.pi / 2, sinp); // use 90 degrees if out of range
+        else
+            angles.beta = mathjs_1.asin(sinp);
+        // yaw (z-axis rotation)
+        var siny_cosp = 2 * (this.w * this.z + this.x * this.y);
+        var cosy_cosp = 1 - 2 * (this.y * this.y + this.z * this.z);
+        angles.alpha = mathjs_1.atan2(siny_cosp, cosy_cosp);
+        return angles;
+    }
+    // ToQuat returns the Quaternion representation of the rotation
+    ToQuat() {
+        return this;
+    }
+    // ToAxAng returns the AxisAngle representation of the rotation
+    ToAxAng() {
+        var rotm = this.ToRotm();
+        return rotm.ToAxAng();
+    }
+    // ToFrame returns the quaternion relative to the new specified frame
+    ToFrame(tree, thisID, thatID) {
+        var rotm = this.ToRotm();
+        var newRotm = rotm.ToFrame(tree, thisID, thatID);
+        return newRotm.ToQuat();
+    }
+}
+exports.Quaternion = Quaternion;

package/utils/rotationmatrix.d.ts

@@ -0,0 +1,23 @@
+import { FrameID } from './misc';
+import { LocalTransformTree } from './localtransformtree';
+import { PositionVector } from './positionvector';
+import { Vector3 } from './vector3';
+import { EulerAngles } from './eulerangles';
+import { Quaternion } from './quaternion';
+import { AxisAngle } from './axisangle';
+export declare class RotationMatrix {
+    elements: number[][];
+    constructor(elements: number[][]);
+    Col1(): Vector3;
+    Col2(): Vector3;
+    Col3(): Vector3;
+    Row1(): Vector3;
+    Row2(): Vector3;
+    Row3(): Vector3;
+    Multiply(mB: RotationMatrix | PositionVector): RotationMatrix | PositionVector;
+    ToRotm(): RotationMatrix;
+    ToEul(): EulerAngles;
+    ToQuat(): Quaternion;
+    ToAxAng(): AxisAngle;
+    ToFrame(tree: LocalTransformTree, thisID: FrameID, thatID: FrameID): RotationMatrix;
+}

package/utils/rotationmatrix.js

@@ -0,0 +1,262 @@
+"use strict";
+Object.defineProperty(exports, "__esModule", { value: true });
+exports.RotationMatrix = void 0;
+const misc_1 = require("./misc");
+const positionvector_1 = require("./positionvector");
+const vector3_1 = require("./vector3");
+const eulerangles_1 = require("./eulerangles");
+const quaternion_1 = require("./quaternion");
+const axisangle_1 = require("./axisangle");
+const mathjs_1 = require("mathjs");
+// RotationMatrix is 3x3 matrix representing rotation
+class RotationMatrix {
+    constructor(elements) {
+        this.elements = elements;
+    }
+    // Col1 returns the first column of the rotation matrix
+    Col1() {
+        let e = this.elements;
+        return new vector3_1.Vector3([[e[0][0]], [e[1][0]], [e[2][0]]]);
+    }
+    // Col2 returns the second column of the rotation matrix
+    Col2() {
+        let e = this.elements;
+        return new vector3_1.Vector3([[e[0][1]], [e[1][1]], [e[2][1]]]);
+    }
+    // Col3 returns the third column of the rotation matrix
+    Col3() {
+        let e = this.elements;
+        return new vector3_1.Vector3([[e[0][2]], [e[1][2]], [e[2][2]]]);
+    }
+    // Row1 returns the first row of the rotation matrix
+    Row1() {
+        let e = this.elements;
+        return new vector3_1.Vector3([[e[0][0]], [e[0][1]], [e[0][2]]]);
+    }
+    // Row2 returns the second row of the rotation matrix
+    Row2() {
+        let e = this.elements;
+        return new vector3_1.Vector3([[e[1][0]], [e[1][1]], [e[2][2]]]);
+    }
+    // Row3 returns the third row of the rotation matrix
+    Row3() {
+        let e = this.elements;
+        return new vector3_1.Vector3([[e[2][0]], [e[2][1]], [e[2][2]]]);
+    }
+    // Multiply multiplies the rotation matrix with given matrix
+    Multiply(mB) {
+        var matrixA = this.elements;
+        var matrixB = mB.elements;
+        // Multiply the two matrices
+        var aNumRows = matrixA.length, aNumCols = matrixA[0].length, bNumCols = matrixB[0].length, m = new Array(aNumRows);
+        for (var r = 0; r < aNumRows; ++r) {
+            m[r] = new Array(bNumCols);
+            for (var c = 0; c < bNumCols; ++c) {
+                m[r][c] = 0;
+                for (var i = 0; i < aNumCols; ++i) {
+                    m[r][c] += matrixA[r][i] * matrixB[i][c];
+                }
+            }
+        }
+        var rows = m.length;
+        var cols = m[0].length;
+        if (rows == 3 && cols == 3) {
+            var result = [
+                [m[0][0], m[0][1], m[0][2]],
+                [m[1][0], m[1][1], m[1][2]],
+                [m[2][0], m[2][1], m[2][2]]
+            ];
+            return new RotationMatrix(result);
+        }
+        else if (rows == 3 && cols == 1) {
+            return new positionvector_1.PositionVector([[m[0][0]], [m[1][0]], [m[2][0]]]);
+        }
+        else {
+            throw "Error occurred";
+        }
+    }
+    // ToRotm returns the RotationMatrix representation of the rotation
+    ToRotm() {
+        return this;
+    }
+    // ToEul returns the EulerAngles representation of the rotation
+    ToEul() {
+        var rotm = this.elements;
+        var m00 = rotm[0][0];
+        var m10 = rotm[1][0];
+        var m20 = rotm[2][0];
+        var m01 = rotm[0][1];
+        //var m11 = rotm[1][1]
+        var m21 = rotm[2][1];
+        var m02 = rotm[0][2];
+        //var m12 = rotm[1][2]
+        var m22 = rotm[2][2];
+        // https://www.gregslabaugh.net/publications/euler.pdf
+        // X: PSI   Y: THETA    Z: PHI
+        //     A         B          G
+        // Uncomment second variables to get second solution
+        if (m20 != 1 && m20 != -1) {
+            var theta1 = -mathjs_1.asin(m20);
+            //var theta2 = pi - theta1
+            var psi1 = mathjs_1.atan2(m21 / mathjs_1.cos(theta1), (m22 / mathjs_1.cos(theta1)));
+            //var psi2 = atan2(m12/cos(theta2), (m22/cos(theta2)))
+            var phi1 = mathjs_1.atan2(m10 / mathjs_1.cos(theta1), (m00 / mathjs_1.cos(theta1)));
+            //var phi2 = atan2(m01/cos(theta2), (m00/cos(theta2)))
+            // two possible solutions
+            var e1 = new eulerangles_1.EulerAngles(phi1, theta1, psi1, misc_1.Seq.ZYX); // convention ZYX
+            // var e2 = new EulerAngles(phi2, theta2, psi2, "ZYX")
+            return e1;
+        }
+        else {
+            var phi = 0; // anything
+            if (m02 == -1) {
+                var theta = mathjs_1.pi / 2;
+                var psi = phi + mathjs_1.atan2(m01, m02);
+            }
+            else {
+                var theta = -mathjs_1.pi / 2;
+                var psi = phi + mathjs_1.atan2(-m01, -20);
+            }
+            return new eulerangles_1.EulerAngles(phi, theta, psi, misc_1.Seq.ZYX);
+        }
+    }
+    // ToQuat returns the Quaternion representation of the rotation
+    ToQuat() {
+        var rotm = this.elements;
+        var m00 = rotm[0][0];
+        var m10 = rotm[1][0];
+        var m20 = rotm[2][0];
+        var m01 = rotm[0][1];
+        var m11 = rotm[1][1];
+        var m21 = rotm[2][1];
+        var m02 = rotm[0][2];
+        var m12 = rotm[1][2];
+        var m22 = rotm[2][2];
+        var t;
+        var quat;
+        if (m22 < 0) {
+            if (m00 > m11) {
+                t = 1 + m00 - m11 - m22;
+                quat = [t, m10 + m01, m02 + m20, m21 - m12];
+            }
+            else {
+                t = 1 - m00 + m11 - m22;
+                quat = [m10 + m01, t, m21 + m12, m02 - m20];
+            }
+        }
+        else {
+            if (m00 < -m11) {
+                t = 1 - m00 - m11 + m22;
+                quat = [m02 + m20, m21 + m12, t, m10 - m01];
+            }
+            else {
+                t = 1 + m00 + m11 + m22;
+                quat = [m21 - m12, m02 - m20, m10 - m01, t];
+            }
+        }
+        var x = quat[0];
+        var y = quat[1];
+        var z = quat[2];
+        var w = quat[3];
+        x *= 0.5 / Math.sqrt(t);
+        y *= 0.5 / Math.sqrt(t);
+        z *= 0.5 / Math.sqrt(t);
+        w *= 0.5 / Math.sqrt(t);
+        var result = new quaternion_1.Quaternion(x, y, z, w);
+        return result;
+    }
+    // ToAxAng returns the AxisAngle representation of the rotation
+    ToAxAng() {
+        var m = this.elements;
+        // Credit: https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/index.htm
+        var angle, x, y, z; // variables for result
+        var epsilon = 0.01; // margin to allow for rounding errors
+        var epsilon2 = 0.1; // margin to distinguish between 0 and 180 degrees
+        if ((Math.abs(m[0][1] - m[1][0]) < epsilon)
+            && (Math.abs(m[0][2] - m[2][0]) < epsilon)
+            && (Math.abs(m[1][2] - m[2][1]) < epsilon)) {
+            // singularity found
+            // first check for identity matrix which must have +1 for all terms
+            //  in leading diagonaland zero in other terms
+            if ((Math.abs(m[0][1] + m[1][0]) < epsilon2)
+                && (Math.abs(m[0][2] + m[2][0]) < epsilon2)
+                && (Math.abs(m[1][2] + m[2][1]) < epsilon2)
+                && (Math.abs(m[0][0] + m[1][1] + m[2][2] - 3) < epsilon2)) {
+                // this singularity is identity matrix so angle = 0
+                return new axisangle_1.AxisAngle(new positionvector_1.PositionVector([[1], [0], [0]]), 0); // zero angle, arbitrary axis
+            }
+            // otherwise this singularity is angle = 180
+            angle = mathjs_1.pi;
+            var xx = (m[0][0] + 1) / 2;
+            var yy = (m[1][1] + 1) / 2;
+            var zz = (m[2][2] + 1) / 2;
+            var xy = (m[0][1] + m[1][0]) / 4;
+            var xz = (m[0][2] + m[2][0]) / 4;
+            var yz = (m[1][2] + m[2][1]) / 4;
+            if ((xx > yy) && (xx > zz)) { // m[0][0] is the largest diagonal term
+                if (xx < epsilon) {
+                    x = 0;
+                    y = 0.70710678118;
+                    z = 0.70710678118;
+                }
+                else {
+                    x = xx ** 0.5;
+                    y = xy / x;
+                    z = xz / x;
+                }
+            }
+            else if (yy > zz) { // m[1][1] is the largest diagonal term
+                if (yy < epsilon) {
+                    x = 0.70710678118;
+                    y = 0;
+                    z = 0.70710678118;
+                }
+                else {
+                    y = yy ** 0.5;
+                    x = xy / y;
+                    z = yz / y;
+                }
+            }
+            else { // m[2][2] is the largest diagonal term so base result on this
+                if (zz < epsilon) {
+                    x = 0.70710678118;
+                    y = 0.70710678118;
+                    z = 0;
+                }
+                else {
+                    z = zz ** 0.5;
+                    x = xz / z;
+                    y = yz / z;
+                }
+            }
+            return new axisangle_1.AxisAngle(new positionvector_1.PositionVector([[x], [y], [z]]), angle); // return 180 deg rotation
+        }
+        // as we have reached here there are no singularities so we can handle normally
+        var s = ((m[2][1] - m[1][2]) * (m[2][1] - m[1][2])
+            + (m[0][2] - m[2][0]) * (m[0][2] - m[2][0])
+            + (m[1][0] - m[0][1]) * (m[1][0] - m[0][1])) ** 0.5; // used to normalise
+        if (mathjs_1.abs(s) < 0.001)
+            s = 1;
+        // prevent divide by zero, should not happen if matrix is orthogonal and should be
+        // caught by singularity test above, but I've left it in just in case
+        angle = mathjs_1.acos((m[0][0] + m[1][1] + m[2][2] - 1) / 2);
+        x = (m[2][1] - m[1][2]) / s;
+        y = (m[0][2] - m[2][0]) / s;
+        z = (m[1][0] - m[0][1]) / s;
+        return new axisangle_1.AxisAngle(new positionvector_1.PositionVector([[x], [y], [z]]), angle); // return 180 deg rotation
+    }
+    // ToFrame returns the rotation relative to the new specified frame
+    ToFrame(tree, thisID, thatID) {
+        var frame1 = tree.getFrame(thisID);
+        var frame2 = tree.getFrame(thatID);
+        var transformationMatrix = frame1.ToFrame(tree, frame2.ID());
+        var e = transformationMatrix.getRotation().Multiply(this);
+        if (e instanceof RotationMatrix) {
+            return new RotationMatrix(e.elements);
+        }
+        else {
+            throw "Error occurred";
+        }
+    }
+}
+exports.RotationMatrix = RotationMatrix;

package/utils/transformationmatrix.d.ts

@@ -0,0 +1,12 @@
+import { FrameID } from './misc';
+import { LocalTransformTree } from './localtransformtree';
+import { PositionVector } from './positionvector';
+import { RotationMatrix } from './rotationmatrix';
+export declare class TransformationMatrix {
+    elements: number[][];
+    constructor(elements: number[][]);
+    getRotation(): RotationMatrix;
+    getPosition(): PositionVector;
+    Multiply(mB: TransformationMatrix | RotationMatrix | PositionVector): TransformationMatrix | RotationMatrix | PositionVector;
+    ToFrame(tree: LocalTransformTree, originalFrameID: FrameID, resultFrameID: FrameID): TransformationMatrix;
+}