List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D getZ
public double getZ()
From source file:org.orekit.frames.Transform.java
/** Compute the Jacobian of the {@link #transformPVCoordinates(PVCoordinates)} * method of the transform.// w ww.j a v a 2 s .co m * <p> * Element {@code jacobian[i][j]} is the derivative of Cartesian coordinate i * of the transformed {@link PVCoordinates} with respect to Cartesian coordinate j * of the input {@link PVCoordinates} in method {@link #transformPVCoordinates(PVCoordinates)}. * </p> * <p> * This definition implies that if we define position-velocity coordinates * <pre> * PV? = transform.transformPVCoordinates(PV), then * </pre> * their differentials dPV? and dPV will obey the following relation * where J is the matrix computed by this method:<br/> * <pre> * dPV? = J × dPV * </pre> * </p> * @param selector selector specifying the size of the upper left corner that must be filled * (either 3x3 for positions only, 6x6 for positions and velocities, 9x9 for positions, * velocities and accelerations) * @param jacobian placeholder matrix whose upper-left corner is to be filled with * the Jacobian, the rest of the matrix remaining untouched */ public void getJacobian(final CartesianDerivativesFilter selector, final double[][] jacobian) { // elementary matrix for rotation final double[][] mData = angular.getRotation().getMatrix(); // dP1/dP0 System.arraycopy(mData[0], 0, jacobian[0], 0, 3); System.arraycopy(mData[1], 0, jacobian[1], 0, 3); System.arraycopy(mData[2], 0, jacobian[2], 0, 3); if (selector.getMaxOrder() >= 1) { // dP1/dV0 Arrays.fill(jacobian[0], 3, 6, 0.0); Arrays.fill(jacobian[1], 3, 6, 0.0); Arrays.fill(jacobian[2], 3, 6, 0.0); // dV1/dP0 final Vector3D o = angular.getRotationRate(); final double ox = o.getX(); final double oy = o.getY(); final double oz = o.getZ(); for (int i = 0; i < 3; ++i) { jacobian[3][i] = -(oy * mData[2][i] - oz * mData[1][i]); jacobian[4][i] = -(oz * mData[0][i] - ox * mData[2][i]); jacobian[5][i] = -(ox * mData[1][i] - oy * mData[0][i]); } // dV1/dV0 System.arraycopy(mData[0], 0, jacobian[3], 3, 3); System.arraycopy(mData[1], 0, jacobian[4], 3, 3); System.arraycopy(mData[2], 0, jacobian[5], 3, 3); if (selector.getMaxOrder() >= 2) { // dP1/dA0 Arrays.fill(jacobian[0], 6, 9, 0.0); Arrays.fill(jacobian[1], 6, 9, 0.0); Arrays.fill(jacobian[2], 6, 9, 0.0); // dV1/dA0 Arrays.fill(jacobian[3], 6, 9, 0.0); Arrays.fill(jacobian[4], 6, 9, 0.0); Arrays.fill(jacobian[5], 6, 9, 0.0); // dA1/dP0 final Vector3D oDot = angular.getRotationAcceleration(); final double oDotx = oDot.getX(); final double oDoty = oDot.getY(); final double oDotz = oDot.getZ(); for (int i = 0; i < 3; ++i) { jacobian[6][i] = -(oDoty * mData[2][i] - oDotz * mData[1][i]) - (oy * jacobian[5][i] - oz * jacobian[4][i]); jacobian[7][i] = -(oDotz * mData[0][i] - oDotx * mData[2][i]) - (oz * jacobian[3][i] - ox * jacobian[5][i]); jacobian[8][i] = -(oDotx * mData[1][i] - oDoty * mData[0][i]) - (ox * jacobian[4][i] - oy * jacobian[3][i]); } // dA1/dV0 for (int i = 0; i < 3; ++i) { jacobian[6][i + 3] = -2 * (oy * mData[2][i] - oz * mData[1][i]); jacobian[7][i + 3] = -2 * (oz * mData[0][i] - ox * mData[2][i]); jacobian[8][i + 3] = -2 * (ox * mData[1][i] - oy * mData[0][i]); } // dA1/dA0 System.arraycopy(mData[0], 0, jacobian[6], 6, 3); System.arraycopy(mData[1], 0, jacobian[7], 6, 3); System.arraycopy(mData[2], 0, jacobian[8], 6, 3); } } }
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 w w . j av a 2s. c o m } }
From source file:org.orekit.frames.TransformTest.java
@Test public void testShiftDerivatives() { RandomGenerator random = new Well19937a(0x5acda4f605aadce7l); for (int i = 0; i < 10; ++i) { Transform t = randomTransform(random); for (double dt = -10.0; dt < 10.0; dt += 0.125) { Transform t0 = t.shiftedBy(dt); double v = t0.getVelocity().getNorm(); double a = t0.getAcceleration().getNorm(); double omega = t0.getRotationRate().getNorm(); double omegaDot = t0.getRotationAcceleration().getNorm(); // numerical derivatives double h = 0.01 / omega; Transform tm4h = t.shiftedBy(dt - 4 * h); Transform tm3h = t.shiftedBy(dt - 3 * h); Transform tm2h = t.shiftedBy(dt - 2 * h); Transform tm1h = t.shiftedBy(dt - 1 * h); Transform tp1h = t.shiftedBy(dt + 1 * h); Transform tp2h = t.shiftedBy(dt + 2 * h); Transform tp3h = t.shiftedBy(dt + 3 * h); Transform tp4h = t.shiftedBy(dt + 4 * h); double numXDot = derivative(h, tm4h.getTranslation().getX(), tm3h.getTranslation().getX(), tm2h.getTranslation().getX(), tm1h.getTranslation().getX(), tp1h.getTranslation().getX(), tp2h.getTranslation().getX(), tp3h.getTranslation().getX(), tp4h.getTranslation().getX()); double numYDot = derivative(h, tm4h.getTranslation().getY(), tm3h.getTranslation().getY(), tm2h.getTranslation().getY(), tm1h.getTranslation().getY(), tp1h.getTranslation().getY(), tp2h.getTranslation().getY(), tp3h.getTranslation().getY(), tp4h.getTranslation().getY()); double numZDot = derivative(h, tm4h.getTranslation().getZ(), tm3h.getTranslation().getZ(), tm2h.getTranslation().getZ(), tm1h.getTranslation().getZ(), tp1h.getTranslation().getZ(), tp2h.getTranslation().getZ(), tp3h.getTranslation().getZ(), tp4h.getTranslation().getZ()); double numXDot2 = derivative(h, tm4h.getVelocity().getX(), tm3h.getVelocity().getX(), tm2h.getVelocity().getX(), tm1h.getVelocity().getX(), tp1h.getVelocity().getX(), tp2h.getVelocity().getX(), tp3h.getVelocity().getX(), tp4h.getVelocity().getX()); double numYDot2 = derivative(h, tm4h.getVelocity().getY(), tm3h.getVelocity().getY(), tm2h.getVelocity().getY(), tm1h.getVelocity().getY(), tp1h.getVelocity().getY(), tp2h.getVelocity().getY(), tp3h.getVelocity().getY(), tp4h.getVelocity().getY()); double numZDot2 = derivative(h, tm4h.getVelocity().getZ(), tm3h.getVelocity().getZ(), tm2h.getVelocity().getZ(), tm1h.getVelocity().getZ(), tp1h.getVelocity().getZ(), tp2h.getVelocity().getZ(), tp3h.getVelocity().getZ(), tp4h.getVelocity().getZ()); double numQ0Dot = derivative(h, tm4h.getRotation().getQ0(), tm3h.getRotation().getQ0(), tm2h.getRotation().getQ0(), tm1h.getRotation().getQ0(), tp1h.getRotation().getQ0(), tp2h.getRotation().getQ0(), tp3h.getRotation().getQ0(), tp4h.getRotation().getQ0()); double numQ1Dot = derivative(h, tm4h.getRotation().getQ1(), tm3h.getRotation().getQ1(), tm2h.getRotation().getQ1(), tm1h.getRotation().getQ1(), tp1h.getRotation().getQ1(), tp2h.getRotation().getQ1(), tp3h.getRotation().getQ1(), tp4h.getRotation().getQ1()); double numQ2Dot = derivative(h, tm4h.getRotation().getQ2(), tm3h.getRotation().getQ2(), tm2h.getRotation().getQ2(), tm1h.getRotation().getQ2(), tp1h.getRotation().getQ2(), tp2h.getRotation().getQ2(), tp3h.getRotation().getQ2(), tp4h.getRotation().getQ2()); double numQ3Dot = derivative(h, tm4h.getRotation().getQ3(), tm3h.getRotation().getQ3(), tm2h.getRotation().getQ3(), tm1h.getRotation().getQ3(), tp1h.getRotation().getQ3(), tp2h.getRotation().getQ3(), tp3h.getRotation().getQ3(), tp4h.getRotation().getQ3()); double numOxDot = derivative(h, tm4h.getRotationRate().getX(), tm3h.getRotationRate().getX(), tm2h.getRotationRate().getX(), tm1h.getRotationRate().getX(), tp1h.getRotationRate().getX(), tp2h.getRotationRate().getX(), tp3h.getRotationRate().getX(), tp4h.getRotationRate().getX()); double numOyDot = derivative(h, tm4h.getRotationRate().getY(), tm3h.getRotationRate().getY(), tm2h.getRotationRate().getY(), tm1h.getRotationRate().getY(), tp1h.getRotationRate().getY(), tp2h.getRotationRate().getY(), tp3h.getRotationRate().getY(), tp4h.getRotationRate().getY()); double numOzDot = derivative(h, tm4h.getRotationRate().getZ(), tm3h.getRotationRate().getZ(), tm2h.getRotationRate().getZ(), tm1h.getRotationRate().getZ(), tp1h.getRotationRate().getZ(), tp2h.getRotationRate().getZ(), tp3h.getRotationRate().getZ(), tp4h.getRotationRate().getZ()); // theoretical derivatives double theXDot = t0.getVelocity().getX(); double theYDot = t0.getVelocity().getY(); double theZDot = t0.getVelocity().getZ(); double theXDot2 = t0.getAcceleration().getX(); double theYDot2 = t0.getAcceleration().getY(); double theZDot2 = t0.getAcceleration().getZ(); Rotation r0 = t0.getRotation(); Vector3D w = t0.getRotationRate(); Vector3D q = new Vector3D(r0.getQ1(), r0.getQ2(), r0.getQ3()); Vector3D qw = Vector3D.crossProduct(q, w); double theQ0Dot = -0.5 * Vector3D.dotProduct(q, w); double theQ1Dot = 0.5 * (r0.getQ0() * w.getX() + qw.getX()); double theQ2Dot = 0.5 * (r0.getQ0() * w.getY() + qw.getY()); double theQ3Dot = 0.5 * (r0.getQ0() * w.getZ() + qw.getZ()); double theOxDot2 = t0.getRotationAcceleration().getX(); double theOyDot2 = t0.getRotationAcceleration().getY(); double theOzDot2 = t0.getRotationAcceleration().getZ(); // check consistency Assert.assertEquals(theXDot, numXDot, 1.0e-13 * v); Assert.assertEquals(theYDot, numYDot, 1.0e-13 * v); Assert.assertEquals(theZDot, numZDot, 1.0e-13 * v); Assert.assertEquals(theXDot2, numXDot2, 1.0e-13 * a); Assert.assertEquals(theYDot2, numYDot2, 1.0e-13 * a); Assert.assertEquals(theZDot2, numZDot2, 1.0e-13 * a); Assert.assertEquals(theQ0Dot, numQ0Dot, 1.0e-13 * omega); Assert.assertEquals(theQ1Dot, numQ1Dot, 1.0e-13 * omega); Assert.assertEquals(theQ2Dot, numQ2Dot, 1.0e-13 * omega); Assert.assertEquals(theQ3Dot, numQ3Dot, 1.0e-13 * omega); Assert.assertEquals(theOxDot2, numOxDot, 1.0e-12 * omegaDot); Assert.assertEquals(theOyDot2, numOyDot, 1.0e-12 * omegaDot); Assert.assertEquals(theOzDot2, numOzDot, 1.0e-12 * omegaDot); }/*from w w w. ja v a2 s . co m*/ } }
From source file:org.orekit.models.earth.GeoMagneticElements.java
/** Construct a new element with the given field vector. The other elements * of the magnetic field are calculated from the field vector. * @param b the magnetic field vector//from w w w .ja v a 2s . c o m */ public GeoMagneticElements(final Vector3D b) { this.b = b; horizontalIntensity = FastMath.hypot(b.getX(), b.getY()); totalIntensity = b.getNorm(); declination = FastMath.toDegrees(FastMath.atan2(b.getY(), b.getX())); inclination = FastMath.toDegrees(FastMath.atan2(b.getZ(), horizontalIntensity)); }
From source file:org.orekit.models.earth.GeoMagneticField.java
/** Rotate the magnetic vectors to geodetic coordinates. * @param sph the spherical coordinates// ww w.j a v a2 s. c o m * @param gp the geodetic point * @param field the magnetic field in spherical coordinates * @return the magnetic field in geodetic coordinates */ private Vector3D rotateMagneticVector(final SphericalCoordinates sph, final GeodeticPoint gp, final Vector3D field) { // difference between the spherical and geodetic latitudes final double psi = sph.phi - gp.getLatitude(); // rotate spherical field components to the geodetic system final double Bz = field.getX() * FastMath.sin(psi) + field.getZ() * FastMath.cos(psi); final double Bx = field.getX() * FastMath.cos(psi) - field.getZ() * FastMath.sin(psi); final double By = field.getY(); return new Vector3D(Bx, By, Bz); }
From source file:org.orekit.models.earth.GeoMagneticFieldTest.java
@Test public void testIGRF() throws Exception { // test values from sample coordinate file // provided as part of the geomag 7.0 distribution available at // http://www.ngdc.noaa.gov/IAGA/vmod/igrf.html // modification: the julian day calculation of geomag is slightly different // to the one from the WMM code, we use the WMM convention thus the outputs // have been adapted. runSampleFile(FieldModel.IGRF, "sample_coords.txt", "sample_out_IGRF12.txt"); final double eps = 1e-1; final double degreeEps = 1e-2; for (int i = 0; i < igrfTestValues.length; i++) { final GeoMagneticField model = GeoMagneticFieldFactory.getIGRF(igrfTestValues[i][0]); final GeoMagneticElements result = model.calculateField(igrfTestValues[i][2], igrfTestValues[i][3], igrfTestValues[i][1]);/* ww w . j ava2 s .co m*/ final Vector3D b = result.getFieldVector(); // X Assert.assertEquals(igrfTestValues[i][4], b.getX(), eps); // Y Assert.assertEquals(igrfTestValues[i][5], b.getY(), eps); // Z Assert.assertEquals(igrfTestValues[i][6], b.getZ(), eps); // H Assert.assertEquals(igrfTestValues[i][7], result.getHorizontalIntensity(), eps); // F Assert.assertEquals(igrfTestValues[i][8], result.getTotalIntensity(), eps); // inclination Assert.assertEquals(igrfTestValues[i][9], result.getInclination(), degreeEps); // declination Assert.assertEquals(igrfTestValues[i][10], result.getDeclination(), degreeEps); } }
From source file:org.orekit.models.earth.tessellation.AlongTrackAiming.java
/** {@inheritDoc} */ @Override//from w w w . j av a2 s . c o m public Vector3D alongTileDirection(final Vector3D point, final GeodeticPoint gp) throws OrekitException { final double lStart = halfTrack.get(0).getFirst().getLatitude(); final double lEnd = halfTrack.get(halfTrack.size() - 1).getFirst().getLatitude(); // check the point can be reached if (gp.getLatitude() < FastMath.min(lStart, lEnd) || gp.getLatitude() > FastMath.max(lStart, lEnd)) { throw new OrekitException(OrekitMessages.OUT_OF_RANGE_LATITUDE, FastMath.toDegrees(gp.getLatitude()), FastMath.toDegrees(FastMath.min(lStart, lEnd)), FastMath.toDegrees(FastMath.max(lStart, lEnd))); } // bracket the point in the half track sample int iInf = 0; int iSup = halfTrack.size() - 1; while (iSup - iInf > 1) { final int iMiddle = (iSup + iInf) / 2; if ((lStart < lEnd) ^ (halfTrack.get(iMiddle).getFirst().getLatitude() > gp.getLatitude())) { // the specified latitude is in the second half iInf = iMiddle; } else { // the specified latitude is in the first half iSup = iMiddle; } } // ensure we can get points at iStart, iStart + 1, iStart + 2 and iStart + 3 final int iStart = FastMath.max(0, FastMath.min(iInf - 1, halfTrack.size() - 4)); // interpolate ground sliding point at specified latitude final HermiteInterpolator interpolator = new HermiteInterpolator(); for (int i = iStart; i < iStart + 4; ++i) { final Vector3D position = halfTrack.get(i).getSecond().getPosition(); final Vector3D velocity = halfTrack.get(i).getSecond().getVelocity(); interpolator.addSamplePoint(halfTrack.get(i).getFirst().getLatitude(), new double[] { position.getX(), position.getY(), position.getZ(), velocity.getX(), velocity.getY(), velocity.getZ() }); } final DerivativeStructure[] p = interpolator.value(new DerivativeStructure(1, 1, 0, gp.getLatitude())); // extract interpolated ground position/velocity final Vector3D position = new Vector3D(p[0].getValue(), p[1].getValue(), p[2].getValue()); final Vector3D velocity = new Vector3D(p[3].getValue(), p[4].getValue(), p[5].getValue()); // adjust longitude to match the specified one final Rotation rotation = new Rotation(Vector3D.PLUS_K, position, Vector3D.PLUS_K, point); final Vector3D fixedVelocity = rotation.applyTo(velocity); // the tile direction is aligned with sliding point velocity return fixedVelocity.normalize(); }
From source file:org.orekit.orbits.CartesianOrbit.java
/** {@inheritDoc} */ public double getHx() { final Vector3D w = getPVCoordinates().getMomentum().normalize(); // Check for equatorial retrograde orbit if (((w.getX() * w.getX() + w.getY() * w.getY()) == 0) && w.getZ() < 0) { return Double.NaN; }/* w w w. ja v a 2s . co m*/ return -w.getY() / (1 + w.getZ()); }
From source file:org.orekit.orbits.CartesianOrbit.java
/** {@inheritDoc} */ public double getHy() { final Vector3D w = getPVCoordinates().getMomentum().normalize(); // Check for equatorial retrograde orbit if (((w.getX() * w.getX() + w.getY() * w.getY()) == 0) && w.getZ() < 0) { return Double.NaN; }/*from www .j a v a2 s . co m*/ return w.getX() / (1 + w.getZ()); }
From source file:org.orekit.orbits.CartesianOrbit.java
/** {@inheritDoc} */ public void addKeplerContribution(final PositionAngle type, final double gm, final double[] pDot) { final PVCoordinates pv = getPVCoordinates(); // position derivative is velocity final Vector3D velocity = pv.getVelocity(); pDot[0] += velocity.getX();/*from w w w. ja va 2 s . c om*/ pDot[1] += velocity.getY(); pDot[2] += velocity.getZ(); // velocity derivative is Newtonian acceleration final Vector3D position = pv.getPosition(); final double r2 = position.getNormSq(); final double coeff = -gm / (r2 * FastMath.sqrt(r2)); pDot[3] += coeff * position.getX(); pDot[4] += coeff * position.getY(); pDot[5] += coeff * position.getZ(); }