List of usage examples for org.apache.commons.math3.geometry.euclidean.threed SphericalCoordinates toCartesianGradient
public double[] toCartesianGradient(final double[] sGradient)
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/*from w ww . j av a 2 s. c o m*/ * @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)); }