RotationNR.java

Go to the documentation of this file.

package com.neuronrobotics.sdk.addons.kinematics.math;
 
import Jama.Matrix;
 
import org.apache.commons.math3.geometry.euclidean.threed.CardanEulerSingularityException;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
 
import com.neuronrobotics.sdk.common.Log;
 
// TODO: Auto-generated Javadoc
public class RotationNR {
 
    // double[][] rotationMatrix = ;
    private Rotation storage = new Rotation(1, 0, 0, 0, false);
    private static RotationOrder order = RotationOrder.ZYX;
    private static RotationConvention convention = RotationConvention.VECTOR_OPERATOR;
    
 
    public RotationNR() {
    }
 
    public RotationNR(Rotation store) {
        storage = store;
    }
 
    // create a new object with the given simplified rotations
    public RotationNR(double tilt, double azumeth, double elevation) {
        if (Double.isNaN(tilt))
            throw new RuntimeException("Value can not be NaN");
        if (Double.isNaN(azumeth))
            throw new RuntimeException("Value can not be NaN");
        if (Double.isNaN(elevation))
            throw new RuntimeException("Value can not be NaN");
        if (elevation > 90 || elevation < -90) {
            throw new RuntimeException("Elevation can not be greater than 90 nor less than -90");
        }
        loadFromAngles(tilt, azumeth, elevation);
        if (Double.isNaN(getRotationMatrix2QuaturnionW()) || Double.isNaN(getRotationMatrix2QuaturnionX())
                || Double.isNaN(getRotationMatrix2QuaturnionY()) || Double.isNaN(getRotationMatrix2QuaturnionZ())) {
            Log.error("Failing to set proper angle, jittering");
            loadFromAngles(tilt + Math.random() * .02 + .001, azumeth + Math.random() * .02 + .001,
                    elevation + Math.random() * .02 + .001);
        }
 
    }
 
    public RotationNR(double[][] rotationMatrix) {
        loadRotations(rotationMatrix);
    }
 
    public RotationNR(double[] values) {
        this(values[0], values[1], values[2], values[3]);
    }
 
    public static RotationNR getRotationX(double rotationAngleDegrees) {
        double[][] rotation = new double[3][3];
        double rotationAngleRadians = Math.PI / 180 * rotationAngleDegrees;
 
        // Rotation matrix, 1st column
        rotation[0][0] = 1;
        rotation[1][0] = 0;
        rotation[2][0] = 0;
        // Rotation matrix, 2nd column
        rotation[0][1] = 0;
        rotation[1][1] = Math.cos(rotationAngleRadians);
        rotation[2][1] = Math.sin(rotationAngleRadians);
        // Rotation matrix, 3rd column
        rotation[0][2] = 0;
        rotation[1][2] = -Math.sin(rotationAngleRadians);
        rotation[2][2] = Math.cos(rotationAngleRadians);
 
        return new RotationNR(rotation);
    }
 
    public static RotationNR getRotationY(double rotationAngleDegrees) {
        double[][] rotation = new double[3][3];
        double rotationAngleRadians = Math.PI / 180 * rotationAngleDegrees;
 
        // Rotation matrix, 1st column
        rotation[0][0] = Math.cos(rotationAngleRadians);
        rotation[1][0] = 0;
        rotation[2][0] = -Math.sin(rotationAngleRadians);
        // Rotation matrix, 2nd column
        rotation[0][1] = 0;
        rotation[1][1] = 1;
        rotation[2][1] = 0;
        // Rotation matrix, 3rd column
        rotation[0][2] = Math.sin(rotationAngleRadians);
        rotation[1][2] = 0;
        rotation[2][2] = Math.cos(rotationAngleRadians);
 
        return new RotationNR(rotation);
    }
 
    public static RotationNR getRotationZ(double rotationAngleDegrees) {
        double[][] rotation = new double[3][3];
        double rotationAngleRadians = Math.PI / 180 * rotationAngleDegrees;
 
        // Rotation matrix, 1st column
        rotation[0][0] = Math.cos(rotationAngleRadians);
        rotation[1][0] = Math.sin(rotationAngleRadians);
        rotation[2][0] = 0;
        // Rotation matrix, 2nd column
        rotation[0][1] = -Math.sin(rotationAngleRadians);
        rotation[1][1] = Math.cos(rotationAngleRadians);
        rotation[2][1] = 0;
        // Rotation matrix, 3rd column
        rotation[0][2] = 0;
        rotation[1][2] = 0;
        rotation[2][2] = 1;
 
        return new RotationNR(rotation);
    }
 
    // create a new object with the given components
    public RotationNR(double w, double x, double y, double z) {
        quaternion2RotationMatrix(w, x, y, z);
    }
 
