List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D PLUS_J
Vector3D PLUS_J
To view the source code for org.apache.commons.math3.geometry.euclidean.threed Vector3D PLUS_J.
Click Source Link
From source file:org.orekit.frames.LocalOrbitalFrameTest.java
private void checkFrame(LOFType type, AbsoluteDate date, Vector3D expectedXDirection, Vector3D expectedYDirection, Vector3D expectedZDirection, Vector3D expectedRotationDirection) throws OrekitException { LocalOrbitalFrame lof = new LocalOrbitalFrame(FramesFactory.getGCRF(), type, provider, type.name()); Transform t = lof.getTransformTo(FramesFactory.getGCRF(), date); PVCoordinates pv1 = t.transformPVCoordinates(PVCoordinates.ZERO); Vector3D p1 = pv1.getPosition(); Vector3D v1 = pv1.getVelocity(); PVCoordinates pv2 = provider.getPVCoordinates(date, FramesFactory.getGCRF()); Vector3D p2 = pv2.getPosition(); Vector3D v2 = pv2.getVelocity(); Assert.assertEquals(0, p1.subtract(p2).getNorm(), 1.0e-14 * p1.getNorm()); Assert.assertEquals(0, v1.subtract(v2).getNorm(), 1.0e-14 * v1.getNorm()); Vector3D xDirection = t.transformVector(Vector3D.PLUS_I); Vector3D yDirection = t.transformVector(Vector3D.PLUS_J); Vector3D zDirection = t.transformVector(Vector3D.PLUS_K); Assert.assertEquals(0, Vector3D.angle(expectedXDirection, xDirection), 2.0e-15); Assert.assertEquals(0, Vector3D.angle(expectedYDirection, yDirection), 1.0e-15); Assert.assertEquals(0, Vector3D.angle(expectedZDirection, zDirection), 1.0e-15); Assert.assertEquals(0, Vector3D.angle(expectedRotationDirection, t.getRotationRate()), 1.0e-15); Assert.assertEquals(initialOrbit.getKeplerianMeanMotion(), t.getRotationRate().getNorm(), 1.0e-7); }
From source file:org.orekit.frames.MODProviderTest.java
@Test public void testEuler1976() throws OrekitException { TransformProvider eulerBasedProvider = new TransformProvider() { private static final long serialVersionUID = 1L; private final PolynomialNutation<DerivativeStructure> zetaA = new PolynomialNutation<DerivativeStructure>( 0.0, 2306.2181 * Constants.ARC_SECONDS_TO_RADIANS, 0.30188 * Constants.ARC_SECONDS_TO_RADIANS, 0.017998 * Constants.ARC_SECONDS_TO_RADIANS); private final PolynomialNutation<DerivativeStructure> thetaA = new PolynomialNutation<DerivativeStructure>( 0.0, 2004.3109 * Constants.ARC_SECONDS_TO_RADIANS, -0.42665 * Constants.ARC_SECONDS_TO_RADIANS, -0.041833 * Constants.ARC_SECONDS_TO_RADIANS); private final PolynomialNutation<DerivativeStructure> zA = new PolynomialNutation<DerivativeStructure>( 0.0, 2306.2181 * Constants.ARC_SECONDS_TO_RADIANS, 1.09468 * Constants.ARC_SECONDS_TO_RADIANS, 0.018203 * Constants.ARC_SECONDS_TO_RADIANS); public Transform getTransform(AbsoluteDate date) { final double tc = IERSConventions.IERS_1996.evaluateTC(date); final Rotation r1 = new Rotation(Vector3D.PLUS_K, zA.value(tc)); final Rotation r2 = new Rotation(Vector3D.PLUS_J, -thetaA.value(tc)); final Rotation r3 = new Rotation(Vector3D.PLUS_K, zetaA.value(tc)); return new Transform(date, r1.applyTo(r2.applyTo(r3))); }//from w w w.j av a 2 s .c o m }; MODProvider modProvider = new MODProvider(IERSConventions.IERS_1996); for (double dt = -5 * Constants.JULIAN_YEAR; dt < 5 * Constants.JULIAN_YEAR; dt += 10 * Constants.JULIAN_DAY) { AbsoluteDate date = AbsoluteDate.J2000_EPOCH.shiftedBy(dt); Transform t = new Transform(date, modProvider.getTransform(date).getInverse(), eulerBasedProvider.getTransform(date)); Assert.assertEquals(0, t.getRotation().getAngle(), 1.01e-11); } }
From source file:org.orekit.frames.MODProviderTest.java
@Test public void testEuler2000() throws OrekitException { // this alternate representation of the transform // is from equation 33 in IERS conventions 2003 TransformProvider eulerBasedProvider = new TransformProvider() { private static final long serialVersionUID = 1L; private final PolynomialNutation<DerivativeStructure> zetaA = new PolynomialNutation<DerivativeStructure>( 2.5976176 * Constants.ARC_SECONDS_TO_RADIANS, 2306.0809506 * Constants.ARC_SECONDS_TO_RADIANS, 0.3019015 * Constants.ARC_SECONDS_TO_RADIANS, 0.0179663 * Constants.ARC_SECONDS_TO_RADIANS, -0.0000327 * Constants.ARC_SECONDS_TO_RADIANS, -0.0000002 * Constants.ARC_SECONDS_TO_RADIANS); private final PolynomialNutation<DerivativeStructure> thetaA = new PolynomialNutation<DerivativeStructure>( 0.0, 2004.1917476 * Constants.ARC_SECONDS_TO_RADIANS, -0.4269353 * Constants.ARC_SECONDS_TO_RADIANS, -0.0418251 * Constants.ARC_SECONDS_TO_RADIANS, -0.0000601 * Constants.ARC_SECONDS_TO_RADIANS, -0.0000001 * Constants.ARC_SECONDS_TO_RADIANS); private final PolynomialNutation<DerivativeStructure> zA = new PolynomialNutation<DerivativeStructure>( -2.5976176 * Constants.ARC_SECONDS_TO_RADIANS, 2306.0803226 * Constants.ARC_SECONDS_TO_RADIANS, 1.0947790 * Constants.ARC_SECONDS_TO_RADIANS, 0.0182273 * Constants.ARC_SECONDS_TO_RADIANS, 0.0000470 * Constants.ARC_SECONDS_TO_RADIANS, -0.0000003 * Constants.ARC_SECONDS_TO_RADIANS); public Transform getTransform(AbsoluteDate date) { final double tc = IERSConventions.IERS_2003.evaluateTC(date); final Rotation r1 = new Rotation(Vector3D.PLUS_K, zA.value(tc)); final Rotation r2 = new Rotation(Vector3D.PLUS_J, -thetaA.value(tc)); final Rotation r3 = new Rotation(Vector3D.PLUS_K, zetaA.value(tc)); return new Transform(date, r1.applyTo(r2.applyTo(r3))); }//from www . jav a 2 s.c o m }; MODProvider modProvider = new MODProvider(IERSConventions.IERS_2003); for (double dt = -Constants.JULIAN_CENTURY; dt < Constants.JULIAN_CENTURY; dt += 50 * Constants.JULIAN_DAY) { AbsoluteDate date = AbsoluteDate.J2000_EPOCH.shiftedBy(dt); Transform t = new Transform(date, modProvider.getTransform(date).getInverse(), eulerBasedProvider.getTransform(date)); Assert.assertEquals(0, t.getRotation().getAngle(), 6.6e-13); } }
From source file:org.orekit.frames.TopocentricFrameTest.java
@Test public void testZero() { final GeodeticPoint point = new GeodeticPoint(0., 0., 0.); final TopocentricFrame topoFrame = new TopocentricFrame(earthSpheric, point, "zero"); // Check that frame directions are aligned final double xDiff = Vector3D.dotProduct(topoFrame.getEast(), Vector3D.PLUS_J); final double yDiff = Vector3D.dotProduct(topoFrame.getNorth(), Vector3D.PLUS_K); final double zDiff = Vector3D.dotProduct(topoFrame.getZenith(), Vector3D.PLUS_I); Assert.assertEquals(1., xDiff, Utils.epsilonTest); Assert.assertEquals(1., yDiff, Utils.epsilonTest); Assert.assertEquals(1., zDiff, Utils.epsilonTest); }
From source file:org.orekit.frames.TopocentricFrameTest.java
@Test public void testPole() { final GeodeticPoint point = new GeodeticPoint(FastMath.PI / 2., 0., 0.); final TopocentricFrame topoFrame = new TopocentricFrame(earthSpheric, point, "north pole"); // Check that frame directions are aligned final double xDiff = Vector3D.dotProduct(topoFrame.getEast(), Vector3D.PLUS_J); final double yDiff = Vector3D.dotProduct(topoFrame.getNorth(), Vector3D.PLUS_I.negate()); final double zDiff = Vector3D.dotProduct(topoFrame.getZenith(), Vector3D.PLUS_K); Assert.assertEquals(1., xDiff, Utils.epsilonTest); Assert.assertEquals(1., yDiff, Utils.epsilonTest); Assert.assertEquals(1., zDiff, Utils.epsilonTest); }
From source file:org.orekit.frames.TransformTest.java
@Test public void testRoughTransPV() { PVCoordinates pointP1 = new PVCoordinates(Vector3D.PLUS_I, Vector3D.PLUS_I, Vector3D.PLUS_I); // translation transform test PVCoordinates pointP2 = new PVCoordinates(new Vector3D(0, 0, 0), new Vector3D(0, 0, 0)); Transform R1toR2 = new Transform(AbsoluteDate.J2000_EPOCH, Vector3D.MINUS_I, Vector3D.MINUS_I, Vector3D.MINUS_I);// w ww .j a va 2 s. c om PVCoordinates result1 = R1toR2.transformPVCoordinates(pointP1); checkVector(pointP2.getPosition(), result1.getPosition(), 1.0e-15); checkVector(pointP2.getVelocity(), result1.getVelocity(), 1.0e-15); checkVector(pointP2.getAcceleration(), result1.getAcceleration(), 1.0e-15); // test inverse translation Transform R2toR1 = R1toR2.getInverse(); PVCoordinates invResult1 = R2toR1.transformPVCoordinates(pointP2); checkVector(pointP1.getPosition(), invResult1.getPosition(), 1.0e-15); checkVector(pointP1.getVelocity(), invResult1.getVelocity(), 1.0e-15); checkVector(pointP1.getAcceleration(), invResult1.getAcceleration(), 1.0e-15); // rotation transform test PVCoordinates pointP3 = new PVCoordinates(Vector3D.PLUS_J, new Vector3D(-2, 1, 0), new Vector3D(-4, -3, -1)); Rotation R = new Rotation(Vector3D.PLUS_K, FastMath.PI / 2); Transform R1toR3 = new Transform(AbsoluteDate.J2000_EPOCH, R, new Vector3D(0, 0, -2), new Vector3D(1, 0, 0)); PVCoordinates result2 = R1toR3.transformPVCoordinates(pointP1); checkVector(pointP3.getPosition(), result2.getPosition(), 1.0e-15); checkVector(pointP3.getVelocity(), result2.getVelocity(), 1.0e-15); checkVector(pointP3.getAcceleration(), result2.getAcceleration(), 1.0e-15); // test inverse rotation Transform R3toR1 = R1toR3.getInverse(); PVCoordinates invResult2 = R3toR1.transformPVCoordinates(pointP3); checkVector(pointP1.getPosition(), invResult2.getPosition(), 1.0e-15); checkVector(pointP1.getVelocity(), invResult2.getVelocity(), 1.0e-15); checkVector(pointP1.getAcceleration(), invResult2.getAcceleration(), 1.0e-15); // combine 2 velocity transform Transform R1toR4 = new Transform(AbsoluteDate.J2000_EPOCH, new Vector3D(-2, 0, 0), new Vector3D(-2, 0, 0), new Vector3D(-2, 0, 0)); PVCoordinates pointP4 = new PVCoordinates(new Vector3D(-1, 0, 0), new Vector3D(-1, 0, 0), new Vector3D(-1, 0, 0)); Transform R2toR4 = new Transform(AbsoluteDate.J2000_EPOCH, R2toR1, R1toR4); PVCoordinates compResult = R2toR4.transformPVCoordinates(pointP2); checkVector(pointP4.getPosition(), compResult.getPosition(), 1.0e-15); checkVector(pointP4.getVelocity(), compResult.getVelocity(), 1.0e-15); checkVector(pointP4.getAcceleration(), compResult.getAcceleration(), 1.0e-15); // combine 2 rotation tranform PVCoordinates pointP5 = new PVCoordinates(new Vector3D(-1, 0, 0), new Vector3D(-1, 0, 3), new Vector3D(8, 0, 6)); Rotation R2 = new Rotation(new Vector3D(0, 0, 1), FastMath.PI); Transform R1toR5 = new Transform(AbsoluteDate.J2000_EPOCH, R2, new Vector3D(0, -3, 0)); Transform R3toR5 = new Transform(AbsoluteDate.J2000_EPOCH, R3toR1, R1toR5); PVCoordinates combResult = R3toR5.transformPVCoordinates(pointP3); checkVector(pointP5.getPosition(), combResult.getPosition(), 1.0e-15); checkVector(pointP5.getVelocity(), combResult.getVelocity(), 1.0e-15); checkVector(pointP5.getAcceleration(), combResult.getAcceleration(), 1.0e-15); // combine translation and rotation Transform R2toR3 = new Transform(AbsoluteDate.J2000_EPOCH, R2toR1, R1toR3); PVCoordinates result = R2toR3.transformPVCoordinates(pointP2); checkVector(pointP3.getPosition(), result.getPosition(), 1.0e-15); checkVector(pointP3.getVelocity(), result.getVelocity(), 1.0e-15); checkVector(pointP3.getAcceleration(), result.getAcceleration(), 1.0e-15); Transform R3toR2 = new Transform(AbsoluteDate.J2000_EPOCH, R3toR1, R1toR2); result = R3toR2.transformPVCoordinates(pointP3); checkVector(pointP2.getPosition(), result.getPosition(), 1.0e-15); checkVector(pointP2.getVelocity(), result.getVelocity(), 1.0e-15); checkVector(pointP2.getAcceleration(), result.getAcceleration(), 1.0e-15); Transform newR1toR5 = new Transform(AbsoluteDate.J2000_EPOCH, R1toR2, R2toR3); newR1toR5 = new Transform(AbsoluteDate.J2000_EPOCH, newR1toR5, R3toR5); result = newR1toR5.transformPVCoordinates(pointP1); checkVector(pointP5.getPosition(), result.getPosition(), 1.0e-15); checkVector(pointP5.getVelocity(), result.getVelocity(), 1.0e-15); checkVector(pointP5.getAcceleration(), result.getAcceleration(), 1.0e-15); // more tests newR1toR5 = new Transform(AbsoluteDate.J2000_EPOCH, R1toR2, R2toR3); Transform R3toR4 = new Transform(AbsoluteDate.J2000_EPOCH, R3toR1, R1toR4); newR1toR5 = new Transform(AbsoluteDate.J2000_EPOCH, newR1toR5, R3toR4); Transform R4toR5 = new Transform(AbsoluteDate.J2000_EPOCH, R1toR4.getInverse(), R1toR5); newR1toR5 = new Transform(AbsoluteDate.J2000_EPOCH, newR1toR5, R4toR5); result = newR1toR5.transformPVCoordinates(pointP1); checkVector(pointP5.getPosition(), result.getPosition(), 1.0e-15); checkVector(pointP5.getVelocity(), result.getVelocity(), 1.0e-15); checkVector(pointP5.getAcceleration(), result.getAcceleration(), 1.0e-15); }
From source file:org.orekit.frames.TransformTest.java
@Test public void testJacobianP() { // base directions for finite differences PVCoordinates[] directions = new PVCoordinates[] { new PVCoordinates(Vector3D.PLUS_I, Vector3D.ZERO, Vector3D.ZERO), new PVCoordinates(Vector3D.PLUS_J, Vector3D.ZERO, Vector3D.ZERO), new PVCoordinates(Vector3D.PLUS_K, Vector3D.ZERO, Vector3D.ZERO), }; double h = 0.01; RandomGenerator random = new Well19937a(0x47fd0d6809f4b173l); for (int i = 0; i < 20; ++i) { // generate a random transform Transform combined = randomTransform(random); // compute Jacobian double[][] jacobian = new double[9][9]; for (int l = 0; l < jacobian.length; ++l) { for (int c = 0; c < jacobian[l].length; ++c) { jacobian[l][c] = l + 0.1 * c; }//from w w w.jav a2 s . c om } combined.getJacobian(CartesianDerivativesFilter.USE_P, jacobian); for (int j = 0; j < 100; ++j) { PVCoordinates pv0 = new PVCoordinates(randomVector(1e3, random), randomVector(1.0, random), randomVector(1.0e-3, random)); double epsilonP = 2.0e-12 * pv0.getPosition().getNorm(); for (int c = 0; c < directions.length; ++c) { // eight points finite differences estimation of a Jacobian column PVCoordinates pvm4h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, -4 * h, directions[c])); PVCoordinates pvm3h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, -3 * h, directions[c])); PVCoordinates pvm2h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, -2 * h, directions[c])); PVCoordinates pvm1h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, -1 * h, directions[c])); PVCoordinates pvp1h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, +1 * h, directions[c])); PVCoordinates pvp2h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, +2 * h, directions[c])); PVCoordinates pvp3h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, +3 * h, directions[c])); PVCoordinates pvp4h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, +4 * h, directions[c])); PVCoordinates d4 = new PVCoordinates(pvm4h, pvp4h); PVCoordinates d3 = new PVCoordinates(pvm3h, pvp3h); PVCoordinates d2 = new PVCoordinates(pvm2h, pvp2h); PVCoordinates d1 = new PVCoordinates(pvm1h, pvp1h); double d = 1.0 / (840 * h); PVCoordinates estimatedColumn = new PVCoordinates(-3 * d, d4, 32 * d, d3, -168 * d, d2, 672 * d, d1); // check analytical Jacobian against finite difference reference Assert.assertEquals(estimatedColumn.getPosition().getX(), jacobian[0][c], epsilonP); Assert.assertEquals(estimatedColumn.getPosition().getY(), jacobian[1][c], epsilonP); Assert.assertEquals(estimatedColumn.getPosition().getZ(), jacobian[2][c], epsilonP); // check the rest of the matrix remains untouched for (int l = 3; l < jacobian.length; ++l) { Assert.assertEquals(l + 0.1 * c, jacobian[l][c], 1.0e-15); } } // check the rest of the matrix remains untouched for (int c = directions.length; c < jacobian[0].length; ++c) { for (int l = 0; l < jacobian.length; ++l) { Assert.assertEquals(l + 0.1 * c, jacobian[l][c], 1.0e-15); } } } } }
From source file:org.orekit.frames.TransformTest.java
@Test public void testJacobianPV() { // base directions for finite differences PVCoordinates[] directions = new PVCoordinates[] { new PVCoordinates(Vector3D.PLUS_I, Vector3D.ZERO, Vector3D.ZERO), new PVCoordinates(Vector3D.PLUS_J, Vector3D.ZERO, Vector3D.ZERO), new PVCoordinates(Vector3D.PLUS_K, Vector3D.ZERO, Vector3D.ZERO), new PVCoordinates(Vector3D.ZERO, Vector3D.PLUS_I, Vector3D.ZERO), new PVCoordinates(Vector3D.ZERO, Vector3D.PLUS_J, Vector3D.ZERO), new PVCoordinates(Vector3D.ZERO, Vector3D.PLUS_K, Vector3D.ZERO) }; double h = 0.01; RandomGenerator random = new Well19937a(0xce2bfddfbb9796bel); for (int i = 0; i < 20; ++i) { // generate a random transform Transform combined = randomTransform(random); // compute Jacobian double[][] jacobian = new double[9][9]; for (int l = 0; l < jacobian.length; ++l) { for (int c = 0; c < jacobian[l].length; ++c) { jacobian[l][c] = l + 0.1 * c; }/*from w w w . j av a2s .c o m*/ } combined.getJacobian(CartesianDerivativesFilter.USE_PV, jacobian); for (int j = 0; j < 100; ++j) { PVCoordinates pv0 = new PVCoordinates(randomVector(1e3, random), randomVector(1.0, random), randomVector(1.0e-3, random)); double epsilonP = 2.0e-12 * pv0.getPosition().getNorm(); double epsilonV = 6.0e-11 * pv0.getVelocity().getNorm(); for (int c = 0; c < directions.length; ++c) { // eight points finite differences estimation of a Jacobian column PVCoordinates pvm4h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, -4 * h, directions[c])); PVCoordinates pvm3h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, -3 * h, directions[c])); PVCoordinates pvm2h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, -2 * h, directions[c])); PVCoordinates pvm1h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, -1 * h, directions[c])); PVCoordinates pvp1h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, +1 * h, directions[c])); PVCoordinates pvp2h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, +2 * h, directions[c])); PVCoordinates pvp3h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, +3 * h, directions[c])); PVCoordinates pvp4h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, +4 * h, directions[c])); PVCoordinates d4 = new PVCoordinates(pvm4h, pvp4h); PVCoordinates d3 = new PVCoordinates(pvm3h, pvp3h); PVCoordinates d2 = new PVCoordinates(pvm2h, pvp2h); PVCoordinates d1 = new PVCoordinates(pvm1h, pvp1h); double d = 1.0 / (840 * h); PVCoordinates estimatedColumn = new PVCoordinates(-3 * d, d4, 32 * d, d3, -168 * d, d2, 672 * d, d1); // check analytical Jacobian against finite difference reference Assert.assertEquals(estimatedColumn.getPosition().getX(), jacobian[0][c], epsilonP); Assert.assertEquals(estimatedColumn.getPosition().getY(), jacobian[1][c], epsilonP); Assert.assertEquals(estimatedColumn.getPosition().getZ(), jacobian[2][c], epsilonP); Assert.assertEquals(estimatedColumn.getVelocity().getX(), jacobian[3][c], epsilonV); Assert.assertEquals(estimatedColumn.getVelocity().getY(), jacobian[4][c], epsilonV); Assert.assertEquals(estimatedColumn.getVelocity().getZ(), jacobian[5][c], epsilonV); // check the rest of the matrix remains untouched for (int l = 6; l < jacobian.length; ++l) { Assert.assertEquals(l + 0.1 * c, jacobian[l][c], 1.0e-15); } } // check the rest of the matrix remains untouched for (int c = directions.length; c < jacobian[0].length; ++c) { for (int l = 0; l < jacobian.length; ++l) { Assert.assertEquals(l + 0.1 * c, jacobian[l][c], 1.0e-15); } } } } }
From source file:org.orekit.frames.TransformTest.java
@Test public void testJacobianPVA() { // base directions for finite differences PVCoordinates[] directions = new PVCoordinates[] { new PVCoordinates(Vector3D.PLUS_I, Vector3D.ZERO, Vector3D.ZERO), new PVCoordinates(Vector3D.PLUS_J, Vector3D.ZERO, Vector3D.ZERO), new PVCoordinates(Vector3D.PLUS_K, Vector3D.ZERO, Vector3D.ZERO), new PVCoordinates(Vector3D.ZERO, Vector3D.PLUS_I, Vector3D.ZERO), new PVCoordinates(Vector3D.ZERO, Vector3D.PLUS_J, Vector3D.ZERO), new PVCoordinates(Vector3D.ZERO, Vector3D.PLUS_K, Vector3D.ZERO), new PVCoordinates(Vector3D.ZERO, Vector3D.ZERO, Vector3D.PLUS_I), new PVCoordinates(Vector3D.ZERO, Vector3D.ZERO, Vector3D.PLUS_J), new PVCoordinates(Vector3D.ZERO, Vector3D.ZERO, Vector3D.PLUS_K) }; double h = 0.01; RandomGenerator random = new Well19937a(0xd223e88b6232198fl); for (int i = 0; i < 20; ++i) { // generate a random transform Transform combined = randomTransform(random); // compute Jacobian double[][] jacobian = new double[9][9]; for (int l = 0; l < jacobian.length; ++l) { for (int c = 0; c < jacobian[l].length; ++c) { jacobian[l][c] = l + 0.1 * c; }/* w w w .ja va 2 s . c om*/ } combined.getJacobian(CartesianDerivativesFilter.USE_PVA, jacobian); for (int j = 0; j < 100; ++j) { PVCoordinates pv0 = new PVCoordinates(randomVector(1e3, random), randomVector(1.0, random), randomVector(1.0e-3, random)); double epsilonP = 2.0e-12 * pv0.getPosition().getNorm(); double epsilonV = 6.0e-11 * pv0.getVelocity().getNorm(); double epsilonA = 2.0e-9 * pv0.getAcceleration().getNorm(); for (int c = 0; c < directions.length; ++c) { // eight points finite differences estimation of a Jacobian column PVCoordinates pvm4h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, -4 * h, directions[c])); PVCoordinates pvm3h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, -3 * h, directions[c])); PVCoordinates pvm2h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, -2 * h, directions[c])); PVCoordinates pvm1h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, -1 * h, directions[c])); PVCoordinates pvp1h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, +1 * h, directions[c])); PVCoordinates pvp2h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, +2 * h, directions[c])); PVCoordinates pvp3h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, +3 * h, directions[c])); PVCoordinates pvp4h = combined .transformPVCoordinates(new PVCoordinates(1.0, pv0, +4 * h, directions[c])); PVCoordinates d4 = new PVCoordinates(pvm4h, pvp4h); PVCoordinates d3 = new PVCoordinates(pvm3h, pvp3h); PVCoordinates d2 = new PVCoordinates(pvm2h, pvp2h); PVCoordinates d1 = new PVCoordinates(pvm1h, pvp1h); double d = 1.0 / (840 * h); PVCoordinates estimatedColumn = new PVCoordinates(-3 * d, d4, 32 * d, d3, -168 * d, d2, 672 * d, d1); // check analytical Jacobian against finite difference reference Assert.assertEquals(estimatedColumn.getPosition().getX(), jacobian[0][c], epsilonP); Assert.assertEquals(estimatedColumn.getPosition().getY(), jacobian[1][c], epsilonP); Assert.assertEquals(estimatedColumn.getPosition().getZ(), jacobian[2][c], epsilonP); Assert.assertEquals(estimatedColumn.getVelocity().getX(), jacobian[3][c], epsilonV); Assert.assertEquals(estimatedColumn.getVelocity().getY(), jacobian[4][c], epsilonV); Assert.assertEquals(estimatedColumn.getVelocity().getZ(), jacobian[5][c], epsilonV); Assert.assertEquals(estimatedColumn.getAcceleration().getX(), jacobian[6][c], epsilonA); Assert.assertEquals(estimatedColumn.getAcceleration().getY(), jacobian[7][c], epsilonA); Assert.assertEquals(estimatedColumn.getAcceleration().getZ(), jacobian[8][c], epsilonA); } } } }
From source file:org.orekit.frames.TransformTest.java
@Test public void testLinear() { RandomGenerator random = new Well19937a(0x14f6411217b148d8l); for (int n = 0; n < 100; ++n) { Transform t = randomTransform(random); // build an equivalent linear transform by extracting raw translation/rotation RealMatrix linearA = MatrixUtils.createRealMatrix(3, 4); linearA.setSubMatrix(t.getRotation().getMatrix(), 0, 0); Vector3D rt = t.getRotation().applyTo(t.getTranslation()); linearA.setEntry(0, 3, rt.getX()); linearA.setEntry(1, 3, rt.getY()); linearA.setEntry(2, 3, rt.getZ()); // build an equivalent linear transform by observing transformed points RealMatrix linearB = MatrixUtils.createRealMatrix(3, 4); Vector3D p0 = t.transformPosition(Vector3D.ZERO); Vector3D pI = t.transformPosition(Vector3D.PLUS_I).subtract(p0); Vector3D pJ = t.transformPosition(Vector3D.PLUS_J).subtract(p0); Vector3D pK = t.transformPosition(Vector3D.PLUS_K).subtract(p0); linearB.setColumn(0, new double[] { pI.getX(), pI.getY(), pI.getZ() }); linearB.setColumn(1, new double[] { pJ.getX(), pJ.getY(), pJ.getZ() }); linearB.setColumn(2, new double[] { pK.getX(), pK.getY(), pK.getZ() }); linearB.setColumn(3, new double[] { p0.getX(), p0.getY(), p0.getZ() }); // both linear transforms should be equal Assert.assertEquals(0.0, linearB.subtract(linearA).getNorm(), 1.0e-15 * linearA.getNorm()); for (int i = 0; i < 100; ++i) { Vector3D p = randomVector(1.0e3, random); Vector3D q = t.transformPosition(p); double[] qA = linearA.operate(new double[] { p.getX(), p.getY(), p.getZ(), 1.0 }); Assert.assertEquals(q.getX(), qA[0], 1.0e-13 * p.getNorm()); Assert.assertEquals(q.getY(), qA[1], 1.0e-13 * p.getNorm()); Assert.assertEquals(q.getZ(), qA[2], 1.0e-13 * p.getNorm()); double[] qB = linearB.operate(new double[] { p.getX(), p.getY(), p.getZ(), 1.0 }); Assert.assertEquals(q.getX(), qB[0], 1.0e-10 * p.getNorm()); Assert.assertEquals(q.getY(), qB[1], 1.0e-10 * p.getNorm()); Assert.assertEquals(q.getZ(), qB[2], 1.0e-10 * p.getNorm()); }// w ww. j a va2 s. c o m } }