TransformNR.java

Go to the documentation of this file.

package com.neuronrobotics.sdk.addons.kinematics.math;
 
import java.math.BigDecimal;
import java.text.DecimalFormat;
import java.util.ArrayList;
import com.neuronrobotics.sdk.common.Log;
import Jama.Matrix;
 
// TODO: Auto-generated Javadoc
public class TransformNR {
  private ArrayList<ITransformNRChangeListener> listeners=null;
  private double x;
 
  private double y;
 
  private double z;
 
  private RotationNR rotation;
 
 
 
  public TransformNR(Matrix m) {
    this.setX(m.get(0, 3));
    this.setY(m.get(1, 3));
    this.setZ(m.get(2, 3));
    this.setRotation(new RotationNR(m));
  }
 
  public TransformNR(double x, double y, double z, double w, double rotx, double roty,
      double rotz) {
    this.setX(x);
    this.setY(y);
    this.setZ(z);
    this.setRotation(new RotationNR(new double[] {w, rotx, roty, rotz}));
  }
 
  public TransformNR(double[] cartesianSpaceVector, double[][] rotationMatrix) {
    this.setX(cartesianSpaceVector[0]);
    this.setY(cartesianSpaceVector[1]);
    this.setZ(cartesianSpaceVector[2]);
    this.setRotation(new RotationNR(rotationMatrix));
  }
 
  public TransformNR(double[] cartesianSpaceVector, double[] quaternionVector) {
    this.setX(cartesianSpaceVector[0]);
    this.setY(cartesianSpaceVector[1]);
    this.setZ(cartesianSpaceVector[2]);
    this.setRotation(new RotationNR(quaternionVector));
  }
 
  public TransformNR(double x, double y, double z, RotationNR q) {
    this.setX(x);
    this.setY(y);
    this.setZ(z);
    this.setRotation(q);
  }
 
  public TransformNR(double[] cartesianSpaceVector, RotationNR q) {
    this.setX(cartesianSpaceVector[0]);
    this.setY(cartesianSpaceVector[1]);
    this.setZ(cartesianSpaceVector[2]);
    this.setRotation(q);
  }
 
  public TransformNR() {
    this.setX(0);
    this.setY(0);
    this.setZ(0);
    this.setRotation(new RotationNR());
  }
 
  public double getX() {
    return x;
  }
 
  public double getY() {
    return y;
  }
 
  public double getZ() {
    return z;
  }
 
  public double[][] getRotationMatrixArray() {
    return getRotation().getRotationMatrix();
  }
 
  public RotationNR getRotationMatrix() {
    return getRotation();
  }
 
  public double getRotationValue(int i, int j) {
    return getRotation().getRotationMatrix()[i][j];
  }
 
  public RotationNR getRotation() {
 
    return rotation;
  }
 
  public TransformNR times(TransformNR t) {
    return new TransformNR(getMatrixTransform().times(t.getMatrixTransform()));
  }
 
  /*
   * (non-Javadoc)
   * 
   * @see java.lang.Object#toString()
   */
  @Override
  public String toString() {
    try {
      return getMatrixString(getMatrixTransform()) + getRotation().toString();
    } catch (Exception ex) {
      return "Transform error" + ex.getLocalizedMessage();
    }
  }
 
  public static String getMatrixString(Matrix matrix) {
 
    String s = "{\n";
    double[][] m = matrix.getArray();
 
    DecimalFormat decimalFormat = new DecimalFormat("000.00");
    int across = m.length;
    int down = m[0].length;
 
    for (int i = 0; i < across; i++) {
      s += "{ ";
      for (int j = 0; j < down; j++) {
        if (m[i][j] < 0) {
            s += decimalFormat.format(m[i][j]);
        } else
          s += decimalFormat.format(m[i][j]);
        if (j < down - 1)
          s += ",";
        s += "\t";
      }
      s += " }";
      if (i < across - 1)
        s += ",";
      s += "\n";
    }
    return s + "}\n";
  }
 
  public double[] getPositionArray() {
    return new double[] {getX(), getY(), getZ()};
  }
 
  public Matrix getMatrixTransform() {
    double[][] transform = new double[4][4];
    double[][] rotation = getRotationMatrixArray();
 
 
    for (int i = 0; i < 3; i++) {
      for (int j = 0; j < 3; j++) {
        transform[i][j] = rotation[i][j];
      }
    }
    for (int i = 0; i < 3; i++) {
      transform[3][i] = 0;
    }
    transform[3][3] = 1;
    transform[0][3] = getX();
    transform[1][3] = getY();
    transform[2][3] = getZ();
 
 
    return new Matrix(transform);
  }
 
  public double getOffsetOrentationMagnitude(TransformNR t) {
    double x = getRotation().getRotationMatrix2QuaturnionX()
        - t.getRotation().getRotationMatrix2QuaturnionX();
    double y = getRotation().getRotationMatrix2QuaturnionY()
        - t.getRotation().getRotationMatrix2QuaturnionY();
    double z = getRotation().getRotationMatrix2QuaturnionZ()
        - t.getRotation().getRotationMatrix2QuaturnionZ();
    double r = Math.sqrt((Math.pow(x, 2) + Math.pow(y, 2) + Math.pow(z, 2)));
    return r;
  }
 
  public double getOffsetVectorMagnitude(TransformNR t) {
    double x = getX() - t.getX();
    double y = getY() - t.getY();
    double z = getZ() - t.getZ();
    double r = Math.sqrt((Math.pow(x, 2) + Math.pow(y, 2) + Math.pow(z, 2)));
    return r;
  }
 
  public TransformNR inverse() {
    return new TransformNR(getMatrixTransform().inverse());
  }
 
