List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D getNorm
public double getNorm()
From source file:org.orekit.orbits.CircularParametersTest.java
@Test public void testGeometryCirc() { // circular and equatorial orbit double hx = 0.1e-8; double hy = 0.1e-8; double i = 2 * FastMath.atan(FastMath.sqrt(hx * hx + hy * hy)); double raan = FastMath.atan2(hy, hx); CircularOrbit pCirEqua = new CircularOrbit(42166.712, 0.1e-8, 0.1e-8, i, raan, 0.67 - raan, PositionAngle.TRUE, FramesFactory.getEME2000(), date, mu); Vector3D position = pCirEqua.getPVCoordinates().getPosition(); Vector3D velocity = pCirEqua.getPVCoordinates().getVelocity(); Vector3D momentum = pCirEqua.getPVCoordinates().getMomentum().normalize(); double apogeeRadius = pCirEqua.getA() * (1 + pCirEqua.getE()); double perigeeRadius = pCirEqua.getA() * (1 - pCirEqua.getE()); // test if apogee equals perigee Assert.assertEquals(perigeeRadius, apogeeRadius, 1.e+4 * Utils.epsilonTest * apogeeRadius); for (double alphaV = 0; alphaV <= 2 * FastMath.PI; alphaV += 2 * FastMath.PI / 100.) { pCirEqua = new CircularOrbit(pCirEqua.getA(), pCirEqua.getCircularEx(), pCirEqua.getCircularEy(), pCirEqua.getI(), pCirEqua.getRightAscensionOfAscendingNode(), alphaV, PositionAngle.TRUE, pCirEqua.getFrame(), date, mu); position = pCirEqua.getPVCoordinates().getPosition(); // test if the norm pf the position is in the range [perigee radius, apogee radius] Assert.assertTrue((position.getNorm() - apogeeRadius) <= (apogeeRadius * Utils.epsilonTest)); Assert.assertTrue((position.getNorm() - perigeeRadius) >= (-perigeeRadius * Utils.epsilonTest)); position = position.normalize(); velocity = pCirEqua.getPVCoordinates().getVelocity(); velocity = velocity.normalize(); // at this stage of computation, all the vectors (position, velocity and momemtum) are normalized here // test of orthogonality between position and momentum Assert.assertTrue(FastMath.abs(Vector3D.dotProduct(position, momentum)) < Utils.epsilonTest); // test of orthogonality between velocity and momentum Assert.assertTrue(FastMath.abs(Vector3D.dotProduct(velocity, momentum)) < Utils.epsilonTest); }/* www .java 2s . c o m*/ }
From source file:org.orekit.orbits.CircularParametersTest.java
@Test public void testSymmetryEll() { // elliptic and non equatorail orbit Vector3D position = new Vector3D(4512.9, 18260., -5127.); Vector3D velocity = new Vector3D(134664.6, 90066.8, 72047.6); PVCoordinates pvCoordinates = new PVCoordinates(position, velocity); CircularOrbit p = new CircularOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu); Vector3D positionOffset = p.getPVCoordinates().getPosition(); Vector3D velocityOffset = p.getPVCoordinates().getVelocity(); positionOffset = positionOffset.subtract(position); velocityOffset = velocityOffset.subtract(velocity); Assert.assertEquals(0.0, positionOffset.getNorm(), position.getNorm() * Utils.epsilonTest); Assert.assertEquals(0.0, velocityOffset.getNorm(), velocity.getNorm() * Utils.epsilonTest); }
From source file:org.orekit.orbits.CircularParametersTest.java
@Test public void testSymmetryCir() { // circular and equatorial orbit Vector3D position = new Vector3D(33051.2, 26184.9, -1.3E-5); Vector3D velocity = new Vector3D(-60376.2, 76208., 2.7E-4); PVCoordinates pvCoordinates = new PVCoordinates(position, velocity); CircularOrbit p = new CircularOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu); Vector3D positionOffset = p.getPVCoordinates().getPosition().subtract(position); Vector3D velocityOffset = p.getPVCoordinates().getVelocity().subtract(velocity); Assert.assertEquals(0.0, positionOffset.getNorm(), position.getNorm() * Utils.epsilonTest); Assert.assertEquals(0.0, velocityOffset.getNorm(), velocity.getNorm() * Utils.epsilonTest); }
From source file:org.orekit.orbits.CircularParametersTest.java
@Test public void testJacobianReference() throws OrekitException { AbsoluteDate dateTca = new AbsoluteDate(2000, 04, 01, 0, 0, 0.000, TimeScalesFactory.getUTC()); double mu = 3.986004415e+14; CircularOrbit orbCir = new CircularOrbit(7000000.0, 0.01, -0.02, 1.2, 2.1, 0.7, PositionAngle.MEAN, FramesFactory.getEME2000(), dateTca, mu); // the following reference values have been computed using the free software // version 6.2 of the MSLIB fortran library by the following program: // program cir_jacobian ///*w w w . j a va 2 s . c om*/ // use mslib // implicit none // // integer, parameter :: nb = 11 // integer :: i,j // type(tm_code_retour) :: code_retour // // real(pm_reel), parameter :: mu= 3.986004415e+14_pm_reel // real(pm_reel),dimension(3)::vit_car,pos_car // type(tm_orb_cir)::cir // real(pm_reel), dimension(6,6)::jacob // real(pm_reel)::norme // // // cir%a=7000000_pm_reel // cir%ex=0.01_pm_reel // cir%ey=-0.02_pm_reel // cir%i=1.2_pm_reel // cir%gom=2.1_pm_reel // cir%pso_M=0.7_pm_reel // // call mv_cir_car(mu,cir,pos_car,vit_car,code_retour) // write(*,*)code_retour%valeur // write(*,1000)pos_car,vit_car // // // call mu_norme(pos_car,norme,code_retour) // write(*,*)norme // // call mv_car_cir (mu, pos_car, vit_car, cir, code_retour, jacob) // write(*,*)code_retour%valeur // // write(*,*)"circular = ", cir%a, cir%ex, cir%ey, cir%i, cir%gom, cir%pso_M // // do i = 1,6 // write(*,*) " ",(jacob(i,j),j=1,6) // end do // // 1000 format (6(f24.15,1x)) // end program cir_jacobian Vector3D pRef = new Vector3D(-4106905.105389204807580, 3603162.539798960555345, 4439730.167038885876536); Vector3D vRef = new Vector3D(740.132407342422994, -5308.773280141396754, 5250.338353483879473); double[][] jRef = { { -1.1535467596325562, 1.0120556393573172, 1.2470306024626943, 181.96913090864561, -1305.2162699469984, 1290.8494448855752 }, { -5.07367368325471104E-008, -1.27870567070456834E-008, 1.31544531338558113E-007, -3.09332106417043592E-005, -9.60781276304445404E-005, 1.91506964883791605E-004 }, { -6.59428471712402018E-008, 1.24561703203882533E-007, -1.41907027322388158E-008, 7.63442601186485441E-005, -1.77446722746170009E-004, 5.99464401287846734E-005 }, { 7.55079920652274275E-008, 4.41606835295069131E-008, 3.40079310688458225E-008, 7.89724635377817962E-005, 4.61868720707717372E-005, 3.55682891687782599E-005 }, { -9.20788748896973282E-008, -5.38521280004949642E-008, -4.14712660805579618E-008, 7.78626692360739821E-005, 4.55378113077967091E-005, 3.50684505810897702E-005 }, { 1.85082436324531617E-008, 1.20506219457886855E-007, -8.31277842285972640E-008, 1.27364008345789645E-004, -1.54770720974742483E-004, -1.78589436862677754E-004 } }; PVCoordinates pv = orbCir.getPVCoordinates(); Assert.assertEquals(0, pv.getPosition().subtract(pRef).getNorm(), 3.0e-16 * pRef.getNorm()); Assert.assertEquals(0, pv.getVelocity().subtract(vRef).getNorm(), 2.0e-16 * vRef.getNorm()); double[][] jacobian = new double[6][6]; orbCir.getJacobianWrtCartesian(PositionAngle.MEAN, jacobian); for (int i = 0; i < jacobian.length; i++) { double[] row = jacobian[i]; double[] rowRef = jRef[i]; for (int j = 0; j < row.length; j++) { Assert.assertEquals(0, (row[j] - rowRef[j]) / rowRef[j], 5.0e-15); } } }
From source file:org.orekit.orbits.CircularParametersTest.java
private void fillColumn(PositionAngle type, int i, CircularOrbit orbit, double hP, double[][] jacobian) { // at constant energy (i.e. constant semi major axis), we have dV = -mu dP / (V * r^2) // we use this to compute a velocity step size from the position step size Vector3D p = orbit.getPVCoordinates().getPosition(); Vector3D v = orbit.getPVCoordinates().getVelocity(); double hV = orbit.getMu() * hP / (v.getNorm() * p.getNormSq()); double h;//ww w .j a v a 2s . co m Vector3D dP = Vector3D.ZERO; Vector3D dV = Vector3D.ZERO; switch (i) { case 0: h = hP; dP = new Vector3D(hP, 0, 0); break; case 1: h = hP; dP = new Vector3D(0, hP, 0); break; case 2: h = hP; dP = new Vector3D(0, 0, hP); break; case 3: h = hV; dV = new Vector3D(hV, 0, 0); break; case 4: h = hV; dV = new Vector3D(0, hV, 0); break; default: h = hV; dV = new Vector3D(0, 0, hV); break; } CircularOrbit oM4h = new CircularOrbit( new PVCoordinates(new Vector3D(1, p, -4, dP), new Vector3D(1, v, -4, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); CircularOrbit oM3h = new CircularOrbit( new PVCoordinates(new Vector3D(1, p, -3, dP), new Vector3D(1, v, -3, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); CircularOrbit oM2h = new CircularOrbit( new PVCoordinates(new Vector3D(1, p, -2, dP), new Vector3D(1, v, -2, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); CircularOrbit oM1h = new CircularOrbit( new PVCoordinates(new Vector3D(1, p, -1, dP), new Vector3D(1, v, -1, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); CircularOrbit oP1h = new CircularOrbit( new PVCoordinates(new Vector3D(1, p, +1, dP), new Vector3D(1, v, +1, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); CircularOrbit oP2h = new CircularOrbit( new PVCoordinates(new Vector3D(1, p, +2, dP), new Vector3D(1, v, +2, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); CircularOrbit oP3h = new CircularOrbit( new PVCoordinates(new Vector3D(1, p, +3, dP), new Vector3D(1, v, +3, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); CircularOrbit oP4h = new CircularOrbit( new PVCoordinates(new Vector3D(1, p, +4, dP), new Vector3D(1, v, +4, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); jacobian[0][i] = (-3 * (oP4h.getA() - oM4h.getA()) + 32 * (oP3h.getA() - oM3h.getA()) - 168 * (oP2h.getA() - oM2h.getA()) + 672 * (oP1h.getA() - oM1h.getA())) / (840 * h); jacobian[1][i] = (-3 * (oP4h.getCircularEx() - oM4h.getCircularEx()) + 32 * (oP3h.getCircularEx() - oM3h.getCircularEx()) - 168 * (oP2h.getCircularEx() - oM2h.getCircularEx()) + 672 * (oP1h.getCircularEx() - oM1h.getCircularEx())) / (840 * h); jacobian[2][i] = (-3 * (oP4h.getCircularEy() - oM4h.getCircularEy()) + 32 * (oP3h.getCircularEy() - oM3h.getCircularEy()) - 168 * (oP2h.getCircularEy() - oM2h.getCircularEy()) + 672 * (oP1h.getCircularEy() - oM1h.getCircularEy())) / (840 * h); jacobian[3][i] = (-3 * (oP4h.getI() - oM4h.getI()) + 32 * (oP3h.getI() - oM3h.getI()) - 168 * (oP2h.getI() - oM2h.getI()) + 672 * (oP1h.getI() - oM1h.getI())) / (840 * h); jacobian[4][i] = (-3 * (oP4h.getRightAscensionOfAscendingNode() - oM4h.getRightAscensionOfAscendingNode()) + 32 * (oP3h.getRightAscensionOfAscendingNode() - oM3h.getRightAscensionOfAscendingNode()) - 168 * (oP2h.getRightAscensionOfAscendingNode() - oM2h.getRightAscensionOfAscendingNode()) + 672 * (oP1h.getRightAscensionOfAscendingNode() - oM1h.getRightAscensionOfAscendingNode())) / (840 * h); jacobian[5][i] = (-3 * (oP4h.getAlpha(type) - oM4h.getAlpha(type)) + 32 * (oP3h.getAlpha(type) - oM3h.getAlpha(type)) - 168 * (oP2h.getAlpha(type) - oM2h.getAlpha(type)) + 672 * (oP1h.getAlpha(type) - oM1h.getAlpha(type))) / (840 * h); }
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 www . ja va 2 s . c o m 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.EquinoctialParametersTest.java
@Test public void testGeometry() { // elliptic and non equatorial (i retrograde) orbit EquinoctialOrbit p = new EquinoctialOrbit(42166.712, 0.5, -0.5, 1.200, 2.1, 0.67, PositionAngle.TRUE, FramesFactory.getEME2000(), date, mu); Vector3D position = p.getPVCoordinates().getPosition(); Vector3D velocity = p.getPVCoordinates().getVelocity(); Vector3D momentum = p.getPVCoordinates().getMomentum().normalize(); double apogeeRadius = p.getA() * (1 + p.getE()); double perigeeRadius = p.getA() * (1 - p.getE()); for (double lv = 0; lv <= 2 * FastMath.PI; lv += 2 * FastMath.PI / 100.) { p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(), p.getEquinoctialEy(), p.getHx(), p.getHy(), lv, PositionAngle.TRUE, p.getFrame(), p.getDate(), p.getMu()); position = p.getPVCoordinates().getPosition(); // test if the norm of the position is in the range [perigee radius, // apogee radius] // Warning: these tests are without absolute value by choice Assert.assertTrue((position.getNorm() - apogeeRadius) <= (apogeeRadius * Utils.epsilonTest)); Assert.assertTrue((position.getNorm() - perigeeRadius) >= (-perigeeRadius * Utils.epsilonTest)); position = position.normalize(); velocity = p.getPVCoordinates().getVelocity(); velocity = velocity.normalize(); // at this stage of computation, all the vectors (position, velocity and // momemtum) are normalized here // test of orthogonality between position and momentum Assert.assertTrue(FastMath.abs(Vector3D.dotProduct(position, momentum)) < Utils.epsilonTest); // test of orthogonality between velocity and momentum Assert.assertTrue(FastMath.abs(Vector3D.dotProduct(velocity, momentum)) < Utils.epsilonTest); }//from ww w. j a va 2s .c o m // circular and equatorial orbit EquinoctialOrbit pCirEqua = new EquinoctialOrbit(42166.712, 0.1e-8, 0.1e-8, 0.1e-8, 0.1e-8, 0.67, PositionAngle.TRUE, FramesFactory.getEME2000(), date, mu); position = pCirEqua.getPVCoordinates().getPosition(); velocity = pCirEqua.getPVCoordinates().getVelocity(); momentum = Vector3D.crossProduct(position, velocity).normalize(); apogeeRadius = pCirEqua.getA() * (1 + pCirEqua.getE()); perigeeRadius = pCirEqua.getA() * (1 - pCirEqua.getE()); // test if apogee equals perigee Assert.assertEquals(perigeeRadius, apogeeRadius, 1.e+4 * Utils.epsilonTest * apogeeRadius); for (double lv = 0; lv <= 2 * FastMath.PI; lv += 2 * FastMath.PI / 100.) { pCirEqua = new EquinoctialOrbit(pCirEqua.getA(), pCirEqua.getEquinoctialEx(), pCirEqua.getEquinoctialEy(), pCirEqua.getHx(), pCirEqua.getHy(), lv, PositionAngle.TRUE, pCirEqua.getFrame(), p.getDate(), p.getMu()); position = pCirEqua.getPVCoordinates().getPosition(); // test if the norm pf the position is in the range [perigee radius, // apogee radius] Assert.assertTrue((position.getNorm() - apogeeRadius) <= (apogeeRadius * Utils.epsilonTest)); Assert.assertTrue((position.getNorm() - perigeeRadius) >= (-perigeeRadius * Utils.epsilonTest)); position = position.normalize(); velocity = pCirEqua.getPVCoordinates().getVelocity(); velocity = velocity.normalize(); // at this stage of computation, all the vectors (position, velocity and // momemtum) are normalized here // test of orthogonality between position and momentum Assert.assertTrue(FastMath.abs(Vector3D.dotProduct(position, momentum)) < Utils.epsilonTest); // test of orthogonality between velocity and momentum Assert.assertTrue(FastMath.abs(Vector3D.dotProduct(velocity, momentum)) < Utils.epsilonTest); } }
From source file:org.orekit.orbits.EquinoctialParametersTest.java
@Test public void testSymmetry() { // elliptic and non equatorial orbit Vector3D position = new Vector3D(4512.9, 18260., -5127.); Vector3D velocity = new Vector3D(134664.6, 90066.8, 72047.6); EquinoctialOrbit p = new EquinoctialOrbit(new PVCoordinates(position, velocity), FramesFactory.getEME2000(), date, mu);//from ww w . j av a 2 s.c o m Vector3D positionOffset = p.getPVCoordinates().getPosition().subtract(position); Vector3D velocityOffset = p.getPVCoordinates().getVelocity().subtract(velocity); Assert.assertTrue(positionOffset.getNorm() < Utils.epsilonTest); Assert.assertTrue(velocityOffset.getNorm() < Utils.epsilonTest); // circular and equatorial orbit position = new Vector3D(33051.2, 26184.9, -1.3E-5); velocity = new Vector3D(-60376.2, 76208., 2.7E-4); p = new EquinoctialOrbit(new PVCoordinates(position, velocity), FramesFactory.getEME2000(), date, mu); positionOffset = p.getPVCoordinates().getPosition().subtract(position); velocityOffset = p.getPVCoordinates().getVelocity().subtract(velocity); Assert.assertTrue(positionOffset.getNorm() < Utils.epsilonTest); Assert.assertTrue(velocityOffset.getNorm() < Utils.epsilonTest); }
From source file:org.orekit.orbits.EquinoctialParametersTest.java
@Test public void testJacobianReference() throws OrekitException { AbsoluteDate dateTca = new AbsoluteDate(2000, 04, 01, 0, 0, 0.000, TimeScalesFactory.getUTC()); double mu = 3.986004415e+14; EquinoctialOrbit orbEqu = new EquinoctialOrbit(7000000.0, 0.01, -0.02, 1.2, 2.1, FastMath.toRadians(40.), PositionAngle.MEAN, FramesFactory.getEME2000(), dateTca, mu); // the following reference values have been computed using the free software // version 6.2 of the MSLIB fortran library by the following program: // program equ_jacobian ///* w ww . ja v a 2s .c o m*/ // use mslib // implicit none // // integer, parameter :: nb = 11 // integer :: i,j // type(tm_code_retour) :: code_retour // // real(pm_reel), parameter :: mu= 3.986004415e+14_pm_reel // real(pm_reel),dimension(3)::vit_car,pos_car // type(tm_orb_cir_equa)::cir_equa // real(pm_reel), dimension(6,6)::jacob // real(pm_reel)::norme,hx,hy,f,dix,diy // intrinsic sqrt // // cir_equa%a=7000000_pm_reel // cir_equa%ex=0.01_pm_reel // cir_equa%ey=-0.02_pm_reel // // ! mslib cir-equ parameters use ix = 2 sin(i/2) cos(gom) and iy = 2 sin(i/2) sin(gom) // ! equinoctial parameters use hx = tan(i/2) cos(gom) and hy = tan(i/2) sin(gom) // ! the conversions between these parameters and their differentials can be computed // ! from the ratio f = 2cos(i/2) which can be found either from (ix, iy) or (hx, hy): // ! f = sqrt(4 - ix^2 - iy^2) = 2 / sqrt(1 + hx^2 + hy^2) // ! hx = ix / f, hy = iy / f // ! ix = hx * f, iy = hy *f // ! dhx = ((1 + hx^2) / f) dix + (hx hy / f) diy, dhy = (hx hy / f) dix + ((1 + hy^2) /f) diy // ! dix = ((1 - ix^2 / 4) f dhx - (ix iy / 4) f dhy, diy = -(ix iy / 4) f dhx + (1 - iy^2 / 4) f dhy // hx=1.2_pm_reel // hy=2.1_pm_reel // f=2_pm_reel/sqrt(1+hx*hx+hy*hy) // cir_equa%ix=hx*f // cir_equa%iy=hy*f // // cir_equa%pso_M=40_pm_reel*pm_deg_rad // // call mv_cir_equa_car(mu,cir_equa,pos_car,vit_car,code_retour) // write(*,*)code_retour%valeur // write(*,1000)pos_car,vit_car // // // call mu_norme(pos_car,norme,code_retour) // write(*,*)norme // // call mv_car_cir_equa (mu, pos_car, vit_car, cir_equa, code_retour, jacob) // write(*,*)code_retour%valeur // // f=sqrt(4_pm_reel-cir_equa%ix*cir_equa%ix-cir_equa%iy*cir_equa%iy) // hx=cir_equa%ix/f // hy=cir_equa%iy/f // write(*,*)"ix = ", cir_equa%ix, ", iy = ", cir_equa%iy // write(*,*)"equinoctial = ", cir_equa%a, cir_equa%ex, cir_equa%ey, hx, hy, cir_equa%pso_M*pm_rad_deg // // do j = 1,6 // dix=jacob(4,j) // diy=jacob(5,j) // jacob(4,j)=((1_pm_reel+hx*hx)*dix+(hx*hy)*diy)/f // jacob(5,j)=((hx*hy)*dix+(1_pm_reel+hy*hy)*diy)/f // end do // // do i = 1,6 // write(*,*) " ",(jacob(i,j),j=1,6) // end do // // 1000 format (6(f24.15,1x)) // end program equ_jacobian Vector3D pRef = new Vector3D(2004367.298657628707588, 6575317.978060320019722, -1518024.843913963763043); Vector3D vRef = new Vector3D(5574.048661495634406, -368.839015744295409, 5009.529487849066754); double[][] jRef = { { 0.56305379787310628, 1.8470954710993663, -0.42643364527246025, 1370.4369387322224, -90.682848736736688, 1231.6441195141242 }, { 9.52434720041122055E-008, 9.49704503778007296E-008, 4.46607520107935678E-008, 1.69704446323098610E-004, 7.05603505855828105E-005, 1.14825140460141970E-004 }, { -5.41784097802642701E-008, 9.54903765833015538E-008, -8.95815777332234450E-008, 1.01864980963344096E-004, -1.03194262242761416E-004, 1.40668700715197768E-004 }, { 1.96680305426455816E-007, -1.12388745957974467E-007, -2.27118924123407353E-007, 2.06472886488132167E-004, -1.17984506564646906E-004, -2.38427023682723818E-004 }, { -2.24382495052235118E-007, 1.28218568601277626E-007, 2.59108357381747656E-007, 1.89034327703662092E-004, -1.08019615830663994E-004, -2.18289640324466583E-004 }, { -3.04001022071876804E-007, 1.22214683774559989E-007, 1.35141804810132761E-007, -1.34034616931480536E-004, -2.14283975204169379E-004, 1.29018773893081404E-004 } }; PVCoordinates pv = orbEqu.getPVCoordinates(); Assert.assertEquals(0, pv.getPosition().subtract(pRef).getNorm(), 2.0e-16 * pRef.getNorm()); Assert.assertEquals(0, pv.getVelocity().subtract(vRef).getNorm(), 2.0e-16 * vRef.getNorm()); double[][] jacobian = new double[6][6]; orbEqu.getJacobianWrtCartesian(PositionAngle.MEAN, jacobian); for (int i = 0; i < jacobian.length; i++) { double[] row = jacobian[i]; double[] rowRef = jRef[i]; for (int j = 0; j < row.length; j++) { Assert.assertEquals(0, (row[j] - rowRef[j]) / rowRef[j], 4.0e-15); } } }