List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D PLUS_K
Vector3D PLUS_K
To view the source code for org.apache.commons.math3.geometry.euclidean.threed Vector3D PLUS_K.
Click Source Link
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.TopocentricFrameTest.java
@Test public void testIssue145() throws OrekitException { Frame itrf2005 = FramesFactory.getITRF(IERSConventions.IERS_2010, true); BodyShape earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS, Constants.WGS84_EARTH_FLATTENING, itrf2005); TopocentricFrame staFrame = new TopocentricFrame(earth, new GeodeticPoint(0.0, 0.0, 0.0), "test"); GeodeticPoint gp = staFrame.computeLimitVisibilityPoint(Constants.WGS84_EARTH_EQUATORIAL_RADIUS + 600000, 0.0, FastMath.toRadians(5.0)); Assert.assertEquals(0.0, gp.getLongitude(), 1.0e-15); Assert.assertTrue(gp.getLatitude() > 0); Assert.assertEquals(0.0, staFrame.getNorth().distance(Vector3D.PLUS_K), 1.0e-15); }
From source file:org.orekit.frames.TransformTest.java
@Test public void testSimpleComposition() { Transform transform = new Transform(AbsoluteDate.J2000_EPOCH, new Transform(AbsoluteDate.J2000_EPOCH, new Rotation(Vector3D.PLUS_K, 0.5 * FastMath.PI)), new Transform(AbsoluteDate.J2000_EPOCH, Vector3D.PLUS_I)); Vector3D u = transform.transformPosition(new Vector3D(1.0, 1.0, 1.0)); Vector3D v = new Vector3D(0.0, 1.0, 1.0); Assert.assertEquals(0, u.subtract(v).getNorm(), 1.0e-15); }
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);//from w w w .ja v a 2 s . com 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 ww w.j av a 2 s . co m } 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; }// www .ja v a 2 s. c om } 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; }/*from w w w .j a va 2 s. co m*/ } 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 v a2 s . c o m } }
From source file:org.orekit.frames.TransformTest.java
@Test public void testShift() { // the following transform corresponds to a frame moving along the line x=1 and rotating around its -z axis // the linear motion velocity is (0, +1, 0), the angular rate is PI/2 // at t = -1 the frame origin is at (1, -1, 0), its X axis is equal to Xref and its Y axis is equal to Yref // at t = 0 the frame origin is at (1, 0, 0), its X axis is equal to -Yref and its Y axis is equal to Xref // at t = +1 the frame origin is at (1, +1, 0), its X axis is equal to -Xref and its Y axis is equal to -Yref AbsoluteDate date = AbsoluteDate.GALILEO_EPOCH; double alpha0 = 0.5 * FastMath.PI; double omega = 0.5 * FastMath.PI; Transform t = new Transform(date, new Transform(date, Vector3D.MINUS_I, Vector3D.MINUS_J, Vector3D.ZERO), new Transform(date, new Rotation(Vector3D.PLUS_K, alpha0), new Vector3D(omega, Vector3D.MINUS_K))); for (double dt = -10.0; dt < 10.0; dt += 0.125) { Transform shifted = t.shiftedBy(dt); // the following point should always remain at moving frame origin PVCoordinates expectedFixedPoint = shifted.transformPVCoordinates( new PVCoordinates(new Vector3D(1, dt, 0), Vector3D.PLUS_J, Vector3D.ZERO)); checkVector(expectedFixedPoint.getPosition(), Vector3D.ZERO, 1.0e-14); checkVector(expectedFixedPoint.getVelocity(), Vector3D.ZERO, 1.0e-14); checkVector(expectedFixedPoint.getAcceleration(), Vector3D.ZERO, 1.0e-14); // fixed frame origin apparent motion in moving frame PVCoordinates expectedApparentMotion = shifted.transformPVCoordinates(PVCoordinates.ZERO); double c = FastMath.cos(alpha0 + omega * dt); double s = FastMath.sin(alpha0 + omega * dt); Vector3D referencePosition = new Vector3D(-c + dt * s, -s - dt * c, 0); Vector3D referenceVelocity = new Vector3D((1 + omega) * s + dt * omega * c, -(1 + omega) * c + dt * omega * s, 0); Vector3D referenceAcceleration = new Vector3D(omega * (2 + omega) * c - dt * omega * omega * s, omega * (2 + omega) * s + dt * omega * omega * c, 0); checkVector(expectedApparentMotion.getPosition(), referencePosition, 1.0e-14); checkVector(expectedApparentMotion.getVelocity(), referenceVelocity, 1.0e-14); checkVector(expectedApparentMotion.getAcceleration(), referenceAcceleration, 1.0e-14); }// www . j av a 2 s . c om }