List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D getX
public double getX()
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); }/*from ww w. j a va2 s . c om*/ // 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); }// ww w. j a v a2s . 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 va 2 s . c o m * @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; }
From source file:org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel.java
/** Compute the gradient of the non-central part of the gravity field. * @param date current date/*www .j a v a 2 s . c o m*/ * @param position position at which gravity field is desired in body frame * @return gradient of the non-central part of the gravity field * @exception OrekitException if position cannot be converted to central body frame */ public double[] gradient(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]; final double[] pnm1 = 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 rho2 = x2 + y2; final double rho = FastMath.sqrt(rho2); 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; final double[] gradient = new double[3]; for (int m = degree; m >= 0; --m) { // compute tesseral terms with derivatives index = computeTesseral(m, degree, index, t, u, tOu, pnm0Plus2, pnm0Plus1, null, pnm0, pnm1, 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; double dSumDegreeSdR = 0; double dSumDegreeCdR = 0; double dSumDegreeSdTheta = 0; double dSumDegreeCdTheta = 0; for (int n = FastMath.max(2, m); n <= degree; ++n) { final double qSnm = aOrN[n] * harmonics.getNormalizedSnm(n, m); final double qCnm = aOrN[n] * harmonics.getNormalizedCnm(n, m); final double nOr = n / r; final double s0 = pnm0[n] * qSnm; final double c0 = pnm0[n] * qCnm; final double s1 = pnm1[n] * qSnm; final double c1 = pnm1[n] * qCnm; sumDegreeS += s0; sumDegreeC += c0; dSumDegreeSdR -= nOr * s0; dSumDegreeCdR -= nOr * c0; dSumDegreeSdTheta += s1; dSumDegreeCdTheta += c1; } // contribution to outer summation over order // beware that we need to order gradient using the mathematical conventions // compliant with the SphericalCoordinates class, so our lambda is its theta // (and hence at index 1) and our theta is its phi (and hence at index 2) final double sML = cosSinLambda[1][m]; final double cML = cosSinLambda[0][m]; value = value * u + sML * sumDegreeS + cML * sumDegreeC; gradient[0] = gradient[0] * u + sML * dSumDegreeSdR + cML * dSumDegreeCdR; gradient[1] = gradient[1] * u + m * (cML * sumDegreeS - sML * sumDegreeC); gradient[2] = gradient[2] * u + sML * dSumDegreeSdTheta + cML * dSumDegreeCdTheta; } // rotate the recursion arrays final double[] tmp = pnm0Plus2; pnm0Plus2 = pnm0Plus1; pnm0Plus1 = pnm0; pnm0 = tmp; } // scale back value = FastMath.scalb(value, SCALING); gradient[0] = FastMath.scalb(gradient[0], SCALING); gradient[1] = FastMath.scalb(gradient[1], SCALING); gradient[2] = FastMath.scalb(gradient[2], SCALING); // apply the global mu/r factor final double muOr = mu / r; value *= muOr; gradient[0] = muOr * gradient[0] - value / r; gradient[1] *= muOr; gradient[2] *= muOr; // convert gradient from spherical to Cartesian return new SphericalCoordinates(position).toCartesianGradient(gradient); }
From source file:org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel.java
/** Compute both the gradient and the hessian of the non-central part of the gravity field. * @param date current date//w w w .ja v a 2 s . c om * @param position position at which gravity field is desired in body frame * @return gradient and hessian of the non-central part of the gravity field * @exception OrekitException if position cannot be converted to central body frame */ public GradientHessian gradientHessian(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]; double[] pnm1Plus1 = new double[degree + 1]; double[] pnm1 = new double[degree + 1]; final double[] pnm2 = 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 rho2 = x2 + y2; final double rho = FastMath.sqrt(rho2); 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; final double[] gradient = new double[3]; final double[][] hessian = new double[3][3]; for (int m = degree; m >= 0; --m) { // compute tesseral terms index = computeTesseral(m, degree, index, t, u, tOu, pnm0Plus2, pnm0Plus1, pnm1Plus1, pnm0, pnm1, pnm2); 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; double dSumDegreeSdR = 0; double dSumDegreeCdR = 0; double dSumDegreeSdTheta = 0; double dSumDegreeCdTheta = 0; double d2SumDegreeSdRdR = 0; double d2SumDegreeSdRdTheta = 0; double d2SumDegreeSdThetadTheta = 0; double d2SumDegreeCdRdR = 0; double d2SumDegreeCdRdTheta = 0; double d2SumDegreeCdThetadTheta = 0; for (int n = FastMath.max(2, m); n <= degree; ++n) { final double qSnm = aOrN[n] * harmonics.getNormalizedSnm(n, m); final double qCnm = aOrN[n] * harmonics.getNormalizedCnm(n, m); final double nOr = n / r; final double nnP1Or2 = nOr * (n + 1) / r; final double s0 = pnm0[n] * qSnm; final double c0 = pnm0[n] * qCnm; final double s1 = pnm1[n] * qSnm; final double c1 = pnm1[n] * qCnm; final double s2 = pnm2[n] * qSnm; final double c2 = pnm2[n] * qCnm; sumDegreeS += s0; sumDegreeC += c0; dSumDegreeSdR -= nOr * s0; dSumDegreeCdR -= nOr * c0; dSumDegreeSdTheta += s1; dSumDegreeCdTheta += c1; d2SumDegreeSdRdR += nnP1Or2 * s0; d2SumDegreeSdRdTheta -= nOr * s1; d2SumDegreeSdThetadTheta += s2; d2SumDegreeCdRdR += nnP1Or2 * c0; d2SumDegreeCdRdTheta -= nOr * c1; d2SumDegreeCdThetadTheta += c2; } // contribution to outer summation over order final double sML = cosSinLambda[1][m]; final double cML = cosSinLambda[0][m]; value = value * u + sML * sumDegreeS + cML * sumDegreeC; gradient[0] = gradient[0] * u + sML * dSumDegreeSdR + cML * dSumDegreeCdR; gradient[1] = gradient[1] * u + m * (cML * sumDegreeS - sML * sumDegreeC); gradient[2] = gradient[2] * u + sML * dSumDegreeSdTheta + cML * dSumDegreeCdTheta; hessian[0][0] = hessian[0][0] * u + sML * d2SumDegreeSdRdR + cML * d2SumDegreeCdRdR; hessian[1][0] = hessian[1][0] * u + m * (cML * dSumDegreeSdR - sML * dSumDegreeCdR); hessian[2][0] = hessian[2][0] * u + sML * d2SumDegreeSdRdTheta + cML * d2SumDegreeCdRdTheta; hessian[1][1] = hessian[1][1] * u - m * m * (sML * sumDegreeS + cML * sumDegreeC); hessian[2][1] = hessian[2][1] * u + m * (cML * dSumDegreeSdTheta - sML * dSumDegreeCdTheta); hessian[2][2] = hessian[2][2] * u + sML * d2SumDegreeSdThetadTheta + cML * d2SumDegreeCdThetadTheta; } // rotate the recursion arrays final double[] tmp0 = pnm0Plus2; pnm0Plus2 = pnm0Plus1; pnm0Plus1 = pnm0; pnm0 = tmp0; final double[] tmp1 = pnm1Plus1; pnm1Plus1 = pnm1; pnm1 = tmp1; } // scale back value = FastMath.scalb(value, SCALING); for (int i = 0; i < 3; ++i) { gradient[i] = FastMath.scalb(gradient[i], SCALING); for (int j = 0; j <= i; ++j) { hessian[i][j] = FastMath.scalb(hessian[i][j], SCALING); } } // apply the global mu/r factor final double muOr = mu / r; value *= muOr; gradient[0] = muOr * gradient[0] - value / r; gradient[1] *= muOr; gradient[2] *= muOr; hessian[0][0] = muOr * hessian[0][0] - 2 * gradient[0] / r; hessian[1][0] = muOr * hessian[1][0] - gradient[1] / r; hessian[2][0] = muOr * hessian[2][0] - gradient[2] / r; hessian[1][1] *= muOr; hessian[2][1] *= muOr; hessian[2][2] *= muOr; // convert gradient and Hessian from spherical to Cartesian final SphericalCoordinates sc = new SphericalCoordinates(position); return new GradientHessian(sc.toCartesianGradient(gradient), sc.toCartesianHessian(hessian, gradient)); }
From source file:org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel.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 Transform fromBodyFrame = bodyFrame.getTransformTo(s.getFrame(), date); final Transform toBodyFrame = fromBodyFrame.getInverse(); final Vector3D position = toBodyFrame.transformPosition(s.getPVCoordinates().getPosition()); // gradient of the non-central part of the gravity field final Vector3D gInertial = fromBodyFrame.transformVector(new Vector3D(gradient(date, position))); adder.addXYZAcceleration(gInertial.getX(), gInertial.getY(), gInertial.getZ()); }
From source file:org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel.java
/** {@inheritDoc} */ public FieldVector3D<DerivativeStructure> accelerationDerivatives(final SpacecraftState s, final String paramName) throws OrekitException, IllegalArgumentException { complainIfNotSupported(paramName);//from www . jav a 2s. co m // get the position in body frame final AbsoluteDate date = s.getDate(); final Transform fromBodyFrame = bodyFrame.getTransformTo(s.getFrame(), date); final Transform toBodyFrame = fromBodyFrame.getInverse(); final Vector3D position = toBodyFrame.transformPosition(s.getPVCoordinates().getPosition()); // gradient of the non-central part of the gravity field final Vector3D gInertial = fromBodyFrame.transformVector(new Vector3D(gradient(date, position))); return new FieldVector3D<DerivativeStructure>( new DerivativeStructure(1, 1, gInertial.getX(), gInertial.getX() / mu), new DerivativeStructure(1, 1, gInertial.getY(), gInertial.getY() / mu), new DerivativeStructure(1, 1, gInertial.getZ(), gInertial.getZ() / mu)); }
From source file:org.orekit.forces.gravity.ReferenceFieldModel.java
public Dfp nonCentralPart(final AbsoluteDate date, final Vector3D position) throws OrekitException { int degree = provider.getMaxDegree(); int order = provider.getMaxOrder(); //use coefficients without caring if they are the correct type final RawSphericalHarmonics harmonics = raw(provider).onDate(date); Dfp x = dfpField.newDfp(position.getX()); Dfp y = dfpField.newDfp(position.getY()); Dfp z = dfpField.newDfp(position.getZ()); Dfp rho2 = x.multiply(x).add(y.multiply(y)); Dfp rho = rho2.sqrt();/* w w w . j a v a2 s . co m*/ Dfp r2 = rho2.add(z.multiply(z)); Dfp r = r2.sqrt(); Dfp aOr = dfpField.newDfp(provider.getAe()).divide(r); Dfp lambda = position.getX() > 0 ? dfpField.getTwo().multiply(DfpMath.atan(y.divide(rho.add(x)))) : dfpField.getPi().subtract(dfpField.getTwo().multiply(DfpMath.atan(y.divide(rho.subtract(x))))); Dfp cosTheta = z.divide(r); Dfp value = dfpField.getZero(); Dfp aOrN = aOr; for (int n = 2; n <= degree; ++n) { Dfp sum = dfpField.getZero(); for (int m = 0; m <= FastMath.min(n, order); ++m) { double cnm = harmonics.getRawCnm(n, m); double snm = harmonics.getRawSnm(n, m); Dfp mLambda = lambda.multiply(m); Dfp c = DfpMath.cos(mLambda).multiply(dfpField.newDfp(cnm)); Dfp s = DfpMath.sin(mLambda).multiply(dfpField.newDfp(snm)); Dfp pnm = alf[n][m].value(cosTheta); sum = sum.add(pnm.multiply(c.add(s))); } aOrN = aOrN.multiply(aOr); value = value.add(aOrN.multiply(sum)); } return value.multiply(dfpField.newDfp(provider.getMu())).divide(r); }
From source file:org.orekit.forces.gravity.SolidTidesField.java
/** {@inheritDoc} */ @Override/*from w w w . j av a 2 s .c om*/ public NormalizedSphericalHarmonics onDate(final AbsoluteDate date) throws OrekitException { // computed Cnm and Snm coefficients final double[][] cnm = buildTriangularArray(5, true); final double[][] snm = buildTriangularArray(5, true); // work array to hold Legendre coefficients final double[][] pnm = buildTriangularArray(5, true); // step 1: frequency independent part // equations 6.6 (for degrees 2 and 3) and 6.7 (for degree 4) in IERS conventions 2010 for (final CelestialBody body : bodies) { // compute tide generating body state final Vector3D position = body.getPVCoordinates(date, centralBodyFrame).getPosition(); // 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 rho2 = x2 + y2; final double rho = FastMath.sqrt(rho2); // evaluate Pnm evaluateLegendre(z / r, rho / r, pnm); // update spherical harmonic coefficients frequencyIndependentPart(r, body.getGM(), x / rho, y / rho, pnm, cnm, snm); } // step 2: frequency dependent corrections frequencyDependentPart(date, cnm, snm); if (centralTideSystem == TideSystem.ZERO_TIDE) { // step 3: remove permanent tide which is already considered // in the central body gravity field removePermanentTide(cnm); } if (poleTideFunction != null) { // add pole tide poleTide(date, cnm, snm); } return new TideHarmonics(date, cnm, snm); }
From source file:org.orekit.forces.gravity.ThirdBodyAttraction.java
/** {@inheritDoc} */ public void addContribution(final SpacecraftState s, final TimeDerivativesEquations adder) throws OrekitException { // compute bodies separation vectors and squared norm final Vector3D centralToBody = body.getPVCoordinates(s.getDate(), s.getFrame()).getPosition(); final double r2Central = centralToBody.getNormSq(); final Vector3D satToBody = centralToBody.subtract(s.getPVCoordinates().getPosition()); final double r2Sat = satToBody.getNormSq(); // compute relative acceleration final Vector3D gamma = new Vector3D(gm / (r2Sat * FastMath.sqrt(r2Sat)), satToBody, -gm / (r2Central * FastMath.sqrt(r2Central)), centralToBody); // add contribution to the ODE second member adder.addXYZAcceleration(gamma.getX(), gamma.getY(), gamma.getZ()); }