@dronedeploy/rocos-js-sdk 2.6.1 → 3.0.0-alpha.20

package/utils/rotationmatrix.js

@@ -1,262 +0,0 @@
-"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

@@ -1,12 +0,0 @@
-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;
-}

package/utils/transformationmatrix.js

@@ -1,123 +0,0 @@
-"use strict";
-Object.defineProperty(exports, "__esModule", { value: true });
-exports.TransformationMatrix = void 0;
-const positionvector_1 = require("./positionvector");
-const rotationmatrix_1 = require("./rotationmatrix");
-const helperfunctions_1 = require("./helperfunctions");
-// TransformationMatrix is 4x4 transformation matrix
-class TransformationMatrix {
-    constructor(elements) {
-        this.elements = elements;
-    }
-    // getRotation returns the local transform's rotation matrix
-    getRotation() {
-        var e = this.elements;
-        return new rotationmatrix_1.RotationMatrix([[e[0][0], e[0][1], e[0][2]],
-            [e[1][0], e[1][1], e[1][2]],
-            [e[2][0], e[2][1], e[2][2]]]);
-    }
-    // getPosition returns the local transform's position vector
-    getPosition() {
-        var e = this.elements;
-        return new positionvector_1.PositionVector([[e[0][3]], [e[1][3]], [e[2][3]]]);
-    }
-    // Multiply multipies with a given matrix
-    Multiply(mB) {
-        var matrixA = this.elements;
-        if (mB instanceof positionvector_1.PositionVector) {
-            var e = mB.elements;
-            var matrixB = [[e[0][0]], [e[1][0]], [e[2][0]], [1]];
-        }
-        else if (mB instanceof rotationmatrix_1.RotationMatrix) {
-            var ele = mB.elements;
-            var matrixB = [
-                [ele[0][0], ele[0][1], ele[0][2]],
-                [ele[1][0], ele[1][1], ele[1][2]],
-                [ele[2][0], ele[2][1], ele[2][2]],
-                [0, 0, 1]
-            ];
-        }
-        else if (mB instanceof TransformationMatrix) {
-            var matrixB = mB.elements;
-        }
-        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 == 4 && cols == 4) {
-            return new TransformationMatrix(m);
-        }
-        else if (rows == 4 && 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_1.RotationMatrix(result);
-        }
-        else if (rows == 4 && cols == 1) {
-            return new positionvector_1.PositionVector([[m[0][0]], [m[1][0]], [m[2][0]]]);
-        }
-        else {
-            throw "Error occurred";
-        }
-    }
-    // ToFrame returns the transformation matrix relative to the new specified frame
-    ToFrame(tree, originalFrameID, resultFrameID) {
-        if (originalFrameID == resultFrameID) {
-            return this;
-        }
-        else if (tree.isDescendant(originalFrameID, resultFrameID)) {
-            var trans = helperfunctions_1.frameToFrameTransformationMatrixSeq(tree, tree.getSeqDown(originalFrameID, resultFrameID), false);
-            var totalTM = trans.Multiply(this);
-            if (totalTM instanceof TransformationMatrix) {
-                return totalTM;
-            }
-            else {
-                throw "Error occurred";
-            }
-        }
-        else if (tree.isAncestor(originalFrameID, resultFrameID)) {
-            var trans = helperfunctions_1.frameToFrameTransformationMatrixSeq(tree, tree.getSeqDown(resultFrameID, originalFrameID), true);
-            var totalTM = trans.Multiply(this);
-            if (totalTM instanceof TransformationMatrix) {
-                return totalTM;
-            }
-            else {
-                throw "Error occurred";
-            }
-        }
-        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) {
-                // Apply the transformation
-                var total = totalTM.Multiply(this);
-                if (total instanceof TransformationMatrix) {
-                    return total;
-                }
-                else {
-                    throw "Erorr occurred";
-                }
-            }
-            else {
-                throw "Error occurred";
-            }
-        }
-    }
-}
-exports.TransformationMatrix = TransformationMatrix;

package/utils/vector3.d.ts

@@ -1,11 +0,0 @@
-import { PositionVector } from './positionvector';
-export declare class Vector3 {
-    elements: number[][];
-    constructor(elements: number[][]);
-    X(): number;
-    Y(): number;
-    Z(): number;
-    magnitude(): number;
-    unitVector(): Vector3;
-    Multiply(mB: Vector3 | PositionVector): Vector3 | PositionVector;
-}

package/utils/vector3.js

@@ -1,61 +0,0 @@
-"use strict";
-Object.defineProperty(exports, "__esModule", { value: true });
-exports.Vector3 = void 0;
-const positionvector_1 = require("./positionvector");
-// Vector3 is a 3X1 matrix that contains a vector
-class Vector3 {
-    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 Vector3([[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_1.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 && mB instanceof positionvector_1.PositionVector) {
-            return new positionvector_1.PositionVector([[m[0][0]], [m[1][0]], [m[2][0]]]);
-        }
-        else if (rows == 3 && cols == 1 && mB instanceof Vector3) {
-            return new positionvector_1.PositionVector([[m[0][0]], [m[1][0]], [m[2][0]]]);
-        }
-        else {
-            throw "Error occurred";
-        }
-    }
-}
-exports.Vector3 = Vector3;