diff --git a/core/src/net/sf/openrocket/util/Coordinate.java b/core/src/net/sf/openrocket/util/Coordinate.java index cd4b2844b..0299bc21f 100644 --- a/core/src/net/sf/openrocket/util/Coordinate.java +++ b/core/src/net/sf/openrocket/util/Coordinate.java @@ -61,10 +61,13 @@ public final class Coordinate implements Cloneable, Serializable { public static final Coordinate ZERO = new Coordinate(0, 0, 0, 0); public static final Coordinate NUL = new Coordinate(0, 0, 0, 0); - public static final Coordinate NaN = new Coordinate(Double.NaN, Double.NaN, - Double.NaN, Double.NaN); + public static final Coordinate NaN = new Coordinate(Double.NaN, Double.NaN,Double.NaN, Double.NaN); public static final Coordinate MAX = new Coordinate(Double.MAX_VALUE,Double.MAX_VALUE,Double.MAX_VALUE,Double.MAX_VALUE); public static final Coordinate MIN = new Coordinate(Double.MIN_VALUE,Double.MIN_VALUE,Double.MIN_VALUE,Double.MIN_VALUE); + + public static final Coordinate X_UNIT = new Coordinate(1, 0, 0); + public static final Coordinate Y_UNIT = new Coordinate(0, 1, 0); + public static final Coordinate Z_UNIT = new Coordinate(0, 0, 1); public final double x, y, z; public final double weight; diff --git a/core/src/net/sf/openrocket/util/Transformation.java b/core/src/net/sf/openrocket/util/Transformation.java index 2b0419789..48ceec437 100644 --- a/core/src/net/sf/openrocket/util/Transformation.java +++ b/core/src/net/sf/openrocket/util/Transformation.java @@ -1,5 +1,6 @@ package net.sf.openrocket.util; +import java.nio.DoubleBuffer; import java.util.ArrayList; import java.util.Collection; import java.util.Iterator; @@ -16,8 +17,7 @@ import java.util.Iterator; public class Transformation implements java.io.Serializable { - public static final Transformation IDENTITY = - new Transformation(); + public static final Transformation IDENTITY = new Transformation(); public static final Transformation PROJECT_XY = new Transformation(new double[][]{{1,0,0},{0,1,0},{0,0,0}}); @@ -26,6 +26,7 @@ public class Transformation implements java.io.Serializable { public static final Transformation PROJECT_XZ = new Transformation(new double[][]{{1,0,0},{0,0,0},{0,0,1}}); + private static final int X = 0; private static final int Y = 1; private static final int Z = 2; @@ -33,10 +34,17 @@ public class Transformation implements java.io.Serializable { private final Coordinate translate; private final double[][] rotation = new double[3][3]; + static public Transformation getTranslationTransform( double x, double y, double z) { + return new Transformation(new Coordinate(x,y,z)); + } + static public Transformation getTranslationTransform( final Coordinate translate ){ + return new Transformation( translate ); + } + /** * Create identity transformation. */ - public Transformation() { + private Transformation() { translate = new Coordinate(0,0,0); rotation[X][X]=1; rotation[Y][Y]=1; @@ -45,6 +53,7 @@ public class Transformation implements java.io.Serializable { /** * Create transformation with only translation. + * * @param x Translation in x-axis. * @param y Translation in y-axis. * @param z Translation in z-axis. @@ -190,6 +199,14 @@ public class Transformation implements java.io.Serializable { return combined; } + /** + * Returns a rotation around the rocket's long axis + * + * @param theta rotation around rocket axis, in radians + */ + static public Transformation getAxialRotation( double theta ) { + return Transformation.rotate_x(theta); + } /** * Rotate around x-axis a given angle. @@ -255,6 +272,34 @@ public class Transformation implements java.io.Serializable { return sb.toString(); } + + /** + * Rotation matrix is constructed from Euler angles, in a z-x-z order + * + * $ y = f(x) = R_z( R_x( R_z( x ))) + v $ + * + * @param alpha rotation around z (in radians) + * @param beta rotation around x' (in radians) + * @param gamma rotation around z' (in radians) + */ + static public Transformation getEulerAngle313Transform( double alpha, double beta, double gamma ) { + return new Transformation( new double[][]{ + { + (Math.cos(alpha)*Math.cos(gamma) - Math.sin(alpha)*Math.cos(beta)*Math.sin(gamma)), + (-Math.cos(alpha)*Math.sin(gamma) - Math.sin(alpha)*Math.cos(beta)*Math.cos(gamma)), + (Math.sin(alpha)*Math.sin(beta)) + },{ + (Math.sin(alpha)*Math.cos(gamma) + Math.cos(alpha)*Math.cos(beta)*Math.sin(gamma)), + (-Math.sin(alpha)*Math.sin(gamma) + Math.cos(alpha)*Math.cos(beta)*Math.cos(gamma)), + (-Math.cos(alpha)*Math.sin(beta)) + },{ + (Math.sin(beta)*Math.sin(gamma)), + (Math.sin(beta)*Math.cos(gamma)), + (Math.cos(beta)) + } + }, + Coordinate.ZERO); + } @Override public boolean equals(Object other) { @@ -269,5 +314,45 @@ public class Transformation implements java.io.Serializable { } return this.translate.equals(o.translate); } + + @Override + public int hashCode() { + long bits = 0; + for(int i=0;i>> 32)); + } + + /** + * + * + * m = [ m[0] m[4] m[ 8] m[12] ] = [ 1 0 0 1 ] + * [ m[1] m[5] m[ 9] m[13] ] [ 0 1 0 1 ] + * [ m[2] m[6] m[10] m[14] ] [ 0 0 1 1 ] + * [ m[3] m[7] m[11] m[15] ] [ 0 0 0 1 ] + * + * @return + */ + public DoubleBuffer toGLTransform() { + double[] data = new double[]{1,0,0,0,0,1,0,0,0,0,1,0,1,1,1,1}; + + // output array is in column-major order + // https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/glLoadMatrix.xml + for( int i=0; i<3; ++i) { + for( int j=0; j<3; ++j) { + data[i+j*4] = this.rotation[i][j]; + } + } + + data[12] = this.translate.x; + data[13] = this.translate.y; + data[14] = this.translate.z; + + return DoubleBuffer.wrap(data); + } } diff --git a/core/test/net/sf/openrocket/util/TestTransformation.java b/core/test/net/sf/openrocket/util/TestTransformation.java new file mode 100644 index 000000000..4d5605669 --- /dev/null +++ b/core/test/net/sf/openrocket/util/TestTransformation.java @@ -0,0 +1,206 @@ +package net.sf.openrocket.util; + +import org.junit.Test; +import static org.junit.Assert.assertEquals; + +import java.nio.DoubleBuffer; + +public class TestTransformation { + static final Coordinate x_unit = Coordinate.X_UNIT; + static final Coordinate y_unit = Coordinate.Y_UNIT; + static final Coordinate z_unit = Coordinate.Z_UNIT; + + static final double M_PI = Math.PI; + static final double M_2PI = 2*Math.PI; + static final double M_PI_2 = Math.PI/2.0; + + + @Test + public void testTransformIdentity() { + Transformation t = Transformation.IDENTITY; + assertEquals( x_unit, t.transform(x_unit) ); + assertEquals( y_unit, t.transform(y_unit) ); + assertEquals( z_unit, t.transform(z_unit) ); + } + + @Test + public void testTransformIdentityToOpenGL() { + Transformation t = Transformation.IDENTITY; + DoubleBuffer buf = t.toGLTransform(); + + assertEquals( 1.0, buf.get(0), 1e-6); + assertEquals( 0.0, buf.get(1), 1e-6); + assertEquals( 0.0, buf.get(2), 1e-6); + assertEquals( 0.0, buf.get(3), 1e-6); + + assertEquals( 0.0, buf.get(4), 1e-6); + assertEquals( 1.0, buf.get(5), 1e-6); + assertEquals( 0.0, buf.get(6), 1e-6); + assertEquals( 0.0, buf.get(7), 1e-6); + + assertEquals( 0.0, buf.get( 8), 1e-6); + assertEquals( 0.0, buf.get( 9), 1e-6); + assertEquals( 1.0, buf.get(10), 1e-6); + assertEquals( 0.0, buf.get(11), 1e-6); + + assertEquals( 0.0, buf.get(12), 1e-6); + assertEquals( 0.0, buf.get(13), 1e-6); + assertEquals( 0.0, buf.get(14), 1e-6); + assertEquals( 1.0, buf.get(15), 1e-6); + } + + @Test + public void testTransformTranslationToOpenGL() { + Transformation translate = new Transformation( 1,2,3 ); + DoubleBuffer buf = translate.toGLTransform(); + + assertEquals( 1.0, buf.get(0), 1e-6); + assertEquals( 0.0, buf.get(1), 1e-6); + assertEquals( 0.0, buf.get(2), 1e-6); + assertEquals( 0.0, buf.get(3), 1e-6); + + assertEquals( 0.0, buf.get(4), 1e-6); + assertEquals( 1.0, buf.get(5), 1e-6); + assertEquals( 0.0, buf.get(6), 1e-6); + assertEquals( 0.0, buf.get(7), 1e-6); + + assertEquals( 0.0, buf.get( 8), 1e-6); + assertEquals( 0.0, buf.get( 9), 1e-6); + assertEquals( 1.0, buf.get(10), 1e-6); + assertEquals( 0.0, buf.get(11), 1e-6); + + assertEquals( 1.0, buf.get(12), 1e-6); + assertEquals( 2.0, buf.get(13), 1e-6); + assertEquals( 3.0, buf.get(14), 1e-6); + assertEquals( 1.0, buf.get(15), 1e-6); + } + + + @Test + public void testTransformRotateByPI2ToOpenGL() { + Transformation translate = Transformation.getAxialRotation(M_PI_2); + DoubleBuffer buf = translate.toGLTransform(); + + assertEquals( 1.0, buf.get(0), 1e-6); + assertEquals( 0.0, buf.get(1), 1e-6); + assertEquals( 0.0, buf.get(2), 1e-6); + assertEquals( 0.0, buf.get(3), 1e-6); + + assertEquals( 0.0, buf.get(4), 1e-6); + assertEquals( 0.0, buf.get(5), 1e-6); + assertEquals( 1.0, buf.get(6), 1e-6); + assertEquals( 0.0, buf.get(7), 1e-6); + + assertEquals( 0.0, buf.get( 8), 1e-6); + assertEquals( -1.0, buf.get( 9), 1e-6); + assertEquals( 0.0, buf.get(10), 1e-6); + assertEquals( 0.0, buf.get(11), 1e-6); + + assertEquals( 0.0, buf.get(12), 1e-6); + assertEquals( 0.0, buf.get(13), 1e-6); + assertEquals( 0.0, buf.get(14), 1e-6); + assertEquals( 1.0, buf.get(15), 1e-6); + } + + @Test + public void testTransformTranslationIndividual() { + Transformation translate = new Transformation( 1,2,3 ); + + assertEquals( new Coordinate(2,2,3), translate.transform( x_unit )); + assertEquals( new Coordinate(1,3,3), translate.transform( y_unit )); + assertEquals( new Coordinate(1,2,4), translate.transform( z_unit )); + } + + @Test + public void testTransformTranslationCoordinate() { + Transformation translate = new Transformation( new Coordinate( 2,3,4)); + + assertEquals( new Coordinate(3,3,4), translate.transform( x_unit )); + assertEquals( new Coordinate(2,4,4), translate.transform( y_unit )); + assertEquals( new Coordinate(2,3,5), translate.transform( z_unit )); + } + + @Test + public void testTransformTranslationConvenience() { + Transformation translate = Transformation.getTranslationTransform( 2,3,4); + + assertEquals( new Coordinate(3,3,4), translate.transform( x_unit )); + assertEquals( new Coordinate(2,4,4), translate.transform( y_unit )); + assertEquals( new Coordinate(2,3,5), translate.transform( z_unit )); + } + + @Test + public void testTransformSmallYRotation() { + Transformation t = Transformation.rotate_y(0.01); + + Coordinate v1 = t.transform( x_unit ); + // we need to test individual coordinates due to error. + assertEquals( 1, v1.x, .001); + assertEquals( 0, v1.y, .001); + assertEquals( -.01, v1.z, .001); + + assertEquals( y_unit, t.transform( y_unit )); + + Coordinate v2 = t.transform( z_unit ); + // we need to test individual coordinates due to error. + assertEquals( .01, v2.x, .001); + assertEquals( 0, v2.y, .001); + assertEquals( 1, v2.z, .001); + } + + @Test + public void testTransformRotateXByPI2() { + Transformation t = Transformation.getAxialRotation(M_PI_2); + + assertEquals( x_unit, t.transform(x_unit)); + assertEquals( z_unit, t.transform( y_unit )); + assertEquals( y_unit.multiply(-1), t.transform( z_unit )); + } + + + @Test + public void testTransformEuler313Transform() { + { + Transformation r313 = Transformation.getEulerAngle313Transform(0.0, 0.0, M_PI_2); + assertEquals( y_unit, r313.transform( x_unit )); + assertEquals( x_unit.multiply(-1), r313.transform( y_unit )); + assertEquals( z_unit, r313.transform( z_unit )); + }{ + Transformation r313 = Transformation.getEulerAngle313Transform(M_PI/4.0, 0.0, M_PI/4.0); + // precision = 8 decimal places + assertEquals( y_unit, r313.transform( x_unit )); + assertEquals( x_unit.multiply(-1), r313.transform( y_unit )); + assertEquals( z_unit, r313.transform( z_unit )); + }{ + Transformation r313 = Transformation.getEulerAngle313Transform(M_PI/4.0, M_PI_2, M_PI/4.0); + // precision = 8 decimal places + assertEquals( new Coordinate(+0.500000, +0.500000, 0.707106781), r313.transform( x_unit )); + assertEquals( new Coordinate(-0.500000, -0.5000000, 0.707106781), r313.transform( y_unit )); + assertEquals( new Coordinate(+0.707106781, -0.707106781, 0.0), r313.transform( z_unit )); + } + } + + @Test + public void testTransformEuler121Transform() { + Transformation r123 = Transformation.rotate_x(-1.0) + .applyTransformation(Transformation.rotate_y(0.01)) + .applyTransformation(Transformation.rotate_z(1.0)); + + + assertEquals( new Coordinate(+0.540275291, +0.450102302, -0.710992634), r123.transform( x_unit )); + assertEquals( new Coordinate(-0.841428912, +0.299007198, -0.450102302), r123.transform( y_unit )); + assertEquals( new Coordinate(+0.009999833334, +0.841428911609, +0.540275290977), r123.transform( z_unit )); + + } + + @Test + public void testTransformRotateTranslate() { + Transformation r = Transformation.getTranslationTransform( 2,3,4) + .applyTransformation(Transformation.getAxialRotation( M_PI_2 )); + + assertEquals( new Coordinate( 3,3,4), r.transform( x_unit )); + assertEquals( new Coordinate( 2,3,5), r.transform( y_unit )); + assertEquals( new Coordinate( 2,2,4), r.transform( z_unit )); + } + +} diff --git a/core/test/net/sf/openrocket/util/TransformationTest.java b/core/test/net/sf/openrocket/util/TransformationTest.java deleted file mode 100644 index cb63b3f27..000000000 --- a/core/test/net/sf/openrocket/util/TransformationTest.java +++ /dev/null @@ -1,81 +0,0 @@ -package net.sf.openrocket.util; - -import org.junit.Test; -import static org.junit.Assert.assertEquals; - - -public class TransformationTest { - @Test - public void oldMainTest() { - Transformation t; - - t = new Transformation(); - { - Coordinate a = t.transform( new Coordinate(1,0,0) ); - assertEquals( new Coordinate(1,0,0), a ); - a = t.transform( new Coordinate(0,1,0) ); - assertEquals( new Coordinate(0,1,0), a ); - a = t.transform( new Coordinate(0,0,1) ); - assertEquals( new Coordinate(0,0,1), a ); - } - - t = new Transformation(1,2,3); - { - Coordinate a = t.transform( new Coordinate(1,0,0) ); - assertEquals( new Coordinate(2,2,3), a ); - a = t.transform( new Coordinate(0,1,0) ); - assertEquals( new Coordinate(1,3,3), a ); - a = t.transform( new Coordinate(0,0,1) ); - assertEquals( new Coordinate(1,2,4), a ); - } - - - t = new Transformation(new Coordinate(2,3,4)); - { - Coordinate a = t.transform( new Coordinate(1,0,0) ); - assertEquals( new Coordinate(3,3,4), a ); - a = t.transform( new Coordinate(0,1,0) ); - assertEquals( new Coordinate(2,4,4), a ); - a = t.transform( new Coordinate(0,0,1) ); - assertEquals( new Coordinate(2,3,5), a ); - } - - t = Transformation.rotate_y(0.01); - { - Coordinate a = t.transform( new Coordinate(1,0,0) ); - // we need to test individual coordinates due to error. - assertEquals( 1, a.x, .001); - assertEquals( 0, a.y, .001); - assertEquals( -.01, a.z, .001); - a = t.transform( new Coordinate(0,1,0) ); - assertEquals( new Coordinate(0,1,0), a ); - a = t.transform( new Coordinate(0,0,1) ); - // we need to test individual coordinates due to error. - assertEquals( .01, a.x, .001); - assertEquals( 0, a.y, .001); - assertEquals( 1, a.z, .001); - } - - t = new Transformation(-1,0,0); - t = t.applyTransformation(Transformation.rotate_y(0.01)); - t = t.applyTransformation(new Transformation(1,0,0)); - { - Coordinate a = t.transform( new Coordinate(1,0,0) ); - // we need to test individual coordinates due to error. - assertEquals( 1, a.x, .001); - assertEquals( 0, a.y, .001); - assertEquals( -.02, a.z, .001); - a = t.transform( new Coordinate(0,1,0) ); - assertEquals( 0, a.x, .001); - assertEquals( 1, a.y, .001); - assertEquals( -.01, a.z, .001); - a = t.transform( new Coordinate(0,0,1) ); - // we need to test individual coordinates due to error. - assertEquals( .01, a.x, .001); - assertEquals( 0, a.y, .001); - assertEquals( .99, a.z, .001); - } - } - - -}