List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D getY
public double getY()
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 w w w . j a v a 2 s . c o 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();/* w w w . j a v a2 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(); }
From source file:org.orekit.orbits.CircularOrbit.java
/** Constructor from cartesian parameters. * * <p> The acceleration provided in {@code pvCoordinates} is accessible using * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods * use {@code mu} and the position to compute the acceleration, including * {@link #shiftedBy(double)} and {@link #getPVCoordinates(AbsoluteDate, Frame)}. * * @param pvCoordinates the {@link PVCoordinates} in inertial frame * @param frame the frame in which are defined the {@link PVCoordinates} * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame}) * @param mu central attraction coefficient (m/s) * @exception IllegalArgumentException if frame is not a {@link * Frame#isPseudoInertial pseudo-inertial frame} *//*from w ww. java 2 s . co m*/ public CircularOrbit(final TimeStampedPVCoordinates pvCoordinates, final Frame frame, final double mu) throws IllegalArgumentException { super(pvCoordinates, frame, mu); // compute semi-major axis final Vector3D pvP = pvCoordinates.getPosition(); final Vector3D pvV = pvCoordinates.getVelocity(); final double r = pvP.getNorm(); final double V2 = pvV.getNormSq(); final double rV2OnMu = r * V2 / mu; if (rV2OnMu > 2) { throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS, getClass().getName()); } a = r / (2 - rV2OnMu); // compute inclination final Vector3D momentum = pvCoordinates.getMomentum(); i = Vector3D.angle(momentum, Vector3D.PLUS_K); // compute right ascension of ascending node final Vector3D node = Vector3D.crossProduct(Vector3D.PLUS_K, momentum); raan = FastMath.atan2(node.getY(), node.getX()); // 2D-coordinates in the canonical frame final double cosRaan = FastMath.cos(raan); final double sinRaan = FastMath.sin(raan); final double cosI = FastMath.cos(i); final double sinI = FastMath.sin(i); final double xP = pvP.getX(); final double yP = pvP.getY(); final double zP = pvP.getZ(); final double x2 = (xP * cosRaan + yP * sinRaan) / a; final double y2 = ((yP * cosRaan - xP * sinRaan) * cosI + zP * sinI) / a; // compute eccentricity vector final double eSE = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(mu * a); final double eCE = rV2OnMu - 1; final double e2 = eCE * eCE + eSE * eSE; final double f = eCE - e2; final double g = FastMath.sqrt(1 - e2) * eSE; final double aOnR = a / r; final double a2OnR2 = aOnR * aOnR; ex = a2OnR2 * (f * x2 + g * y2); ey = a2OnR2 * (f * y2 - g * x2); // compute latitude argument final double beta = 1 / (1 + FastMath.sqrt(1 - ex * ex - ey * ey)); alphaV = eccentricToTrue(FastMath.atan2(y2 + ey + eSE * beta * ex, x2 + ex - eSE * beta * ey)); serializePV = true; }
From source file:org.orekit.orbits.CircularOrbit.java
/** {@inheritDoc} */ protected double[][] computeJacobianMeanWrtCartesian() { final double[][] jacobian = new double[6][6]; // compute various intermediate parameters final PVCoordinates pvc = getPVCoordinates(); final Vector3D position = pvc.getPosition(); final Vector3D velocity = pvc.getVelocity(); final double x = position.getX(); final double y = position.getY(); final double z = position.getZ(); final double vx = velocity.getX(); final double vy = velocity.getY(); final double vz = velocity.getZ(); final double pv = Vector3D.dotProduct(position, velocity); final double r2 = position.getNormSq(); final double r = FastMath.sqrt(r2); final double v2 = velocity.getNormSq(); final double mu = getMu(); final double oOsqrtMuA = 1 / FastMath.sqrt(mu * a); final double rOa = r / a; final double aOr = a / r; final double aOr2 = a / r2; final double a2 = a * a; final double ex2 = ex * ex; final double ey2 = ey * ey; final double e2 = ex2 + ey2; final double epsilon = FastMath.sqrt(1 - e2); final double beta = 1 / (1 + epsilon); final double eCosE = 1 - rOa; final double eSinE = pv * oOsqrtMuA; final double cosI = FastMath.cos(i); final double sinI = FastMath.sin(i); final double cosRaan = FastMath.cos(raan); final double sinRaan = FastMath.sin(raan); // da/*from w w w .j av a2 s .c o m*/ fillHalfRow(2 * aOr * aOr2, position, jacobian[0], 0); fillHalfRow(2 * a2 / mu, velocity, jacobian[0], 3); // differentials of the normalized momentum final Vector3D danP = new Vector3D(v2, position, -pv, velocity); final Vector3D danV = new Vector3D(r2, velocity, -pv, position); final double recip = 1 / pvc.getMomentum().getNorm(); final double recip2 = recip * recip; final Vector3D dwXP = new Vector3D(recip, new Vector3D(0, vz, -vy), -recip2 * sinRaan * sinI, danP); final Vector3D dwYP = new Vector3D(recip, new Vector3D(-vz, 0, vx), recip2 * cosRaan * sinI, danP); final Vector3D dwZP = new Vector3D(recip, new Vector3D(vy, -vx, 0), -recip2 * cosI, danP); final Vector3D dwXV = new Vector3D(recip, new Vector3D(0, -z, y), -recip2 * sinRaan * sinI, danV); final Vector3D dwYV = new Vector3D(recip, new Vector3D(z, 0, -x), recip2 * cosRaan * sinI, danV); final Vector3D dwZV = new Vector3D(recip, new Vector3D(-y, x, 0), -recip2 * cosI, danV); // di fillHalfRow(sinRaan * cosI, dwXP, -cosRaan * cosI, dwYP, -sinI, dwZP, jacobian[3], 0); fillHalfRow(sinRaan * cosI, dwXV, -cosRaan * cosI, dwYV, -sinI, dwZV, jacobian[3], 3); // dRaan fillHalfRow(sinRaan / sinI, dwYP, cosRaan / sinI, dwXP, jacobian[4], 0); fillHalfRow(sinRaan / sinI, dwYV, cosRaan / sinI, dwXV, jacobian[4], 3); // orbital frame: (p, q, w) p along ascending node, w along momentum // the coordinates of the spacecraft in this frame are: (u, v, 0) final double u = x * cosRaan + y * sinRaan; final double cv = -x * sinRaan + y * cosRaan; final double v = cv * cosI + z * sinI; // du final Vector3D duP = new Vector3D(cv * cosRaan / sinI, dwXP, cv * sinRaan / sinI, dwYP, 1, new Vector3D(cosRaan, sinRaan, 0)); final Vector3D duV = new Vector3D(cv * cosRaan / sinI, dwXV, cv * sinRaan / sinI, dwYV); // dv final Vector3D dvP = new Vector3D(-u * cosRaan * cosI / sinI + sinRaan * z, dwXP, -u * sinRaan * cosI / sinI - cosRaan * z, dwYP, cv, dwZP, 1, new Vector3D(-sinRaan * cosI, cosRaan * cosI, sinI)); final Vector3D dvV = new Vector3D(-u * cosRaan * cosI / sinI + sinRaan * z, dwXV, -u * sinRaan * cosI / sinI - cosRaan * z, dwYV, cv, dwZV); final Vector3D dc1P = new Vector3D(aOr2 * (2 * eSinE * eSinE + 1 - eCosE) / r2, position, -2 * aOr2 * eSinE * oOsqrtMuA, velocity); final Vector3D dc1V = new Vector3D(-2 * aOr2 * eSinE * oOsqrtMuA, position, 2 / mu, velocity); final Vector3D dc2P = new Vector3D(aOr2 * eSinE * (eSinE * eSinE - (1 - e2)) / (r2 * epsilon), position, aOr2 * (1 - e2 - eSinE * eSinE) * oOsqrtMuA / epsilon, velocity); final Vector3D dc2V = new Vector3D(aOr2 * (1 - e2 - eSinE * eSinE) * oOsqrtMuA / epsilon, position, eSinE / (mu * epsilon), velocity); final double cof1 = aOr2 * (eCosE - e2); final double cof2 = aOr2 * epsilon * eSinE; final Vector3D dexP = new Vector3D(u, dc1P, v, dc2P, cof1, duP, cof2, dvP); final Vector3D dexV = new Vector3D(u, dc1V, v, dc2V, cof1, duV, cof2, dvV); final Vector3D deyP = new Vector3D(v, dc1P, -u, dc2P, cof1, dvP, -cof2, duP); final Vector3D deyV = new Vector3D(v, dc1V, -u, dc2V, cof1, dvV, -cof2, duV); fillHalfRow(1, dexP, jacobian[1], 0); fillHalfRow(1, dexV, jacobian[1], 3); fillHalfRow(1, deyP, jacobian[2], 0); fillHalfRow(1, deyV, jacobian[2], 3); final double cle = u / a + ex - eSinE * beta * ey; final double sle = v / a + ey + eSinE * beta * ex; final double m1 = beta * eCosE; final double m2 = 1 - m1 * eCosE; final double m3 = (u * ey - v * ex) + eSinE * beta * (u * ex + v * ey); final double m4 = -sle + cle * eSinE * beta; final double m5 = cle + sle * eSinE * beta; fillHalfRow((2 * m3 / r + aOr * eSinE + m1 * eSinE * (1 + m1 - (1 + aOr) * m2) / epsilon) / r2, position, (m1 * m2 / epsilon - 1) * oOsqrtMuA, velocity, m4, dexP, m5, deyP, -sle / a, duP, cle / a, dvP, jacobian[5], 0); fillHalfRow((m1 * m2 / epsilon - 1) * oOsqrtMuA, position, (2 * m3 + eSinE * a + m1 * eSinE * r * (eCosE * beta * 2 - aOr * m2) / epsilon) / mu, velocity, m4, dexV, m5, deyV, -sle / a, duV, cle / a, dvV, jacobian[5], 3); return jacobian; }
From source file:org.orekit.orbits.CircularParametersTest.java
@Test public void testCircularToCartesian() { double ix = 1.200e-04; double iy = -1.16e-04; double i = 2 * FastMath.asin(FastMath.sqrt((ix * ix + iy * iy) / 4)); double raan = FastMath.atan2(iy, ix); double cosRaan = FastMath.cos(raan); double sinRaan = FastMath.sin(raan); double exTilde = -7.900e-6; double eyTilde = 1.100e-4; double ex = exTilde * cosRaan + eyTilde * sinRaan; double ey = eyTilde * cosRaan - exTilde * sinRaan; CircularOrbit circ = new CircularOrbit(42166.712, ex, ey, i, raan, 5.300 - raan, PositionAngle.MEAN, FramesFactory.getEME2000(), date, mu); Vector3D pos = circ.getPVCoordinates().getPosition(); Vector3D vel = circ.getPVCoordinates().getVelocity(); // check 1/a = 2/r - V2/mu double r = pos.getNorm(); double v = vel.getNorm(); Assert.assertEquals(2 / r - v * v / mu, 1 / circ.getA(), 1.0e-7); Assert.assertEquals(0.233745668678733e+05, pos.getX(), Utils.epsilonTest * r); Assert.assertEquals(-0.350998914352669e+05, pos.getY(), Utils.epsilonTest * r); Assert.assertEquals(-0.150053723123334e+01, pos.getZ(), Utils.epsilonTest * r); Assert.assertEquals(0.809135038364960e+05, vel.getX(), Utils.epsilonTest * v); Assert.assertEquals(0.538902268252598e+05, vel.getY(), Utils.epsilonTest * v); Assert.assertEquals(0.158527938296630e+02, vel.getZ(), Utils.epsilonTest * v); }
From source file:org.orekit.orbits.EquinoctialOrbit.java
/** Constructor from cartesian parameters. * * <p> The acceleration provided in {@code pvCoordinates} is accessible using * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods * use {@code mu} and the position to compute the acceleration, including * {@link #shiftedBy(double)} and {@link #getPVCoordinates(AbsoluteDate, Frame)}. * * @param pvCoordinates the position, velocity and acceleration * @param frame the frame in which are defined the {@link PVCoordinates} * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame}) * @param mu central attraction coefficient (m/s) * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame} *///from w ww. ja va 2s . com public EquinoctialOrbit(final TimeStampedPVCoordinates pvCoordinates, final Frame frame, final double mu) throws IllegalArgumentException { super(pvCoordinates, frame, mu); // compute semi-major axis final Vector3D pvP = pvCoordinates.getPosition(); final Vector3D pvV = pvCoordinates.getVelocity(); final double r = pvP.getNorm(); final double V2 = pvV.getNormSq(); final double rV2OnMu = r * V2 / mu; if (rV2OnMu > 2) { throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS, getClass().getName()); } // compute inclination vector final Vector3D w = pvCoordinates.getMomentum().normalize(); final double d = 1.0 / (1 + w.getZ()); hx = -d * w.getY(); hy = d * w.getX(); // compute true longitude argument final double cLv = (pvP.getX() - d * pvP.getZ() * w.getX()) / r; final double sLv = (pvP.getY() - d * pvP.getZ() * w.getY()) / r; lv = FastMath.atan2(sLv, cLv); // compute semi-major axis a = r / (2 - rV2OnMu); // compute eccentricity vector final double eSE = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(mu * a); final double eCE = rV2OnMu - 1; final double e2 = eCE * eCE + eSE * eSE; final double f = eCE - e2; final double g = FastMath.sqrt(1 - e2) * eSE; ex = a * (f * cLv + g * sLv) / r; ey = a * (f * sLv - g * cLv) / r; }
From source file:org.orekit.orbits.EquinoctialParametersTest.java
@Test public void testEquinoctialToCartesian() { double ix = 1.200e-04; double iy = -1.16e-04; double inc = 2 * FastMath.asin(FastMath.sqrt((ix * ix + iy * iy) / 4.)); double hx = FastMath.tan(inc / 2.) * ix / (2 * FastMath.sin(inc / 2.)); double hy = FastMath.tan(inc / 2.) * iy / (2 * FastMath.sin(inc / 2.)); EquinoctialOrbit equi = new EquinoctialOrbit(42166.712, -7.900e-06, 1.100e-04, hx, hy, 5.300, PositionAngle.MEAN, FramesFactory.getEME2000(), date, mu); Vector3D pos = equi.getPVCoordinates().getPosition(); Vector3D vit = equi.getPVCoordinates().getVelocity(); // verif of 1/a = 2/X - V2/mu double oneovera = (2. / pos.getNorm()) - vit.getNorm() * vit.getNorm() / mu; Assert.assertEquals(oneovera, 1. / equi.getA(), 1.0e-7); Assert.assertEquals(0.233745668678733e+05, pos.getX(), Utils.epsilonTest * FastMath.abs(pos.getX())); Assert.assertEquals(-0.350998914352669e+05, pos.getY(), Utils.epsilonTest * FastMath.abs(pos.getY())); Assert.assertEquals(-0.150053723123334e+01, pos.getZ(), Utils.epsilonTest * FastMath.abs(pos.getZ())); Assert.assertEquals(0.809135038364960e+05, vit.getX(), Utils.epsilonTest * FastMath.abs(vit.getX())); Assert.assertEquals(0.538902268252598e+05, vit.getY(), Utils.epsilonTest * FastMath.abs(vit.getY())); Assert.assertEquals(0.158527938296630e+02, vit.getZ(), Utils.epsilonTest * FastMath.abs(vit.getZ())); }
From source file:org.orekit.orbits.KeplerianOrbit.java
/** Compute the Jacobian of the orbital parameters with respect to the cartesian parameters. * <p>/*www .j av a 2 s. co m*/ * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with * respect to cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3, * yDot for j=4, zDot for j=5). * </p> * @return 6x6 Jacobian matrix */ private double[][] computeJacobianMeanWrtCartesianElliptical() { final double[][] jacobian = new double[6][6]; // compute various intermediate parameters final PVCoordinates pvc = getPVCoordinates(); final Vector3D position = pvc.getPosition(); final Vector3D velocity = pvc.getVelocity(); final Vector3D momentum = pvc.getMomentum(); final double v2 = velocity.getNormSq(); final double r2 = position.getNormSq(); final double r = FastMath.sqrt(r2); final double r3 = r * r2; final double px = position.getX(); final double py = position.getY(); final double pz = position.getZ(); final double vx = velocity.getX(); final double vy = velocity.getY(); final double vz = velocity.getZ(); final double mx = momentum.getX(); final double my = momentum.getY(); final double mz = momentum.getZ(); final double mu = getMu(); final double sqrtMuA = FastMath.sqrt(a * mu); final double sqrtAoMu = FastMath.sqrt(a / mu); final double a2 = a * a; final double twoA = 2 * a; final double rOnA = r / a; final double oMe2 = 1 - e * e; final double epsilon = FastMath.sqrt(oMe2); final double sqrtRec = 1 / epsilon; final double cosI = FastMath.cos(i); final double sinI = FastMath.sin(i); final double cosPA = FastMath.cos(pa); final double sinPA = FastMath.sin(pa); final double pv = Vector3D.dotProduct(position, velocity); final double cosE = (a - r) / (a * e); final double sinE = pv / (e * sqrtMuA); // da final Vector3D vectorAR = new Vector3D(2 * a2 / r3, position); final Vector3D vectorARDot = velocity.scalarMultiply(2 * a2 / mu); fillHalfRow(1, vectorAR, jacobian[0], 0); fillHalfRow(1, vectorARDot, jacobian[0], 3); // de final double factorER3 = pv / twoA; final Vector3D vectorER = new Vector3D(cosE * v2 / (r * mu), position, sinE / sqrtMuA, velocity, -factorER3 * sinE / sqrtMuA, vectorAR); final Vector3D vectorERDot = new Vector3D(sinE / sqrtMuA, position, cosE * 2 * r / mu, velocity, -factorER3 * sinE / sqrtMuA, vectorARDot); fillHalfRow(1, vectorER, jacobian[1], 0); fillHalfRow(1, vectorERDot, jacobian[1], 3); // dE / dr (Eccentric anomaly) final double coefE = cosE / (e * sqrtMuA); final Vector3D vectorEAnR = new Vector3D(-sinE * v2 / (e * r * mu), position, coefE, velocity, -factorER3 * coefE, vectorAR); // dE / drDot final Vector3D vectorEAnRDot = new Vector3D(-sinE * 2 * r / (e * mu), velocity, coefE, position, -factorER3 * coefE, vectorARDot); // precomputing some more factors final double s1 = -sinE * pz / r - cosE * vz * sqrtAoMu; final double s2 = -cosE * pz / r3; final double s3 = -sinE * vz / (2 * sqrtMuA); final double t1 = sqrtRec * (cosE * pz / r - sinE * vz * sqrtAoMu); final double t2 = sqrtRec * (-sinE * pz / r3); final double t3 = sqrtRec * (cosE - e) * vz / (2 * sqrtMuA); final double t4 = sqrtRec * (e * sinI * cosPA * sqrtRec - vz * sqrtAoMu); final Vector3D s = new Vector3D(cosE / r, Vector3D.PLUS_K, s1, vectorEAnR, s2, position, s3, vectorAR); final Vector3D sDot = new Vector3D(-sinE * sqrtAoMu, Vector3D.PLUS_K, s1, vectorEAnRDot, s3, vectorARDot); final Vector3D t = new Vector3D(sqrtRec * sinE / r, Vector3D.PLUS_K) .add(new Vector3D(t1, vectorEAnR, t2, position, t3, vectorAR, t4, vectorER)); final Vector3D tDot = new Vector3D(sqrtRec * (cosE - e) * sqrtAoMu, Vector3D.PLUS_K, t1, vectorEAnRDot, t3, vectorARDot, t4, vectorERDot); // di final double factorI1 = -sinI * sqrtRec / sqrtMuA; final double i1 = factorI1; final double i2 = -factorI1 * mz / twoA; final double i3 = factorI1 * mz * e / oMe2; final double i4 = cosI * sinPA; final double i5 = cosI * cosPA; fillHalfRow(i1, new Vector3D(vy, -vx, 0), i2, vectorAR, i3, vectorER, i4, s, i5, t, jacobian[2], 0); fillHalfRow(i1, new Vector3D(-py, px, 0), i2, vectorARDot, i3, vectorERDot, i4, sDot, i5, tDot, jacobian[2], 3); // dpa fillHalfRow(cosPA / sinI, s, -sinPA / sinI, t, jacobian[3], 0); fillHalfRow(cosPA / sinI, sDot, -sinPA / sinI, tDot, jacobian[3], 3); // dRaan final double factorRaanR = 1 / (mu * a * oMe2 * sinI * sinI); fillHalfRow(-factorRaanR * my, new Vector3D(0, vz, -vy), factorRaanR * mx, new Vector3D(-vz, 0, vx), jacobian[4], 0); fillHalfRow(-factorRaanR * my, new Vector3D(0, -pz, py), factorRaanR * mx, new Vector3D(pz, 0, -px), jacobian[4], 3); // dM fillHalfRow(rOnA, vectorEAnR, -sinE, vectorER, jacobian[5], 0); fillHalfRow(rOnA, vectorEAnRDot, -sinE, vectorERDot, jacobian[5], 3); return jacobian; }
From source file:org.orekit.orbits.KeplerianOrbit.java
/** Compute the Jacobian of the orbital parameters with respect to the cartesian parameters. * <p>//w w w . j a v a 2s. c o m * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with * respect to cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3, * yDot for j=4, zDot for j=5). * </p> * @return 6x6 Jacobian matrix */ private double[][] computeJacobianMeanWrtCartesianHyperbolic() { final double[][] jacobian = new double[6][6]; // compute various intermediate parameters final PVCoordinates pvc = getPVCoordinates(); final Vector3D position = pvc.getPosition(); final Vector3D velocity = pvc.getVelocity(); final Vector3D momentum = pvc.getMomentum(); final double r2 = position.getNormSq(); final double r = FastMath.sqrt(r2); final double r3 = r * r2; final double x = position.getX(); final double y = position.getY(); final double z = position.getZ(); final double vx = velocity.getX(); final double vy = velocity.getY(); final double vz = velocity.getZ(); final double mx = momentum.getX(); final double my = momentum.getY(); final double mz = momentum.getZ(); final double mu = getMu(); final double absA = -a; final double sqrtMuA = FastMath.sqrt(absA * mu); final double a2 = a * a; final double rOa = r / absA; final double cosI = FastMath.cos(i); final double sinI = FastMath.sin(i); final double pv = Vector3D.dotProduct(position, velocity); // da final Vector3D vectorAR = new Vector3D(-2 * a2 / r3, position); final Vector3D vectorARDot = velocity.scalarMultiply(-2 * a2 / mu); fillHalfRow(-1, vectorAR, jacobian[0], 0); fillHalfRow(-1, vectorARDot, jacobian[0], 3); // differentials of the momentum final double m = momentum.getNorm(); final double oOm = 1 / m; final Vector3D dcXP = new Vector3D(0, vz, -vy); final Vector3D dcYP = new Vector3D(-vz, 0, vx); final Vector3D dcZP = new Vector3D(vy, -vx, 0); final Vector3D dcXV = new Vector3D(0, -z, y); final Vector3D dcYV = new Vector3D(z, 0, -x); final Vector3D dcZV = new Vector3D(-y, x, 0); final Vector3D dCP = new Vector3D(mx * oOm, dcXP, my * oOm, dcYP, mz * oOm, dcZP); final Vector3D dCV = new Vector3D(mx * oOm, dcXV, my * oOm, dcYV, mz * oOm, dcZV); // dp final double mOMu = m / mu; final Vector3D dpP = new Vector3D(2 * mOMu, dCP); final Vector3D dpV = new Vector3D(2 * mOMu, dCV); // de final double p = m * mOMu; final double moO2ae = 1 / (2 * absA * e); final double m2OaMu = -p / absA; fillHalfRow(moO2ae, dpP, m2OaMu * moO2ae, vectorAR, jacobian[1], 0); fillHalfRow(moO2ae, dpV, m2OaMu * moO2ae, vectorARDot, jacobian[1], 3); // di final double cI1 = 1 / (m * sinI); final double cI2 = cosI * cI1; fillHalfRow(cI2, dCP, -cI1, dcZP, jacobian[2], 0); fillHalfRow(cI2, dCV, -cI1, dcZV, jacobian[2], 3); // dPA final double cP1 = y * oOm; final double cP2 = -x * oOm; final double cP3 = -(mx * cP1 + my * cP2); final double cP4 = cP3 * oOm; final double cP5 = -1 / (r2 * sinI * sinI); final double cP6 = z * cP5; final double cP7 = cP3 * cP5; final Vector3D dacP = new Vector3D(cP1, dcXP, cP2, dcYP, cP4, dCP, oOm, new Vector3D(-my, mx, 0)); final Vector3D dacV = new Vector3D(cP1, dcXV, cP2, dcYV, cP4, dCV); final Vector3D dpoP = new Vector3D(cP6, dacP, cP7, Vector3D.PLUS_K); final Vector3D dpoV = new Vector3D(cP6, dacV); final double re2 = r2 * e * e; final double recOre2 = (p - r) / re2; final double resOre2 = (pv * mOMu) / re2; final Vector3D dreP = new Vector3D(mOMu, velocity, pv / mu, dCP); final Vector3D dreV = new Vector3D(mOMu, position, pv / mu, dCV); final Vector3D davP = new Vector3D(-resOre2, dpP, recOre2, dreP, resOre2 / r, position); final Vector3D davV = new Vector3D(-resOre2, dpV, recOre2, dreV); fillHalfRow(1, dpoP, -1, davP, jacobian[3], 0); fillHalfRow(1, dpoV, -1, davV, jacobian[3], 3); // dRAAN final double cO0 = cI1 * cI1; final double cO1 = mx * cO0; final double cO2 = -my * cO0; fillHalfRow(cO1, dcYP, cO2, dcXP, jacobian[4], 0); fillHalfRow(cO1, dcYV, cO2, dcXV, jacobian[4], 3); // dM final double s2a = pv / (2 * absA); final double oObux = 1 / FastMath.sqrt(m * m + mu * absA); final double scasbu = pv * oObux; final Vector3D dauP = new Vector3D(1 / sqrtMuA, velocity, -s2a / sqrtMuA, vectorAR); final Vector3D dauV = new Vector3D(1 / sqrtMuA, position, -s2a / sqrtMuA, vectorARDot); final Vector3D dbuP = new Vector3D(oObux * mu / 2, vectorAR, m * oObux, dCP); final Vector3D dbuV = new Vector3D(oObux * mu / 2, vectorARDot, m * oObux, dCV); final Vector3D dcuP = new Vector3D(oObux, velocity, -scasbu * oObux, dbuP); final Vector3D dcuV = new Vector3D(oObux, position, -scasbu * oObux, dbuV); fillHalfRow(1, dauP, -e / (1 + rOa), dcuP, jacobian[5], 0); fillHalfRow(1, dauV, -e / (1 + rOa), dcuV, jacobian[5], 3); return jacobian; }
From source file:org.orekit.orbits.KeplerianParametersTest.java
public void testKeplerianToCartesian() { KeplerianOrbit kep = new KeplerianOrbit(24464560.0, 0.7311, 0.122138, 3.10686, 1.00681, 0.048363, PositionAngle.MEAN, FramesFactory.getEME2000(), date, mu); Vector3D pos = kep.getPVCoordinates().getPosition(); Vector3D vit = kep.getPVCoordinates().getVelocity(); Assert.assertEquals(-0.107622532467967e+07, pos.getX(), Utils.epsilonTest * FastMath.abs(pos.getX())); Assert.assertEquals(-0.676589636432773e+07, pos.getY(), Utils.epsilonTest * FastMath.abs(pos.getY())); Assert.assertEquals(-0.332308783350379e+06, pos.getZ(), Utils.epsilonTest * FastMath.abs(pos.getZ())); Assert.assertEquals(0.935685775154103e+04, vit.getX(), Utils.epsilonTest * FastMath.abs(vit.getX())); Assert.assertEquals(-0.331234775037644e+04, vit.getY(), Utils.epsilonTest * FastMath.abs(vit.getY())); Assert.assertEquals(-0.118801577532701e+04, vit.getZ(), Utils.epsilonTest * FastMath.abs(vit.getZ())); }