    public RotationNR(Matrix m) {
        double[][] rotation = new double[3][3];
        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++) {
                rotation[i][j] = m.get(i, j);
            }
        }
        loadRotations(rotation);
    }
 
    private void loadRotations(double[][] rotM) {
        if (rotM.length != 3)
            throw new RuntimeException("Must be 3x3 rotation matrix");
        for (int i = 0; i < 3; i++) {
            if (rotM[i].length != 3) {
                throw new RuntimeException("Must be 3x3 rotation matrix");
            }
        }
        setStorage(new Rotation(rotM, 0.00001));
    }
 
    public double[][] getRotationMatrix() {
 
        return getStorage().getMatrix();
    }
 
    /*
     * (non-Javadoc)
     * 
     * @see java.lang.Object#toString()
     */
    // return a string representation of the invoking object
    public String toString() {
        try{
            return "Quaturnion: " + "W=" + getRotationMatrix2QuaturnionW() + ", " + "x=" + getRotationMatrix2QuaturnionX()
                + ", " + "y=" + getRotationMatrix2QuaturnionY() + ", " + "z=" + getRotationMatrix2QuaturnionZ() + "\n"
                + "Rotation angle (degrees): " + "az= " + Math.toDegrees(getRotationAzimuth()) + ", elev= "
                + Math.toDegrees(getRotationElevation()) + ", tilt=" + Math.toDegrees(getRotationTilt());
        }catch(Exception ex){
            return "Rotation error"+ex.getLocalizedMessage();
        }
        
    }
 
    // return a string representation of the invoking object
    public String toString(double[][] array) {
        String s = "[\n";
        for (int i = 0; i < 3; i++) {
            s += "[ ";
            for (int j = 0; j < 3; j++) {
                s += array[i][j] + "\t\t";
            }
            s += " ]\n";
        }
        s += "]";
        return "Matrix = " + s;
    }
 
    protected void quaternion2RotationMatrix(double w, double x, double y, double z) {
        if (Double.isNaN(w))
            throw new RuntimeException("Value can not be NaN");
        if (Double.isNaN(x))
            throw new RuntimeException("Value can not be NaN");
        if (Double.isNaN(y))
            throw new RuntimeException("Value can not be NaN");
        if (Double.isNaN(z))
            throw new RuntimeException("Value can not be NaN");
        setStorage(new Rotation(w,- x, -y, -z, true));
    }
 
    public static boolean bound(double low, double high, double n) {
        return n >= low && n <= high;
    }
 
    private void loadFromAngles(double tilt, double azumeth, double elevation) {
        setStorage(new Rotation(getOrder(), getConvention(), Math.toRadians(azumeth), Math.toRadians(elevation),
                Math.toRadians(tilt)));
    }
 
    public double getRotationTilt() {
        return getAngle(2);
    }
 
    public double getRotationElevation() {
        return getAngle(1);
 
    }
 
    public double getRotationAzimuth() {
        return getAngle(0);
    }
    private void simpilfyAngles(double [] angles){
        double epsilon=1.0E-7;
        if(Math.abs(angles[0] - Math.toRadians(180)) < epsilon&&
           Math.abs(angles[2] - Math.toRadians(180)) < epsilon  ){
            if(!(Math.abs(getRotationMatrix2QuaturnionZ())>epsilon))
                angles[0]=0;
            if(!(Math.abs(getRotationMatrix2QuaturnionX())>epsilon))
                angles[2]=0;
        }
    }
    private double eulerFix(double offsetSize, int index){
        double offset = (index==1?offsetSize:0);
        TransformNR current = new TransformNR(0, 0, 0, this);
        TransformNR newTf = current.times(new TransformNR(0, 0, 0, new RotationNR(0,0,Math.toDegrees(offsetSize))));
        double[] angles = newTf.getRotation().getStorage().getAngles(getOrder(), getConvention());
        simpilfyAngles(angles);
        double finalResult= angles[index];
        return finalResult+offset;
    }
    private double getAngle(int index){
        
        try {
            return getStorage().getAngles(getOrder(), getConvention())[index];
        } catch (CardanEulerSingularityException e) {
            try {
                return eulerFix( Math.toRadians(5),  index);
            } catch (CardanEulerSingularityException ex) {
                return eulerFix(  Math.toRadians(-5),  index);
    
            }
        }
    }
     
 
    public double getRotationMatrix2QuaturnionW() {
        return getStorage().getQ0();
    }
 
    public double getRotationMatrix2QuaturnionX() {
        return -getStorage().getQ1();
    }
 
    public double getRotationMatrix2QuaturnionY() {
        return -getStorage().getQ2();
    }
 
    public double getRotationMatrix2QuaturnionZ() {
        return -getStorage().getQ3();
    }
 
    public static RotationOrder getOrder() {
        return order;
    }
 
    public static void setOrder(RotationOrder o) {
        order = o;
    }
 
    public static RotationConvention getConvention() {
        return convention;
    }
 
    public static void setConvention(RotationConvention convention) {
        RotationNR.convention = convention;
    }
 
    public Rotation getStorage() {
        return storage;
    }
 
    public void setStorage(Rotation storage) {
        this.storage = storage;
    }
 
    public void set(double[][] poseRot) {
        loadRotations(poseRot);
    }
 
 
 
}