List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D getY
public double getY()
From source file:org.orekit.files.sp3.SP3ParserTest.java
private void checkPVEntry(final PVCoordinates expected, final PVCoordinates actual) { final Vector3D expectedPos = expected.getPosition(); final Vector3D expectedVel = expected.getVelocity(); final Vector3D actualPos = actual.getPosition(); final Vector3D actualVel = actual.getVelocity(); // sp3 files can have mm accuracy final double eps = 1e-3; Assert.assertEquals(expectedPos.getX(), actualPos.getX(), eps); Assert.assertEquals(expectedPos.getY(), actualPos.getY(), eps); Assert.assertEquals(expectedPos.getZ(), actualPos.getZ(), eps); Assert.assertEquals(expectedVel.getX(), actualVel.getX(), eps); Assert.assertEquals(expectedVel.getY(), actualVel.getY(), eps); Assert.assertEquals(expectedVel.getZ(), actualVel.getZ(), eps); }
From source file:org.orekit.forces.BoxAndSolarArraySpacecraftTest.java
@Test public void testBestPointing() throws OrekitException { AbsoluteDate initialDate = propagator.getInitialState().getDate(); CelestialBody sun = CelestialBodyFactory.getSun(); BoxAndSolarArraySpacecraft s = new BoxAndSolarArraySpacecraft(1.5, 3.5, 2.5, sun, 20.0, Vector3D.PLUS_J, 0.0, 0.0, 0.0);/*w w w. ja v a 2 s . com*/ for (double dt = 0; dt < 4000; dt += 60) { SpacecraftState state = propagator.propagate(initialDate.shiftedBy(dt)); Vector3D sunInert = sun.getPVCoordinates(initialDate, state.getFrame()).getPosition(); Vector3D momentum = state.getPVCoordinates().getMomentum(); double sunElevation = FastMath.PI / 2 - Vector3D.angle(sunInert, momentum); Assert.assertEquals(15.1, FastMath.toDegrees(sunElevation), 0.1); Vector3D n = s.getNormal(state.getDate(), state.getFrame(), state.getPVCoordinates().getPosition(), state.getAttitude().getRotation()); Assert.assertEquals(0.0, n.getY(), 1.0e-10); // normal misalignment should be entirely due to sun being out of orbital plane Vector3D sunSat = state.getAttitude().getRotation().applyTo(sunInert); double misAlignment = Vector3D.angle(sunSat, n); Assert.assertEquals(sunElevation, misAlignment, 1.0e-3); } }
From source file:org.orekit.forces.BoxAndSolarArraySpacecraftTest.java
@Test public void testCorrectFixedRate() throws OrekitException { AbsoluteDate initialDate = propagator.getInitialState().getDate(); CelestialBody sun = CelestialBodyFactory.getSun(); BoxAndSolarArraySpacecraft s = new BoxAndSolarArraySpacecraft(1.5, 3.5, 2.5, sun, 20.0, Vector3D.PLUS_J, initialDate, new Vector3D(0.46565509814462996, 0.0, 0.884966287251619), propagator.getInitialState().getKeplerianMeanMotion(), 0.0, 0.0, 0.0); for (double dt = 0; dt < 4000; dt += 60) { SpacecraftState state = propagator.propagate(initialDate.shiftedBy(dt)); Vector3D sunInert = sun.getPVCoordinates(initialDate, state.getFrame()).getPosition(); Vector3D momentum = state.getPVCoordinates().getMomentum(); double sunElevation = FastMath.PI / 2 - Vector3D.angle(sunInert, momentum); Assert.assertEquals(15.1, FastMath.toDegrees(sunElevation), 0.1); Vector3D n = s.getNormal(state.getDate(), state.getFrame(), state.getPVCoordinates().getPosition(), state.getAttitude().getRotation()); Assert.assertEquals(0.0, n.getY(), 1.0e-10); // normal misalignment should be entirely due to sun being out of orbital plane Vector3D sunSat = state.getAttitude().getRotation().applyTo(sunInert); double misAlignment = Vector3D.angle(sunSat, n); Assert.assertEquals(sunElevation, misAlignment, 1.0e-3); }//from ww w.ja va 2 s . c o m }
From source file:org.orekit.forces.BoxAndSolarArraySpacecraftTest.java
@Test public void testTooSlowFixedRate() throws OrekitException { AbsoluteDate initialDate = propagator.getInitialState().getDate(); CelestialBody sun = CelestialBodyFactory.getSun(); BoxAndSolarArraySpacecraft s = new BoxAndSolarArraySpacecraft(1.5, 3.5, 2.5, sun, 20.0, Vector3D.PLUS_J, initialDate, new Vector3D(0.46565509814462996, 0.0, 0.884966287251619), 0.1 * propagator.getInitialState().getKeplerianMeanMotion(), 0.0, 0.0, 0.0); double maxDelta = 0; for (double dt = 0; dt < 4000; dt += 60) { SpacecraftState state = propagator.propagate(initialDate.shiftedBy(dt)); Vector3D sunInert = sun.getPVCoordinates(initialDate, state.getFrame()).getPosition(); Vector3D momentum = state.getPVCoordinates().getMomentum(); double sunElevation = FastMath.PI / 2 - Vector3D.angle(sunInert, momentum); Assert.assertEquals(15.1, FastMath.toDegrees(sunElevation), 0.1); Vector3D n = s.getNormal(state.getDate(), state.getFrame(), state.getPVCoordinates().getPosition(), state.getAttitude().getRotation()); Assert.assertEquals(0.0, n.getY(), 1.0e-10); // normal misalignment should become very large as solar array rotation is plain wrong Vector3D sunSat = state.getAttitude().getRotation().applyTo(sunInert); double misAlignment = Vector3D.angle(sunSat, n); maxDelta = FastMath.max(maxDelta, FastMath.abs(sunElevation - misAlignment)); }/*from w ww. j a va 2 s. co m*/ Assert.assertTrue(FastMath.toDegrees(maxDelta) > 120.0); }
From source file:org.orekit.forces.drag.DragForce.java
/** {@inheritDoc} */ public FieldVector3D<DerivativeStructure> accelerationDerivatives(final AbsoluteDate date, final Frame frame, final FieldVector3D<DerivativeStructure> position, final FieldVector3D<DerivativeStructure> velocity, final FieldRotation<DerivativeStructure> rotation, final DerivativeStructure mass) throws OrekitException { // retrieve derivation properties final int parameters = mass.getFreeParameters(); final int order = mass.getOrder(); // get atmosphere properties in atmosphere own frame final Frame atmFrame = atmosphere.getFrame(); final Transform toBody = frame.getTransformTo(atmFrame, date); final FieldVector3D<DerivativeStructure> posBodyDS = toBody.transformPosition(position); final Vector3D posBody = posBodyDS.toVector3D(); final double rho = atmosphere.getDensity(date, posBody, atmFrame); final Vector3D vAtmBody = atmosphere.getVelocity(date, posBody, atmFrame); // we consider that at first order the atmosphere velocity in atmosphere frame // does not depend on local position; however atmosphere velocity in inertial // frame DOES depend on position since the transform between the frames depends // on it, due to central body rotation rate and velocity composition. // So we use the transform to get the correct partial derivatives on vAtm final FieldVector3D<DerivativeStructure> vAtmBodyDS = new FieldVector3D<DerivativeStructure>( new DerivativeStructure(parameters, order, vAtmBody.getX()), new DerivativeStructure(parameters, order, vAtmBody.getY()), new DerivativeStructure(parameters, order, vAtmBody.getZ())); final FieldPVCoordinates<DerivativeStructure> pvAtmBody = new FieldPVCoordinates<DerivativeStructure>( posBodyDS, vAtmBodyDS);/* w w w. j a v a 2s . c o m*/ final FieldPVCoordinates<DerivativeStructure> pvAtm = toBody.getInverse().transformPVCoordinates(pvAtmBody); // now we can compute relative velocity, it takes into account partial derivatives with respect to position final FieldVector3D<DerivativeStructure> relativeVelocity = pvAtm.getVelocity().subtract(velocity); // compute acceleration with all its partial derivatives return spacecraft.dragAcceleration(date, frame, position, rotation, mass, rho, relativeVelocity); }
From source file:org.orekit.forces.drag.DTM2000.java
/** Get the local density. * @param date current date/*from w ww .j a va 2 s .c om*/ * @param position current position in frame * @param frame the frame in which is defined the position * @return local density (kg/m) * @exception OrekitException if date is out of range of solar activity model * or if some frame conversion cannot be performed */ public double getDensity(final AbsoluteDate date, final Vector3D position, final Frame frame) throws OrekitException { // check if data are available : if ((date.compareTo(inputParams.getMaxDate()) > 0) || (date.compareTo(inputParams.getMinDate()) < 0)) { throw new OrekitException(OrekitMessages.NO_SOLAR_ACTIVITY_AT_DATE, date, inputParams.getMinDate(), inputParams.getMaxDate()); } // compute day number in current year final Calendar cal = new GregorianCalendar(); cal.setTime(date.toDate(TimeScalesFactory.getUTC())); final int day = cal.get(Calendar.DAY_OF_YEAR); // compute geodetic position final GeodeticPoint inBody = earth.transform(position, frame, date); final double alti = inBody.getAltitude(); final double lon = inBody.getLongitude(); final double lat = inBody.getLatitude(); // compute local solar time final Vector3D sunPos = sun.getPVCoordinates(date, frame).getPosition(); final double hl = FastMath.PI + FastMath.atan2(sunPos.getX() * position.getY() - sunPos.getY() * position.getX(), sunPos.getX() * position.getX() + sunPos.getY() * position.getY()); // get current solar activity data and compute return getDensity(day, alti, lon, lat, hl, inputParams.getInstantFlux(date), inputParams.getMeanFlux(date), inputParams.getThreeHourlyKP(date), inputParams.get24HoursKp(date)); }
From source file:org.orekit.forces.drag.HarrisPriester.java
/** Get the local density. * @param sunInEarth position of the Sun in Earth frame (m) * @param posInEarth target position in Earth frame (m) * @return the local density (kg/m)//from w w w . j a va2s. co m * @exception OrekitException if altitude is below the model minimal altitude */ public double getDensity(final Vector3D sunInEarth, final Vector3D posInEarth) throws OrekitException { final double posAlt = getHeight(posInEarth); // Check for height boundaries if (posAlt < getMinAlt()) { throw new OrekitException(OrekitMessages.ALTITUDE_BELOW_ALLOWED_THRESHOLD, posAlt, getMinAlt()); } if (posAlt > getMaxAlt()) { return 0.; } // Diurnal bulge apex direction final Vector3D sunDir = sunInEarth.normalize(); final Vector3D bulDir = new Vector3D(sunDir.getX() * COSLAG - sunDir.getY() * SINLAG, sunDir.getX() * SINLAG + sunDir.getY() * COSLAG, sunDir.getZ()); // Cosine of angle Psi between the diurnal bulge apex and the satellite final double cosPsi = bulDir.normalize().dotProduct(posInEarth.normalize()); // (1 + cos(Psi))/2 = cos(Psi/2) final double c2Psi2 = (1. + cosPsi) / 2.; final double cPsi2 = FastMath.sqrt(c2Psi2); final double cosPow = (cPsi2 > MIN_COS) ? c2Psi2 * FastMath.pow(cPsi2, n - 2) : 0.; // Search altitude index in density table int ia = 0; while (ia < tabAltRho.length - 2 && posAlt > tabAltRho[ia + 1][0]) { ia++; } // Fractional satellite height final double dH = (tabAltRho[ia][0] - posAlt) / (tabAltRho[ia][0] - tabAltRho[ia + 1][0]); // Min exponential density interpolation final double rhoMin = tabAltRho[ia][1] * FastMath.pow(tabAltRho[ia + 1][1] / tabAltRho[ia][1], dH); if (Precision.equals(cosPow, 0.)) { return rhoMin; } else { // Max exponential density interpolation final double rhoMax = tabAltRho[ia][2] * FastMath.pow(tabAltRho[ia + 1][2] / tabAltRho[ia][2], dH); return rhoMin + (rhoMax - rhoMin) * cosPow; } }
From source file:org.orekit.forces.gravity.CunninghamAttractionModel.java
/** {@inheritDoc} */ public void addContribution(final SpacecraftState s, final TimeDerivativesEquations adder) throws OrekitException { // get the position in body frame final AbsoluteDate date = s.getDate(); final UnnormalizedSphericalHarmonics harmonics = provider.onDate(date); final Transform fromBodyFrame = bodyFrame.getTransformTo(s.getFrame(), date); final Transform toBodyFrame = fromBodyFrame.getInverse(); final Vector3D relative = toBodyFrame.transformPosition(s.getPVCoordinates().getPosition()); final double x = relative.getX(); final double y = relative.getY(); final double z = relative.getZ(); final double x2 = x * x; final double y2 = y * y; final double z2 = z * z; final double r2 = x2 + y2 + z2; final double r = FastMath.sqrt(r2); final double equatorialRadius = provider.getAe(); if (r <= equatorialRadius) { throw new OrekitException(OrekitMessages.TRAJECTORY_INSIDE_BRILLOUIN_SPHERE, r); }// w w w . j a v a2 s . c o m // define some intermediate variables final double onR2 = 1 / r2; final double onR3 = onR2 / r; final double rEqOnR2 = equatorialRadius / r2; final double rEqOnR4 = rEqOnR2 / r2; final double rEq2OnR2 = equatorialRadius * rEqOnR2; double cmx = -x * rEqOnR2; double cmy = -y * rEqOnR2; double cmz = -z * rEqOnR2; final double dx = -2 * cmx; final double dy = -2 * cmy; final double dz = -2 * cmz; // intermediate variables gradients // since dcy/dx = dcx/dy, dcz/dx = dcx/dz and dcz/dy = dcy/dz, // we reuse the existing variables double dcmxdx = (x2 - y2 - z2) * rEqOnR4; double dcmxdy = dx * y * onR2; double dcmxdz = dx * z * onR2; double dcmydy = (y2 - x2 - z2) * rEqOnR4; double dcmydz = dy * z * onR2; double dcmzdz = (z2 - x2 - y2) * rEqOnR4; final double ddxdx = -2 * dcmxdx; final double ddxdy = -2 * dcmxdy; final double ddxdz = -2 * dcmxdz; final double ddydy = -2 * dcmydy; final double ddydz = -2 * dcmydz; final double ddzdz = -2 * dcmzdz; final double donr2dx = -dx * rEqOnR2; final double donr2dy = -dy * rEqOnR2; final double donr2dz = -dz * rEqOnR2; // potential coefficients (4 per matrix) double vrn = 0.0; double vin = 0.0; double vrd = 1.0 / (equatorialRadius * r); double vid = 0.0; double vrn1 = 0.0; double vin1 = 0.0; double vrn2 = 0.0; double vin2 = 0.0; // gradient coefficients (4 per matrix) double gradXVrn = 0.0; double gradXVin = 0.0; double gradXVrd = -x * onR3 / equatorialRadius; double gradXVid = 0.0; double gradXVrn1 = 0.0; double gradXVin1 = 0.0; double gradXVrn2 = 0.0; double gradXVin2 = 0.0; double gradYVrn = 0.0; double gradYVin = 0.0; double gradYVrd = -y * onR3 / equatorialRadius; double gradYVid = 0.0; double gradYVrn1 = 0.0; double gradYVin1 = 0.0; double gradYVrn2 = 0.0; double gradYVin2 = 0.0; double gradZVrn = 0.0; double gradZVin = 0.0; double gradZVrd = -z * onR3 / equatorialRadius; double gradZVid = 0.0; double gradZVrn1 = 0.0; double gradZVin1 = 0.0; double gradZVrn2 = 0.0; double gradZVin2 = 0.0; // acceleration coefficients double vdX = 0.0; double vdY = 0.0; double vdZ = 0.0; // start calculating for (int m = 0; m <= provider.getMaxOrder(); m++) { double cx = cmx; double cy = cmy; double cz = cmz; double dcxdx = dcmxdx; double dcxdy = dcmxdy; double dcxdz = dcmxdz; double dcydy = dcmydy; double dcydz = dcmydz; double dczdz = dcmzdz; for (int n = m; n <= provider.getMaxDegree(); n++) { if (n == m) { // calculate the first element of the next column vrn = equatorialRadius * vrd; vin = equatorialRadius * vid; gradXVrn = equatorialRadius * gradXVrd; gradXVin = equatorialRadius * gradXVid; gradYVrn = equatorialRadius * gradYVrd; gradYVin = equatorialRadius * gradYVid; gradZVrn = equatorialRadius * gradZVrd; gradZVin = equatorialRadius * gradZVid; final double tmpGradXVrd = (cx + dx) * gradXVrd - (cy + dy) * gradXVid + (dcxdx + ddxdx) * vrd - (dcxdy + ddxdy) * vid; gradXVid = (cy + dy) * gradXVrd + (cx + dx) * gradXVid + (dcxdy + ddxdy) * vrd + (dcxdx + ddxdx) * vid; gradXVrd = tmpGradXVrd; final double tmpGradYVrd = (cx + dx) * gradYVrd - (cy + dy) * gradYVid + (dcxdy + ddxdy) * vrd - (dcydy + ddydy) * vid; gradYVid = (cy + dy) * gradYVrd + (cx + dx) * gradYVid + (dcydy + ddydy) * vrd + (dcxdy + ddxdy) * vid; gradYVrd = tmpGradYVrd; final double tmpGradZVrd = (cx + dx) * gradZVrd - (cy + dy) * gradZVid + (dcxdz + ddxdz) * vrd - (dcydz + ddydz) * vid; gradZVid = (cy + dy) * gradZVrd + (cx + dx) * gradZVid + (dcydz + ddydz) * vrd + (dcxdz + ddxdz) * vid; gradZVrd = tmpGradZVrd; final double tmpVrd = (cx + dx) * vrd - (cy + dy) * vid; vid = (cy + dy) * vrd + (cx + dx) * vid; vrd = tmpVrd; } else if (n == m + 1) { // calculate the second element of the column vrn = cz * vrn1; vin = cz * vin1; gradXVrn = cz * gradXVrn1 + dcxdz * vrn1; gradXVin = cz * gradXVin1 + dcxdz * vin1; gradYVrn = cz * gradYVrn1 + dcydz * vrn1; gradYVin = cz * gradYVin1 + dcydz * vin1; gradZVrn = cz * gradZVrn1 + dczdz * vrn1; gradZVin = cz * gradZVin1 + dczdz * vin1; } else { // calculate the other elements of the column final double inv = 1.0 / (n - m); final double coeff = n + m - 1.0; vrn = (cz * vrn1 - coeff * rEq2OnR2 * vrn2) * inv; vin = (cz * vin1 - coeff * rEq2OnR2 * vin2) * inv; gradXVrn = (cz * gradXVrn1 - coeff * rEq2OnR2 * gradXVrn2 + dcxdz * vrn1 - coeff * donr2dx * vrn2) * inv; gradXVin = (cz * gradXVin1 - coeff * rEq2OnR2 * gradXVin2 + dcxdz * vin1 - coeff * donr2dx * vin2) * inv; gradYVrn = (cz * gradYVrn1 - coeff * rEq2OnR2 * gradYVrn2 + dcydz * vrn1 - coeff * donr2dy * vrn2) * inv; gradYVin = (cz * gradYVin1 - coeff * rEq2OnR2 * gradYVin2 + dcydz * vin1 - coeff * donr2dy * vin2) * inv; gradZVrn = (cz * gradZVrn1 - coeff * rEq2OnR2 * gradZVrn2 + dczdz * vrn1 - coeff * donr2dz * vrn2) * inv; gradZVin = (cz * gradZVin1 - coeff * rEq2OnR2 * gradZVin2 + dczdz * vin1 - coeff * donr2dz * vin2) * inv; } // increment variables cx += dx; cy += dy; cz += dz; dcxdx += ddxdx; dcxdy += ddxdy; dcxdz += ddxdz; dcydy += ddydy; dcydz += ddydz; dczdz += ddzdz; vrn2 = vrn1; vin2 = vin1; gradXVrn2 = gradXVrn1; gradXVin2 = gradXVin1; gradYVrn2 = gradYVrn1; gradYVin2 = gradYVin1; gradZVrn2 = gradZVrn1; gradZVin2 = gradZVin1; vrn1 = vrn; vin1 = vin; gradXVrn1 = gradXVrn; gradXVin1 = gradXVin; gradYVrn1 = gradYVrn; gradYVin1 = gradYVin; gradZVrn1 = gradZVrn; gradZVin1 = gradZVin; // compute the acceleration due to the Cnm and Snm coefficients // ignoring the central attraction if (n > 0) { final double cnm = harmonics.getUnnormalizedCnm(n, m); final double snm = harmonics.getUnnormalizedSnm(n, m); vdX += cnm * gradXVrn + snm * gradXVin; vdY += cnm * gradYVrn + snm * gradYVin; vdZ += cnm * gradZVrn + snm * gradZVin; } } // increment variables cmx += dx; cmy += dy; cmz += dz; dcmxdx += ddxdx; dcmxdy += ddxdy; dcmxdz += ddxdz; dcmydy += ddydy; dcmydz += ddydz; dcmzdz += ddzdz; } // compute acceleration in inertial frame final Vector3D acceleration = fromBodyFrame.transformVector(new Vector3D(mu * vdX, mu * vdY, mu * vdZ)); adder.addXYZAcceleration(acceleration.getX(), acceleration.getY(), acceleration.getZ()); }
From source file:org.orekit.forces.gravity.DrozinerAttractionModel.java
/** {@inheritDoc} */ public void addContribution(final SpacecraftState s, final TimeDerivativesEquations adder) throws OrekitException { // Get the position in body frame final AbsoluteDate date = s.getDate(); final UnnormalizedSphericalHarmonics harmonics = provider.onDate(date); final Transform bodyToInertial = centralBodyFrame.getTransformTo(s.getFrame(), date); final Vector3D posInBody = bodyToInertial.getInverse().transformVector(s.getPVCoordinates().getPosition()); final double xBody = posInBody.getX(); final double yBody = posInBody.getY(); final double zBody = posInBody.getZ(); // Computation of intermediate variables final double r12 = xBody * xBody + yBody * yBody; final double r1 = FastMath.sqrt(r12); if (r1 <= 10e-2) { throw new OrekitException(OrekitMessages.POLAR_TRAJECTORY, r1); }// w ww.ja v a2 s . c o m final double r2 = r12 + zBody * zBody; final double r = FastMath.sqrt(r2); final double equatorialRadius = provider.getAe(); if (r <= equatorialRadius) { throw new OrekitException(OrekitMessages.TRAJECTORY_INSIDE_BRILLOUIN_SPHERE, r); } final double r3 = r2 * r; final double aeOnr = equatorialRadius / r; final double zOnr = zBody / r; final double r1Onr = r1 / r; // Definition of the first acceleration terms final double mMuOnr3 = -mu / r3; final double xDotDotk = xBody * mMuOnr3; final double yDotDotk = yBody * mMuOnr3; // Zonal part of acceleration double sumA = 0.0; double sumB = 0.0; double bk1 = zOnr; double bk0 = aeOnr * (3 * bk1 * bk1 - 1.0); double jk = -harmonics.getUnnormalizedCnm(1, 0); // first zonal term sumA += jk * (2 * aeOnr * bk1 - zOnr * bk0); sumB += jk * bk0; // other terms for (int k = 2; k <= provider.getMaxDegree(); k++) { final double bk2 = bk1; bk1 = bk0; final double p = (1.0 + k) / k; bk0 = aeOnr * ((1 + p) * zOnr * bk1 - (k * aeOnr * bk2) / (k - 1)); final double ak0 = p * aeOnr * bk1 - zOnr * bk0; jk = -harmonics.getUnnormalizedCnm(k, 0); sumA += jk * ak0; sumB += jk * bk0; } // calculate the acceleration final double p = -sumA / (r1Onr * r1Onr); double aX = xDotDotk * p; double aY = yDotDotk * p; double aZ = mu * sumB / r2; // Tessereal-sectorial part of acceleration if (provider.getMaxOrder() > 0) { // latitude and longitude in body frame final double cosL = xBody / r1; final double sinL = yBody / r1; // intermediate variables double betaKminus1 = aeOnr; double cosjm1L = cosL; double sinjm1L = sinL; double sinjL = sinL; double cosjL = cosL; double betaK = 0; double Bkj = 0.0; double Bkm1j = 3 * betaKminus1 * zOnr * r1Onr; double Bkm2j = 0; double Bkminus1kminus1 = Bkm1j; // first terms final double c11 = harmonics.getUnnormalizedCnm(1, 1); final double s11 = harmonics.getUnnormalizedSnm(1, 1); double Gkj = c11 * cosL + s11 * sinL; double Hkj = c11 * sinL - s11 * cosL; double Akj = 2 * r1Onr * betaKminus1 - zOnr * Bkminus1kminus1; double Dkj = (Akj + zOnr * Bkminus1kminus1) * 0.5; double sum1 = Akj * Gkj; double sum2 = Bkminus1kminus1 * Gkj; double sum3 = Dkj * Hkj; // the other terms for (int j = 1; j <= provider.getMaxOrder(); ++j) { double innerSum1 = 0.0; double innerSum2 = 0.0; double innerSum3 = 0.0; for (int k = FastMath.max(2, j); k <= provider.getMaxDegree(); ++k) { final double ckj = harmonics.getUnnormalizedCnm(k, j); final double skj = harmonics.getUnnormalizedSnm(k, j); Gkj = ckj * cosjL + skj * sinjL; Hkj = ckj * sinjL - skj * cosjL; if (j <= (k - 2)) { Bkj = aeOnr * (zOnr * Bkm1j * (2.0 * k + 1.0) / (k - j) - aeOnr * Bkm2j * (k + j) / (k - 1 - j)); Akj = aeOnr * Bkm1j * (k + 1.0) / (k - j) - zOnr * Bkj; } else if (j == (k - 1)) { betaK = aeOnr * (2.0 * k - 1.0) * r1Onr * betaKminus1; Bkj = aeOnr * (2.0 * k + 1.0) * zOnr * Bkm1j - betaK; Akj = aeOnr * (k + 1.0) * Bkm1j - zOnr * Bkj; betaKminus1 = betaK; } else if (j == k) { Bkj = (2 * k + 1) * aeOnr * r1Onr * Bkminus1kminus1; Akj = (k + 1) * r1Onr * betaK - zOnr * Bkj; Bkminus1kminus1 = Bkj; } Dkj = (Akj + zOnr * Bkj) * j / (k + 1.0); Bkm2j = Bkm1j; Bkm1j = Bkj; innerSum1 += Akj * Gkj; innerSum2 += Bkj * Gkj; innerSum3 += Dkj * Hkj; } sum1 += innerSum1; sum2 += innerSum2; sum3 += innerSum3; sinjL = sinjm1L * cosL + cosjm1L * sinL; cosjL = cosjm1L * cosL - sinjm1L * sinL; sinjm1L = sinjL; cosjm1L = cosjL; } // compute the acceleration final double r2Onr12 = r2 / (r1 * r1); final double p1 = r2Onr12 * xDotDotk; final double p2 = r2Onr12 * yDotDotk; aX += p1 * sum1 - p2 * sum3; aY += p2 * sum1 + p1 * sum3; aZ -= mu * sum2 / r2; } // provide the perturbing acceleration to the derivatives adder in inertial frame final Vector3D accInInert = bodyToInertial.transformVector(new Vector3D(aX, aY, aZ)); adder.addXYZAcceleration(accInInert.getX(), accInInert.getY(), accInInert.getZ()); }
From source file:org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel.java
/** Compute the non-central part of the gravity field. * @param date current date/*from ww w . j a v a 2s . c om*/ * @param position position at which gravity field is desired in body frame * @return value of the non-central part of the gravity field * @exception OrekitException if position cannot be converted to central body frame */ public double nonCentralPart(final AbsoluteDate date, final Vector3D position) throws OrekitException { final int degree = provider.getMaxDegree(); final int order = provider.getMaxOrder(); final NormalizedSphericalHarmonics harmonics = provider.onDate(date); // allocate the columns for recursion double[] pnm0Plus2 = new double[degree + 1]; double[] pnm0Plus1 = new double[degree + 1]; double[] pnm0 = new double[degree + 1]; // compute polar coordinates final double x = position.getX(); final double y = position.getY(); final double z = position.getZ(); final double x2 = x * x; final double y2 = y * y; final double z2 = z * z; final double r2 = x2 + y2 + z2; final double r = FastMath.sqrt(r2); final double rho = FastMath.sqrt(x2 + y2); final double t = z / r; // cos(theta), where theta is the polar angle final double u = rho / r; // sin(theta), where theta is the polar angle final double tOu = z / rho; // compute distance powers final double[] aOrN = createDistancePowersArray(provider.getAe() / r); // compute longitude cosines/sines final double[][] cosSinLambda = createCosSinArrays(position.getX() / rho, position.getY() / rho); // outer summation over order int index = 0; double value = 0; for (int m = degree; m >= 0; --m) { // compute tesseral terms without derivatives index = computeTesseral(m, degree, index, t, u, tOu, pnm0Plus2, pnm0Plus1, null, pnm0, null, null); if (m <= order) { // compute contribution of current order to field (equation 5 of the paper) // inner summation over degree, for fixed order double sumDegreeS = 0; double sumDegreeC = 0; for (int n = FastMath.max(2, m); n <= degree; ++n) { sumDegreeS += pnm0[n] * aOrN[n] * harmonics.getNormalizedSnm(n, m); sumDegreeC += pnm0[n] * aOrN[n] * harmonics.getNormalizedCnm(n, m); } // contribution to outer summation over order value = value * u + cosSinLambda[1][m] * sumDegreeS + cosSinLambda[0][m] * sumDegreeC; } // rotate the recursion arrays final double[] tmp = pnm0Plus2; pnm0Plus2 = pnm0Plus1; pnm0Plus1 = pnm0; pnm0 = tmp; } // scale back value = FastMath.scalb(value, SCALING); // apply the global mu/r factor return mu * value / r; }