com.neuronrobotics.sdk.addons.kinematics.math.TransformNR Maven / Gradle / Ivy

Go to download
Show more of this group Show more artifacts with this name
Show all versions of java-bowler Show documentation
A command line utility for accesing the bowler framework.
There is a newer version: 3.28.0
package com.neuronrobotics.sdk.addons.kinematics.math;

import java.math.BigDecimal;
import java.text.DecimalFormat;
import java.util.ArrayList;

import com.neuronrobotics.sdk.addons.kinematics.DHLink;
import com.neuronrobotics.sdk.common.Log;

import Jama.Matrix;

// TODO: Auto-generated Javadoc
/**
 * The Class TransformNR.
 */
public class TransformNR {
	
	/** The x. */
	private double x;
	
	/** The y. */
	private double y;
	
	/** The z. */
	private double z;
	
	/** The rotation. */
	private  RotationNR rotation;
	
	
	
	/**
	 * Instantiates a new transform nr.
	 *
	 * @param m the m
	 */
	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));
	}
	
	/**
	 * Instantiates a new transform nr.
	 *
	 * @param x the x
	 * @param y the y
	 * @param z the z
	 * @param w the w
	 * @param rotx the rotx
	 * @param roty the roty
	 * @param rotz the rotz
	 */
	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}));
	}
	
	/**
	 * Instantiates a new transform nr.
	 *
	 * @param cartesianSpaceVector the cartesian space vector
	 * @param rotationMatrix the rotation matrix
	 */
	public TransformNR(double[] cartesianSpaceVector, double[][] rotationMatrix) {
		this.setX(cartesianSpaceVector[0]);
		this.setY(cartesianSpaceVector[1]);
		this.setZ(cartesianSpaceVector[2]);
		this.setRotation(new RotationNR(rotationMatrix));
	}
	
	/**
	 * Instantiates a new transform nr.
	 *
	 * @param cartesianSpaceVector the cartesian space vector
	 * @param quaternionVector the quaternion vector
	 */
	public TransformNR(double[] cartesianSpaceVector, double[] quaternionVector) {
		this.setX(cartesianSpaceVector[0]);
		this.setY(cartesianSpaceVector[1]);
		this.setZ(cartesianSpaceVector[2]);
		this.setRotation(new RotationNR(quaternionVector));
	}
	
	/**
	 * Instantiates a new transform nr.
	 *
	 * @param x the x
	 * @param y the y
	 * @param z the z
	 * @param q the q
	 */
	public TransformNR(double x, double y, double z, RotationNR q){
		this.setX(x);
		this.setY(y);
		this.setZ(z);
		this.setRotation(q);
	}
	
	/**
	 * Instantiates a new transform nr.
	 *
	 * @param cartesianSpaceVector the cartesian space vector
	 * @param q the q
	 */
	public TransformNR(double[] cartesianSpaceVector, RotationNR q) {
		this.setX(cartesianSpaceVector[0]);
		this.setY(cartesianSpaceVector[1]);
		this.setZ(cartesianSpaceVector[2]);
		this.setRotation(q);
	}
	
	/**
	 * Instantiates a new transform nr.
	 */
	public TransformNR() {
		this.setX(0);
		this.setY(0);
		this.setZ(0);
		this.setRotation(new RotationNR());
	}
	
	/**
	 * Gets the x.
	 *
	 * @return the x
	 */
	public double getX() {
		return x;
	}
	
	/**
	 * Gets the y.
	 *
	 * @return the y
	 */
	public double getY() {
		return y;
	}
	
	/**
	 * Gets the z.
	 *
	 * @return the z
	 */
	public double getZ() {
		return z;
	}
	
	/**
	 * Gets the rotation matrix array.
	 *
	 * @return the rotation matrix array
	 */
	public double [][] getRotationMatrixArray(){
		return getRotation().getRotationMatrix();
	}

	/**
	 * Gets the rotation matrix.
	 *
	 * @return the rotation matrix
	 */
	public RotationNR getRotationMatrix() {
		return getRotation();
	}

	/**
	 * Gets the rotation value.
	 *
	 * @param i the i
	 * @param j the j
	 * @return the rotation value
	 */
	public double getRotationValue(int i,int j) {
		return getRotation().getRotationMatrix()[i][j];
	}
	
	/**
	 * Gets the rotation.
	 *
	 * @return the rotation
	 */
	public RotationNR getRotation() {

		return rotation;
	}
	
	/**
	 * Times.
	 *
	 * @param t the t
	 * @return the transform nr
	 */
	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();
		}
	}
	
	/**
	 * Gets the matrix string.
	 *
	 * @param matrix the matrix
	 * @return the matrix string
	 */
	public static String getMatrixString(Matrix matrix){
		if(!Log.isPrinting()){
			return "no print transform, enable Log.enableSystemPrint(true)";
		}
		String s = "{\n";
		double [][] m = matrix.getArray();
		
		int across = m.length;
		int down = m[0].length;
		
		for(int i=0;i\n"+
						"\t"+getY()+"\n"+
						"\t"+getZ()+"\n";
		if(		Double.isNaN(getRotation().getRotationMatrix2QuaturnionW())||
				Double.isNaN(getRotation().getRotationMatrix2QuaturnionX())||
				Double.isNaN(getRotation().getRotationMatrix2QuaturnionY())||
				Double.isNaN(getRotation().getRotationMatrix2QuaturnionZ())
				){
			xml +="\n\t\n";
			setRotation(new RotationNR());
		}
		xml +="\t"+getRotation().getRotationMatrix2QuaturnionW()+"\n"+
		"\t"+getRotation().getRotationMatrix2QuaturnionX()+"\n"+
		"\t"+getRotation().getRotationMatrix2QuaturnionY()+"\n"+
		"\t"+getRotation().getRotationMatrix2QuaturnionZ()+"";

		return xml;
	}

	/**
	 * Sets the rotation.
	 *
	 * @param rotation the new rotation
	 */
	public void setRotation(RotationNR rotation) {
		this.rotation = rotation;
	}

	

}