List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Rotation distance
public static double distance(Rotation r1, Rotation r2)
From source file:org.orekit.utils.AngularCoordinatesTest.java
@Test public void testShiftWithAcceleration() throws OrekitException { double rate = 2 * FastMath.PI / (12 * 60); double acc = 0.001; double dt = 1.0; int n = 2000; final AngularCoordinates quadratic = new AngularCoordinates(Rotation.IDENTITY, new Vector3D(rate, Vector3D.PLUS_K), new Vector3D(acc, Vector3D.PLUS_J)); final AngularCoordinates linear = new AngularCoordinates(quadratic.getRotation(), quadratic.getRotationRate(), Vector3D.ZERO); final FirstOrderDifferentialEquations ode = new FirstOrderDifferentialEquations() { public int getDimension() { return 4; }/* w w w . j a va 2 s .c o m*/ public void computeDerivatives(final double t, final double[] q, final double[] qDot) { final double omegaX = quadratic.getRotationRate().getX() + t * quadratic.getRotationAcceleration().getX(); final double omegaY = quadratic.getRotationRate().getY() + t * quadratic.getRotationAcceleration().getY(); final double omegaZ = quadratic.getRotationRate().getZ() + t * quadratic.getRotationAcceleration().getZ(); qDot[0] = 0.5 * MathArrays.linearCombination(-q[1], omegaX, -q[2], omegaY, -q[3], omegaZ); qDot[1] = 0.5 * MathArrays.linearCombination(q[0], omegaX, -q[3], omegaY, q[2], omegaZ); qDot[2] = 0.5 * MathArrays.linearCombination(q[3], omegaX, q[0], omegaY, -q[1], omegaZ); qDot[3] = 0.5 * MathArrays.linearCombination(-q[2], omegaX, q[1], omegaY, q[0], omegaZ); } }; FirstOrderIntegrator integrator = new DormandPrince853Integrator(1.0e-6, 1.0, 1.0e-12, 1.0e-12); integrator.addStepHandler(new StepNormalizer(dt / n, new FixedStepHandler() { public void init(double t0, double[] y0, double t) { } public void handleStep(double t, double[] y, double[] yDot, boolean isLast) { Rotation reference = new Rotation(y[0], y[1], y[2], y[3], true); // the error in shiftedBy taking acceleration into account is cubic double expectedCubicError = 1.4544e-6 * t * t * t; Assert.assertEquals(expectedCubicError, Rotation.distance(reference, quadratic.shiftedBy(t).getRotation()), 0.0001 * expectedCubicError); // the error in shiftedBy not taking acceleration into account is quadratic double expectedQuadraticError = 5.0e-4 * t * t; Assert.assertEquals(expectedQuadraticError, Rotation.distance(reference, linear.shiftedBy(t).getRotation()), 0.00001 * expectedQuadraticError); } })); double[] y = new double[] { quadratic.getRotation().getQ0(), quadratic.getRotation().getQ1(), quadratic.getRotation().getQ2(), quadratic.getRotation().getQ3() }; integrator.integrate(ode, 0, y, dt, y); }
From source file:org.orekit.utils.AngularCoordinatesTest.java
@Test public void testSpin() throws OrekitException { double rate = 2 * FastMath.PI / (12 * 60); AngularCoordinates angularCoordinates = new AngularCoordinates(new Rotation(0.48, 0.64, 0.36, 0.48, false), new Vector3D(rate, Vector3D.PLUS_K)); Assert.assertEquals(rate, angularCoordinates.getRotationRate().getNorm(), 1.0e-10); double dt = 10.0; AngularCoordinates shifted = angularCoordinates.shiftedBy(dt); Assert.assertEquals(rate, shifted.getRotationRate().getNorm(), 1.0e-10); Assert.assertEquals(rate * dt, Rotation.distance(angularCoordinates.getRotation(), shifted.getRotation()), 1.0e-10);//w ww . j a v a2 s . c o m Vector3D shiftedX = shifted.getRotation().applyInverseTo(Vector3D.PLUS_I); Vector3D shiftedY = shifted.getRotation().applyInverseTo(Vector3D.PLUS_J); Vector3D shiftedZ = shifted.getRotation().applyInverseTo(Vector3D.PLUS_K); Vector3D originalX = angularCoordinates.getRotation().applyInverseTo(Vector3D.PLUS_I); Vector3D originalY = angularCoordinates.getRotation().applyInverseTo(Vector3D.PLUS_J); Vector3D originalZ = angularCoordinates.getRotation().applyInverseTo(Vector3D.PLUS_K); Assert.assertEquals(FastMath.cos(rate * dt), Vector3D.dotProduct(shiftedX, originalX), 1.0e-10); Assert.assertEquals(FastMath.sin(rate * dt), Vector3D.dotProduct(shiftedX, originalY), 1.0e-10); Assert.assertEquals(0.0, Vector3D.dotProduct(shiftedX, originalZ), 1.0e-10); Assert.assertEquals(-FastMath.sin(rate * dt), Vector3D.dotProduct(shiftedY, originalX), 1.0e-10); Assert.assertEquals(FastMath.cos(rate * dt), Vector3D.dotProduct(shiftedY, originalY), 1.0e-10); Assert.assertEquals(0.0, Vector3D.dotProduct(shiftedY, originalZ), 1.0e-10); Assert.assertEquals(0.0, Vector3D.dotProduct(shiftedZ, originalX), 1.0e-10); Assert.assertEquals(0.0, Vector3D.dotProduct(shiftedZ, originalY), 1.0e-10); Assert.assertEquals(1.0, Vector3D.dotProduct(shiftedZ, originalZ), 1.0e-10); Vector3D forward = AngularCoordinates.estimateRate(angularCoordinates.getRotation(), shifted.getRotation(), dt); Assert.assertEquals(0.0, forward.subtract(angularCoordinates.getRotationRate()).getNorm(), 1.0e-10); Vector3D reversed = AngularCoordinates.estimateRate(shifted.getRotation(), angularCoordinates.getRotation(), dt); Assert.assertEquals(0.0, reversed.add(angularCoordinates.getRotationRate()).getNorm(), 1.0e-10); }
From source file:org.orekit.utils.AngularCoordinatesTest.java
@Test public void testNoCommute() { AngularCoordinates ac1 = new AngularCoordinates(new Rotation(0.48, 0.64, 0.36, 0.48, false), Vector3D.ZERO); AngularCoordinates ac2 = new AngularCoordinates(new Rotation(0.36, -0.48, 0.48, 0.64, false), Vector3D.ZERO);//from w w w . j a va2s . c o m AngularCoordinates add12 = ac1.addOffset(ac2); AngularCoordinates add21 = ac2.addOffset(ac1); // the rotations are really different from each other Assert.assertEquals(2.574, Rotation.distance(add12.getRotation(), add21.getRotation()), 1.0e-3); }
From source file:org.orekit.utils.AngularCoordinatesTest.java
@Test public void testRoundTripNoOp() { RandomGenerator random = new Well1024a(0x1e610cfe89306669l); for (int i = 0; i < 100; ++i) { Rotation r1 = randomRotation(random); Vector3D o1 = randomVector(random, 1.0e-2); Vector3D oDot1 = randomVector(random, 1.0e-2); AngularCoordinates ac1 = new AngularCoordinates(r1, o1, oDot1); Rotation r2 = randomRotation(random); Vector3D o2 = randomVector(random, 1.0e-2); Vector3D oDot2 = randomVector(random, 1.0e-2); AngularCoordinates ac2 = new AngularCoordinates(r2, o2, oDot2); AngularCoordinates roundTripSA = ac1.subtractOffset(ac2).addOffset(ac2); Assert.assertEquals(0.0, Rotation.distance(ac1.getRotation(), roundTripSA.getRotation()), 5.0e-16); Assert.assertEquals(0.0, Vector3D.distance(ac1.getRotationRate(), roundTripSA.getRotationRate()), 2.0e-17);//from w w w . j ava 2 s .co m Assert.assertEquals(0.0, Vector3D.distance(ac1.getRotationAcceleration(), roundTripSA.getRotationAcceleration()), 2.0e-17); AngularCoordinates roundTripAS = ac1.addOffset(ac2).subtractOffset(ac2); Assert.assertEquals(0.0, Rotation.distance(ac1.getRotation(), roundTripAS.getRotation()), 5.0e-16); Assert.assertEquals(0.0, Vector3D.distance(ac1.getRotationRate(), roundTripAS.getRotationRate()), 2.0e-17); Assert.assertEquals(0.0, Vector3D.distance(ac1.getRotationAcceleration(), roundTripAS.getRotationAcceleration()), 2.0e-17); } }
From source file:org.orekit.utils.AngularCoordinatesTest.java
@Test public void testRodriguesSymmetry() { // check the two-way conversion result in identity RandomGenerator random = new Well1024a(0xb1e615aaa8236b52l); for (int i = 0; i < 1000; ++i) { Rotation rotation = randomRotation(random); Vector3D rotationRate = randomVector(random, 0.01); Vector3D rotationAcceleration = randomVector(random, 0.01); AngularCoordinates ac = new AngularCoordinates(rotation, rotationRate, rotationAcceleration); AngularCoordinates rebuilt = AngularCoordinates .createFromModifiedRodrigues(ac.getModifiedRodrigues(1.0)); Assert.assertEquals(0.0, Rotation.distance(rotation, rebuilt.getRotation()), 1.0e-14); Assert.assertEquals(0.0, Vector3D.distance(rotationRate, rebuilt.getRotationRate()), 1.0e-15); Assert.assertEquals(0.0, Vector3D.distance(rotationAcceleration, rebuilt.getRotationAcceleration()), 1.0e-15);/*from w w w.j a v a 2 s.c o m*/ } }
From source file:org.orekit.utils.AngularCoordinatesTest.java
@Test public void testRandomPVCoordinates() throws OrekitException { RandomGenerator generator = new Well1024a(0x49eb5b92d1f94b89l); for (int i = 0; i < 100; ++i) { Rotation r = randomRotation(generator); Vector3D omega = randomVector(generator, 10 * generator.nextDouble() + 1.0); Vector3D omegaDot = randomVector(generator, 0.1 * generator.nextDouble() + 0.01); AngularCoordinates ref = new AngularCoordinates(r, omega, omegaDot); AngularCoordinates inv = ref.revert(); for (int j = 0; j < 100; ++j) { PVCoordinates v1 = randomPVCoordinates(generator, 1000, 1.0, 0.001); PVCoordinates v2 = randomPVCoordinates(generator, 1000, 1.0, 0.0010); PVCoordinates u1 = inv.applyTo(v1); PVCoordinates u2 = inv.applyTo(v2); AngularCoordinates rebuilt = new AngularCoordinates(u1, u2, v1, v2, 1.0e-9); Assert.assertEquals(0.0, Rotation.distance(r, rebuilt.getRotation()), 4.0e-14); Assert.assertEquals(0.0, Vector3D.distance(omega, rebuilt.getRotationRate()), 3.0e-12 * omega.getNorm()); Assert.assertEquals(0.0, Vector3D.distance(omegaDot, rebuilt.getRotationAcceleration()), 2.0e-6 * omegaDot.getNorm()); }//from w ww . jav a 2 s . co m } }
From source file:org.orekit.utils.AngularCoordinatesTest.java
@Test @Deprecated // to be removed when AngularCoordinates.interpolate is removed public void testInterpolationSimple() throws OrekitException { AbsoluteDate date = AbsoluteDate.GALILEO_EPOCH; double alpha0 = 0.5 * FastMath.PI; double omega = 0.5 * FastMath.PI; AngularCoordinates reference = new AngularCoordinates(new Rotation(Vector3D.PLUS_K, alpha0), new Vector3D(omega, Vector3D.MINUS_K), Vector3D.ZERO); List<Pair<AbsoluteDate, AngularCoordinates>> sample = new ArrayList<Pair<AbsoluteDate, AngularCoordinates>>(); for (double dt : new double[] { 0.0, 0.5, 1.0 }) { sample.add(new Pair<AbsoluteDate, AngularCoordinates>(date.shiftedBy(dt), reference.shiftedBy(dt))); }// w w w . j a v a 2 s .c o m for (double dt = 0; dt < 1.0; dt += 0.001) { AngularCoordinates interpolated = AngularCoordinates.interpolate(date.shiftedBy(dt), true, sample); Rotation r = interpolated.getRotation(); Vector3D rate = interpolated.getRotationRate(); Vector3D acceleration = interpolated.getRotationAcceleration(); Assert.assertEquals(0.0, Rotation.distance(new Rotation(Vector3D.PLUS_K, alpha0 + omega * dt), r), 1.1e-15); Assert.assertEquals(0.0, Vector3D.distance(new Vector3D(omega, Vector3D.MINUS_K), rate), 4.0e-15); Assert.assertEquals(0.0, Vector3D.distance(Vector3D.ZERO, acceleration), 3.0e-14); } }
From source file:org.orekit.utils.FieldAngularCoordinatesTest.java
@Test public void testToAC() { Random random = new Random(0xc9b4cf6c371108e0l); for (int i = 0; i < 100; ++i) { FieldRotation<DerivativeStructure> r = randomRotation(random); FieldVector3D<DerivativeStructure> o = randomVector(random, 1.0e-3); FieldVector3D<DerivativeStructure> a = randomVector(random, 1.0e-3); FieldAngularCoordinates<DerivativeStructure> acds = new FieldAngularCoordinates<DerivativeStructure>(r, o, a);//w w w. ja v a 2s. c o m AngularCoordinates ac = acds.toAngularCoordinates(); Assert.assertEquals(0, Rotation.distance(r.toRotation(), ac.getRotation()), 1.0e-15); Assert.assertEquals(0, FieldVector3D.distance(o, ac.getRotationRate()).getReal(), 1.0e-15); } }
From source file:org.orekit.utils.TimeStampedAngularCoordinatesTest.java
@Test public void testZeroRate() throws OrekitException { TimeStampedAngularCoordinates ac = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, new Rotation(0.48, 0.64, 0.36, 0.48, false), Vector3D.ZERO, Vector3D.ZERO); Assert.assertEquals(Vector3D.ZERO, ac.getRotationRate()); double dt = 10.0; TimeStampedAngularCoordinates shifted = ac.shiftedBy(dt); Assert.assertEquals(Vector3D.ZERO, shifted.getRotationAcceleration()); Assert.assertEquals(Vector3D.ZERO, shifted.getRotationRate()); Assert.assertEquals(0.0, Rotation.distance(ac.getRotation(), shifted.getRotation()), 1.0e-15); }
From source file:org.orekit.utils.TimeStampedAngularCoordinatesTest.java
@Test public void testDerivativesStructures2() throws OrekitException { RandomGenerator random = new Well1024a(0x75fbebbdbf127b3dl); Rotation r = randomRotation(random); Vector3D o = randomVector(random, 1.0e-2); Vector3D oDot = randomVector(random, 1.0e-2); TimeStampedAngularCoordinates ac = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r, o, oDot); TimeStampedAngularCoordinates rebuilt = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, ac.toDerivativeStructureRotation(2)); Assert.assertEquals(0.0, Rotation.distance(ac.getRotation(), rebuilt.getRotation()), 1.0e-15); Assert.assertEquals(0.0, Vector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()), 1.0e-15); Assert.assertEquals(0.0, Vector3D.distance(ac.getRotationAcceleration(), rebuilt.getRotationAcceleration()), 1.0e-15);// www . j a v a 2s . c o m }