package edu.stanford.rsl.conrad.geometry;
import edu.stanford.rsl.conrad.numerics.SimpleMatrix;
import edu.stanford.rsl.conrad.numerics.SimpleOperators;
import edu.stanford.rsl.conrad.numerics.SimpleVector;
import edu.stanford.rsl.conrad.utils.CONRAD;
public abstract class Rotations {
public enum BasicAxis {
X_AXIS,
Y_AXIS,
Z_AXIS
}
/**
* Computes the rotation matrix from a to b.
* Note that both vectors must have the same length.
*
* @param a the Vector a
* @param b the Vector b
* @return the rotation matrix from a to b
*/
public static SimpleMatrix getRotationMatrixFromAtoB(SimpleVector a, SimpleVector b){
SimpleVector normal = General.crossProduct(a, b);
double lenA = a.normL2();
double lenB = b.normL2();
if (Math.abs(lenA - lenB) > CONRAD.FLOAT_EPSILON) {
throw new RuntimeException("Vector must have same length!");
}
double angle = Math.asin(normal.normL2() / (lenA*lenB));
SimpleMatrix rot = Rotations.createRotationMatrixAboutAxis(new Axis(normal), angle);
SimpleVector afterRotation = SimpleOperators.multiply(rot, a);
if (General.euclideanDistance(afterRotation, b) < CONRAD.FLOAT_EPSILON) {
return rot;
} else {
return Rotations.createRotationMatrixAboutAxis(new Axis(normal), -angle);
}
}
/**
* Computes the angle (in radians) of the rotation from a to b (in the plane that is defined by (0,0,0), a, b).
* Note that both vectors must have the same length.
*
* @param a the Vector a
* @param b the Vector b
* @return the rotation matrix from a to b
*/
public static double getRotationFromAtoB(SimpleVector a, SimpleVector b){
SimpleVector ab = a.clone();
ab.add(b.negated());
if (ab.normL2() < CONRAD.FLOAT_EPSILON){
return 0;
} else {
if (Math.abs(ab.normL2()-2.0) < CONRAD.FLOAT_EPSILON){
return Math.PI;
}
}
SimpleVector normal = General.crossProduct(a, b);
double lenA = a.normL2();
double lenB = b.normL2();
if (Math.abs(lenA - lenB) > CONRAD.FLOAT_EPSILON) {
throw new RuntimeException("Vector must have same length: " + lenA + " " + lenB);
}
double angle = Math.asin(normal.normL2() / (lenA*lenB));
SimpleMatrix rot = Rotations.createRotationMatrixAboutAxis(new Axis(normal), angle);
SimpleVector afterRotation = SimpleOperators.multiply(rot, a);
@SuppressWarnings("unused")
double angle1 = General.toDegrees(angle);
if (General.euclideanDistance(afterRotation, b) < CONRAD.FLOAT_EPSILON) {
return angle;
} else {
rot = Rotations.createRotationMatrixAboutAxis(new Axis(normal), -angle);
afterRotation = SimpleOperators.multiply(rot, a);
@SuppressWarnings("unused")
double angle2 = General.toDegrees(-angle);
if (General.euclideanDistance(afterRotation, b) < CONRAD.FLOAT_EPSILON) {
return -angle;
} else {
rot = Rotations.createRotationMatrixAboutAxis(new Axis(normal), Math.PI-angle);
afterRotation = SimpleOperators.multiply(rot, a);
if (General.euclideanDistance(afterRotation, b) < CONRAD.FLOAT_EPSILON) {
return Math.PI-angle;
} else {
rot = Rotations.createRotationMatrixAboutAxis(new Axis(normal), Math.PI+angle);
afterRotation = SimpleOperators.multiply(rot, a);
System.out.println("Problem");
return Double.MAX_VALUE;
}
}
}
}
public static SimpleMatrix createBasicRotationMatrix(final BasicAxis axis, double angle) {
final double s = Math.sin(angle);
final double c = Math.cos(angle);
if (axis == BasicAxis.X_AXIS) return new SimpleMatrix(new double[][] {
{1.0, 0.0, 0.0},
{0.0, c , -s },
{0.0, s , c }
});
else if (axis == BasicAxis.Y_AXIS) return new SimpleMatrix(new double[][] {
{ c , 0.0, s },
{0.0, 1.0, 0.0},
{-s , 0.0, c }
});
else if (axis == BasicAxis.Z_AXIS) return new SimpleMatrix(new double[][] {
{ c , -s , 0.0},
{ s , c , 0.0},
{0.0, 0.0, 1.0}
});
else throw new RuntimeException("Unknown axis!");
}
public static SimpleMatrix createBasicRotationMatrixDerivative(final BasicAxis axis, double angle) {
final double s = Math.sin(angle);
final double c = Math.cos(angle);
if (axis == BasicAxis.X_AXIS) return new SimpleMatrix(new double[][] {
{0.0, 0.0, 0.0},
{0.0, -s , -c },
{0.0, c , -s }
});
else if (axis == BasicAxis.Y_AXIS) return new SimpleMatrix(new double[][] {
{ -s , 0.0, c },
{0.0, 0.0, 0.0},
{-c , 0.0, -s }
});
else if (axis == BasicAxis.Z_AXIS) return new SimpleMatrix(new double[][] {
{ -s , -c , 0.0},
{ c , -s , 0.0},
{0.0, 0.0, 0.0}
});
else throw new RuntimeException("Unknown axis!");
}
/**
* Creates a rotation matrix derivative w.r.t. the given basic axis and given angles
*
* @param axis the axis to derive for
* @param angleX the angle in X
* @param angleY the angle in Y
* @param angleZ the angle in Z
* @return the matrix
*/
public static SimpleMatrix createRotationMatrixDerivative(BasicAxis axis, double angleX, double angleY, double angleZ){
if (axis == BasicAxis.X_AXIS){
SimpleMatrix xrot = createBasicRotationMatrixDerivative(axis, angleX);
SimpleMatrix xyrot = SimpleOperators.multiplyMatrixProd(xrot, createBasicYRotationMatrix(angleY));
return SimpleOperators.multiplyMatrixProd(xyrot, createBasicZRotationMatrix(angleZ));
}
else if (axis == BasicAxis.Y_AXIS){
SimpleMatrix xrot = createBasicXRotationMatrix(angleX);
SimpleMatrix xyrot = SimpleOperators.multiplyMatrixProd(xrot, createBasicRotationMatrixDerivative(axis, angleY));
return SimpleOperators.multiplyMatrixProd(xyrot, createBasicZRotationMatrix(angleZ));
}
else if (axis == BasicAxis.Z_AXIS){
SimpleMatrix xrot = createBasicXRotationMatrix(angleX);
SimpleMatrix xyrot = SimpleOperators.multiplyMatrixProd(xrot, createBasicYRotationMatrix(angleY));
return SimpleOperators.multiplyMatrixProd(xyrot, createBasicRotationMatrixDerivative(axis,angleZ));
}
else throw new RuntimeException("Unknown axis!");
}
/**
* Creates a rotation matrix as the product of
* RotationMatrixX * RotationMatrixY * RotationMatrixZ
*
* @param angleX the angle in X
* @param angleY the angle in Y
* @param angleZ the angle in Z
* @return the matrix
*/
public static SimpleMatrix createRotationMatrix(double angleX, double angleY, double angleZ){
SimpleMatrix xrot = createBasicXRotationMatrix(angleX);
SimpleMatrix xyrot = SimpleOperators.multiplyMatrixProd(xrot, createBasicYRotationMatrix(angleY));
return SimpleOperators.multiplyMatrixProd(xyrot, createBasicZRotationMatrix(angleZ));
}
public static SimpleMatrix createBasicXRotationMatrix(double angle){
return createBasicRotationMatrix(BasicAxis.X_AXIS, angle);
}
public static SimpleMatrix createBasicYRotationMatrix(double angle){
return createBasicRotationMatrix(BasicAxis.Y_AXIS, angle);
}
public static SimpleMatrix createBasicZRotationMatrix(double angle){
return createBasicRotationMatrix(BasicAxis.Z_AXIS, angle);
}
/**
*
* @param axis the direction of the axis (can be any length)
* @param angle the angle in radians.
* @return the Rotation Matrix
*/
public static SimpleMatrix createRotationMatrixAboutAxis(final SimpleVector axis, double angle){
return createRotationMatrixAboutAxis(new Axis(axis), angle);
}
/**
* Creates a Rotation Matrix about an arbitrary axis.
* @param axis Axis of Rotation
* @param angle in radians
* @return rotation matrix
*/
public static SimpleMatrix createRotationMatrixAboutAxis(Axis axis, double angle){
final SimpleVector axisVec = axis.getAxisVector();
assert (Math.abs(axisVec.normL2() - 1.0) < Math.sqrt(CONRAD.DOUBLE_EPSILON));
final double x = axisVec.getElement(0), y = axisVec.getElement(1), z = axisVec.getElement(2);
final double s = Math.sin(angle);
final double c = Math.cos(angle);
final double omc = 1 - c;
return new SimpleMatrix(new double[][] {
{x*x*omc + c, x*y*omc - z*s, x*z*omc + y*s},
{x*y*omc + z*s, y*y*omc + c, y*z*omc - x*s},
{x*z*omc - y*s, y*z*omc + x*s, z*z*omc + c}
});
}
/**
* Calculates rotational change of axis matrix from old system to new system using directional cosines.
* @param oldSystem Old coordinate System
* @param newSystem New Coordinate System
* @return change of coordinate matrix
*/
public static SimpleMatrix create3DChangeOfAxesMatrix(CoordinateSystem oldSystem, CoordinateSystem newSystem){
if(oldSystem.dimension() != newSystem.dimension()){
throw new RuntimeException("Incompartible Coordinate Systems");
}
SimpleMatrix rotator = new SimpleMatrix(oldSystem.dimension(), oldSystem.dimension());
Axis [] newAxes = newSystem.Axes();
Axis [] oldAxes = oldSystem.Axes();
for(int i = 0; i < newAxes.length; i++){
SimpleVector currAxis = newAxes[i].getAxisVector();
for(int j = 0; j < newAxes.length; j++){
rotator.addToElement(i, j, SimpleOperators.multiplyInnerProd(currAxis,oldAxes[j].getAxisVector()));
}
}
return rotator.transposed();
}
}
/*
* Copyright (C) 2010-2014 Andreas Maier, Andreas Keil
* CONRAD is developed as an Open Source project under the GNU General Public License (GPL).
*/