  public TransformNR scale(BigDecimal scale) {
    return scale(scale.doubleValue());
  }
 
  public TransformNR scale(double t) {
    if (t > 1)
      t = 1;
    if (t <= 0)
      return new TransformNR();
 
    double tilt = Math.toDegrees(getRotation().getRotationTilt() * t);
    double az = Math.toDegrees(getRotation().getRotationAzimuth() * t);
    double ele = Math.toDegrees(getRotation().getRotationElevation() * t);
    return new TransformNR(getX() * t, getY() * t, getZ() * t, new RotationNR(tilt, az, ele));
  }
 
  public TransformNR copy() {
    return new TransformNR(getMatrixTransform());
  }
 
  public TransformNR translateX(double translation) {
    setX(getX() + translation);
    return this;
  }
 
  public TransformNR translateY(double translation) {
    setY(getY() + translation);
    return this;
 
  }
 
  public TransformNR translateZ(double translation) {
 
    setZ(getZ() + translation);
    return this;
  }
  
    public TransformNR set(double tx, double ty, double tz, double[][] poseRot) {
        if (Double.isNaN(tx))
            throw new RuntimeException("Value can not be NaN");
        x = tx;
        if (Double.isNaN(ty))
            throw new RuntimeException("Value can not be NaN");
        y = ty;
        if (Double.isNaN(tz))
            throw new RuntimeException("Value can not be NaN");
        z = tz;
        getRotation().set(poseRot);
        fireChangeEvent();
        return this;
    }
 
  public TransformNR setX(double tx) {
    if (Double.isNaN(tx))
      throw new RuntimeException("Value can not be NaN");
    x = tx;
    fireChangeEvent();
    return this;
  }
 
  public TransformNR setY(double ty) {
    if (Double.isNaN(ty))
      throw new RuntimeException("Value can not be NaN");
    y = ty;
    fireChangeEvent();
    return this;
  }
 
  public TransformNR setZ(double tz) {
    if (Double.isNaN(tz))
      throw new RuntimeException("Value can not be NaN");
    z = tz;
    fireChangeEvent();
    return this;
  }
 
  /*
   * 
   * Generate the xml configuration to generate an XML of this robot.
   */
  public String getXml() {
    String xml =
        "\t<x>" + getX() + "</x>\n" + "\t<y>" + getY() + "</y>\n" + "\t<z>" + getZ() + "</z>\n";
    if (Double.isNaN(getRotation().getRotationMatrix2QuaturnionW())
        || Double.isNaN(getRotation().getRotationMatrix2QuaturnionX())
        || Double.isNaN(getRotation().getRotationMatrix2QuaturnionY())
        || Double.isNaN(getRotation().getRotationMatrix2QuaturnionZ())) {
      xml += "\n\t<!-- ERROR a NaN was detected and replaced with a valid rotation -->\n";
      setRotation(new RotationNR());
    }
    xml += "\t<rotw>" + getRotation().getRotationMatrix2QuaturnionW() + "</rotw>\n" + "\t<rotx>"
        + getRotation().getRotationMatrix2QuaturnionX() + "</rotx>\n" + "\t<roty>"
        + getRotation().getRotationMatrix2QuaturnionY() + "</roty>\n" + "\t<rotz>"
        + getRotation().getRotationMatrix2QuaturnionZ() + "</rotz>";
 
    return xml;
  }
 
  public TransformNR setRotation(RotationNR rotation) {
    this.rotation = rotation;
    fireChangeEvent();
    return this;
  }
 
 
    public void addChangeListener(ITransformNRChangeListener l) {
        if(!getListeners().contains(l))
            getListeners().add(l);
    }
    public void removeChangeListener(ITransformNRChangeListener l) {
        if(getListeners().contains(l))
            getListeners().remove(l);
    }
    public void clearChangeListener() {
        getListeners().clear();
        listeners=null;
    }
    public ArrayList<ITransformNRChangeListener> getListeners() {
        if(listeners==null)
            listeners=new ArrayList<ITransformNRChangeListener>();
        return listeners;
    }
 
    void fireChangeEvent() {
        if(listeners!=null) {
            for(int i=0;i<listeners.size();i++) {
                try {
                    listeners.get(i).event(this);
                }catch (Throwable t) {
                    t.printStackTrace();
                }
            }
        }
    }
 
    public void setTiltDegrees(double newAngleDegrees) {
        double  e=0;
        try{
            e=Math.toDegrees(getRotation().getRotationElevation());
        }catch(Exception ex){
            ex.printStackTrace();
        }
        double  a=0;
        try{
            a=Math.toDegrees(getRotation().getRotationAzimuth());
        }catch(Exception ex){
            ex.printStackTrace();
        }
        
        setRotation(new RotationNR(newAngleDegrees, a, e));
        
        
    }
 
    public void setElevationDegrees(double newAngleDegrees) {
        double  t=0;
        try{
            t=Math.toDegrees(getRotation().getRotationTilt());
        }catch(Exception ex){
            ex.printStackTrace();
        }
        
        double  a=0;
        try{
            a=Math.toDegrees(getRotation().getRotationAzimuth());
        }catch(Exception ex){
            ex.printStackTrace();
        }
        setRotation(new RotationNR(t, a, newAngleDegrees));
    }
 
    public void setAzimuthDegrees(double newAngleDegrees) {
        double  t=0;
        try{
            t=Math.toDegrees(getRotation().getRotationTilt());
        }catch(Exception ex){
            ex.printStackTrace();
        }
            
        double  e=0;
        try{
            e=Math.toDegrees(getRotation().getRotationElevation());
        }catch(Exception ex){
            ex.printStackTrace();
        }
        setRotation(new RotationNR(t, newAngleDegrees, e));
    }
}