Examples with SphericalCoordinates org.hipparchus.geometry.euclidean.threed.SphericalCoordinates used on opensource projects

Example 1 with SphericalCoordinates

use of org.hipparchus.geometry.euclidean.threed.SphericalCoordinates in project Orekit by CS-SI.

the class HolmesFeatherstoneAttractionModelTest method gradientHessian.

private GradientHessian gradientHessian(final HolmesFeatherstoneAttractionModel hfModel, final AbsoluteDate date, final Vector3D position) throws OrekitException {
    try {
        java.lang.reflect.Field providerField = HolmesFeatherstoneAttractionModel.class.getDeclaredField("provider");
        providerField.setAccessible(true);
        NormalizedSphericalHarmonicsProvider provider = (NormalizedSphericalHarmonicsProvider) providerField.get(hfModel);
        java.lang.reflect.Method createDistancePowersArrayMethod = HolmesFeatherstoneAttractionModel.class.getDeclaredMethod("createDistancePowersArray", Double.TYPE);
        createDistancePowersArrayMethod.setAccessible(true);
        java.lang.reflect.Method createCosSinArraysMethod = HolmesFeatherstoneAttractionModel.class.getDeclaredMethod("createCosSinArrays", Double.TYPE, Double.TYPE);
        createCosSinArraysMethod.setAccessible(true);
        java.lang.reflect.Method computeTesseralMethod = HolmesFeatherstoneAttractionModel.class.getDeclaredMethod("computeTesseral", Integer.TYPE, Integer.TYPE, Integer.TYPE, Double.TYPE, Double.TYPE, Double.TYPE, double[].class, double[].class, double[].class, double[].class, double[].class, double[].class);
        computeTesseralMethod.setAccessible(true);
        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);
        // cos(theta), where theta is the polar angle
        final double t = z / r;
        // sin(theta), where theta is the polar angle
        final double u = rho / r;
        final double tOu = z / rho;
        // compute distance powers
        final double[] aOrN = (double[]) createDistancePowersArrayMethod.invoke(hfModel, provider.getAe() / r);
        // compute longitude cosines/sines
        final double[][] cosSinLambda = (double[][]) createCosSinArraysMethod.invoke(hfModel, 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 = ((Integer) computeTesseralMethod.invoke(hfModel, m, degree, index, t, u, tOu, pnm0Plus2, pnm0Plus1, pnm1Plus1, pnm0, pnm1, pnm2)).intValue();
            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 = provider.getMu() / 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));
    } catch (IllegalArgumentException | IllegalAccessException | NoSuchFieldException | SecurityException | InvocationTargetException | NoSuchMethodException e) {
        return null;
    }
}

Example 2 with SphericalCoordinates

use of org.hipparchus.geometry.euclidean.threed.SphericalCoordinates in project Orekit by CS-SI.

the class HolmesFeatherstoneAttractionModel method gradientHessian.

/**
 * Compute both the gradient and the hessian of the non-central part of the gravity field.
 * @param date current date
 * @param position position at which gravity field is desired in body frame
 * @param mu central attraction coefficient to use
 * @return gradient and hessian of the non-central part of the gravity field
 * @exception OrekitException if position cannot be converted to central body frame
 */
private GradientHessian gradientHessian(final AbsoluteDate date, final Vector3D position, final double mu) 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);
    // cos(theta), where theta is the polar angle
    final double t = z / r;
    // sin(theta), where theta is the polar angle
    final double u = rho / r;
    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));
}

Example 3 with SphericalCoordinates

use of org.hipparchus.geometry.euclidean.threed.SphericalCoordinates in project Orekit by CS-SI.

the class HolmesFeatherstoneAttractionModel method gradient.

/**
 * Compute the gradient of the non-central part of the gravity field.
 * @param date current date
 * @param position position at which gravity field is desired in body frame
 * @param mu central attraction coefficient to use
 * @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, final double mu) 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);
    // cos(theta), where theta is the polar angle
    final double t = z / r;
    // sin(theta), where theta is the polar angle
    final double u = rho / r;
    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);
}