ExtendedEllipsoid.java

/* Copyright 2013-2019 CS Systèmes d'Information
 * Licensed to CS Systèmes d'Information (CS) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * CS licenses this file to You under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with
 * the License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
package org.orekit.rugged.utils;

import org.hipparchus.geometry.euclidean.threed.Line;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.MathArrays;
import org.orekit.bodies.GeodeticPoint;
import org.orekit.bodies.OneAxisEllipsoid;
import org.orekit.frames.Frame;
import org.orekit.rugged.errors.DumpManager;
import org.orekit.rugged.errors.RuggedException;
import org.orekit.rugged.errors.RuggedMessages;
import org.orekit.time.AbsoluteDate;

/** Transform provider from Spacecraft frame to observed body frame.
 * @author Luc Maisonobe
 */
public class ExtendedEllipsoid extends OneAxisEllipsoid {

    /** Serializable UID. */
    private static final long serialVersionUID = 20140312L;

    /** Convergence threshold for {@link #pointAtAltitude(Vector3D, Vector3D, double)}. */
    private static final double ALTITUDE_CONVERGENCE = 1.0e-3;

    /** Equatorial radius power 2. */
    private final double a2;

    /** Polar radius power 2. */
    private final double b2;

    /** Simple constructor.
     * @param ae equatorial radius (m)
     * @param f the flattening (f = (a-b)/a)
     * @param bodyFrame body frame related to body shape
     * @see org.orekit.frames.FramesFactory#getITRF(org.orekit.utils.IERSConventions, boolean)
     */
    public ExtendedEllipsoid(final double ae, final double f, final Frame bodyFrame) {
        super(ae, f, bodyFrame);
        a2 = ae * ae;
        final double b = ae * (1.0 - f);
        b2 = b * b;
    }

    /** {@inheritDoc} */
    @Override
    public Vector3D transform(final GeodeticPoint point) {
        DumpManager.dumpEllipsoid(this);
        return super.transform(point);
    }

    /** {@inheritDoc} */
    @Override
    public GeodeticPoint transform(final Vector3D point, final Frame frame, final AbsoluteDate date) {
        DumpManager.dumpEllipsoid(this);
        return super.transform(point, frame, date);
    }

    /** Get point at some latitude along a pixel line of sight.
     * @param position cell position (in body frame) (m)
     * @param los pixel line-of-sight, not necessarily normalized (in body frame)
     * @param latitude latitude with respect to ellipsoid (rad)
     * @param closeReference reference point used to select the closest solution
     * when there are two points at the desired latitude along the line, it should
     * be close to los surface intersection (m)
     * @return point at latitude (m)
     */
    public Vector3D pointAtLatitude(final Vector3D position, final Vector3D los,
                                    final double latitude, final Vector3D closeReference) {

        DumpManager.dumpEllipsoid(this);

        // find apex of iso-latitude cone, somewhere along polar axis
        final double sinPhi  = FastMath.sin(latitude);
        final double sinPhi2 = sinPhi * sinPhi;
        final double e2      = getFlattening() * (2 - getFlattening());
        final double apexZ   = -getA() * e2 * sinPhi / FastMath.sqrt(1 - e2 * sinPhi2);

        // quadratic equation representing line intersection with iso-latitude cone
        // a k² + 2 b k + c = 0
        // when line of sight is almost along an iso-latitude generatrix, the quadratic
        // equation above may become unsolvable due to numerical noise (we get catastrophic
        // cancellation when computing b * b - a * c). So we set up the model in two steps,
        // first searching k₀ such that position + k₀ los is close to closeReference, and
        // then using position + k₀ los as the new initial position, which should be in
        // the neighborhood of the solution
        final double cosPhi  = FastMath.cos(latitude);
        final double cosPhi2 = cosPhi * cosPhi;
        final double k0 = Vector3D.dotProduct(closeReference.subtract(position), los) / los.getNormSq();

        final Vector3D delta  = new Vector3D(MathArrays.linearCombination(1, position.getX(), k0, los.getX()),
                                             MathArrays.linearCombination(1, position.getY(), k0, los.getY()),
                                             MathArrays.linearCombination(1, position.getZ(), k0, los.getZ(), -1.0, apexZ));
        final double   a     = MathArrays.linearCombination(+sinPhi2, los.getX() * los.getX() + los.getY() * los.getY(),
                                                            -cosPhi2, los.getZ() * los.getZ());
        final double   b      = MathArrays.linearCombination(+sinPhi2, MathArrays.linearCombination(delta.getX(), los.getX(),
                                                                                                    delta.getY(), los.getY()),
                                                             -cosPhi2, delta.getZ() * los.getZ());
        final double   c      = MathArrays.linearCombination(+sinPhi2, delta.getX() * delta.getX() + delta.getY() * delta.getY(),
                                                             -cosPhi2, delta.getZ() * delta.getZ());

        // find the two intersections along the line
        if (b * b < a * c) {
            throw new RuggedException(RuggedMessages.LINE_OF_SIGHT_NEVER_CROSSES_LATITUDE,
                                      FastMath.toDegrees(latitude));
        }
        final double s  = FastMath.sqrt(MathArrays.linearCombination(b, b, -a, c));
        final double k1 = (b > 0) ? -(s + b) / a : c / (s - b);
        final double k2 = c / (a * k1);

        // the quadratic equation has two solutions
        final boolean  k1IsOK = (delta.getZ() + k1 * los.getZ()) * latitude >= 0;
        final boolean  k2IsOK = (delta.getZ() + k2 * los.getZ()) * latitude >= 0;
        final double selectedK;
        if (k1IsOK) {
            if (k2IsOK) {
                // both solutions are in the good nappe,
                // select the one closest to the specified reference
                final double kRef = Vector3D.dotProduct(los, closeReference.subtract(position)) /
                                    los.getNormSq() - k0;
                selectedK = FastMath.abs(k1 - kRef) <= FastMath.abs(k2 - kRef) ? k1 : k2;
            } else {
                // only k1 is in the good nappe
                selectedK = k1;
            }
        } else {
            if (k2IsOK) {
                // only k2 is in the good nappe
                selectedK = k2;
            } else {
                // both solutions are in the wrong nappe,
                // there are no solutions
                throw new RuggedException(RuggedMessages.LINE_OF_SIGHT_NEVER_CROSSES_LATITUDE,
                                          FastMath.toDegrees(latitude));
            }
        }

        // compute point
        return new Vector3D(1, position, k0 + selectedK, los);

    }

    /** Get point at some longitude along a pixel line of sight.
     * @param position cell position (in body frame) (m)
     * @param los pixel line-of-sight, not necessarily normalized (in body frame)
     * @param longitude longitude with respect to ellipsoid (rad)
     * @return point at longitude (m)
     */
    public Vector3D pointAtLongitude(final Vector3D position, final Vector3D los, final double longitude) {

        DumpManager.dumpEllipsoid(this);

        // normal to meridian
        final Vector3D normal = new Vector3D(-FastMath.sin(longitude), FastMath.cos(longitude), 0);
        final double d = Vector3D.dotProduct(los, normal);
        if (FastMath.abs(d) < 1.0e-12) {
            throw new RuggedException(RuggedMessages.LINE_OF_SIGHT_NEVER_CROSSES_LONGITUDE,
                                      FastMath.toDegrees(longitude));
        }

        // compute point
        return new Vector3D(1, position, -Vector3D.dotProduct(position, normal) / d, los);

    }

    /** Get point on ground along a pixel line of sight.
     * @param position cell position (in body frame) (m)
     * @param los pixel line-of-sight, not necessarily normalized (in body frame)
     * @param centralLongitude reference longitude lc such that the point longitude will
     * be normalized between lc-π and lc+π (rad)
     * @return point on ground
     */
    public NormalizedGeodeticPoint pointOnGround(final Vector3D position, final Vector3D los,
                                                 final double centralLongitude) {

        DumpManager.dumpEllipsoid(this);
        final GeodeticPoint gp =
                getIntersectionPoint(new Line(position, new Vector3D(1, position, 1e6, los), 1.0e-12),
                        position, getBodyFrame(), null);
        if (gp == null) {
            throw new RuggedException(RuggedMessages.LINE_OF_SIGHT_DOES_NOT_REACH_GROUND);
        }
        return new NormalizedGeodeticPoint(gp.getLatitude(), gp.getLongitude(), gp.getAltitude(),
                centralLongitude);
    }

    /** Get point at some altitude along a pixel line of sight.
     * @param position cell position (in body frame) (m)
     * @param los pixel line-of-sight, not necessarily normalized (in body frame)
     * @param altitude altitude with respect to ellipsoid (m)
     * @return point at altitude (m)
     */
    public Vector3D pointAtAltitude(final Vector3D position, final Vector3D los, final double altitude) {

        DumpManager.dumpEllipsoid(this);

        // point on line closest to origin
        final double   los2   = los.getNormSq();
        final double   dot    = Vector3D.dotProduct(position, los);
        final double   k0     = -dot / los2;
        final Vector3D close0 = new Vector3D(1, position, k0, los);

        // very rough guess: if body is spherical, the desired point on line
        // is at distance ae + altitude from origin
        final double r        = getEquatorialRadius() + altitude;
        final double delta2   = r * r - close0.getNormSq();
        if (delta2 < 0) {
            throw new RuggedException(RuggedMessages.LINE_OF_SIGHT_NEVER_CROSSES_ALTITUDE, altitude);
        }
        final double deltaK   = FastMath.sqrt(delta2 / los2);
        final double k1       = k0 + deltaK;
        final double k2       = k0 - deltaK;
        double k              = (FastMath.abs(k1) <= FastMath.abs(k2)) ? k1 : k2;

        // this loop generally converges in 3 iterations
        for (int i = 0; i < 100; ++i) {

            final Vector3D      point   = new Vector3D(1, position, k, los);
            final GeodeticPoint gpK     = transform(point, getBodyFrame(), null);
            final double        deltaH  = altitude - gpK.getAltitude();
            if (FastMath.abs(deltaH) <= ALTITUDE_CONVERGENCE) {
                return point;
            }

            // improve the offset using linear ratio between
            // altitude variation and displacement along line-of-sight
            k += deltaH / Vector3D.dotProduct(gpK.getZenith(), los);

        }

        // this should never happen
        throw new RuggedException(RuggedMessages.LINE_OF_SIGHT_NEVER_CROSSES_ALTITUDE, altitude);
    }

    /** Convert a line-of-sight from Cartesian to topocentric.
     * @param point geodetic point on the line-of-sight
     * @param los line-of-sight, not necessarily normalized (in body frame and Cartesian coordinates)
     * @return line-of-sight in topocentric frame (East, North, Zenith) of the point,
     * scaled to match radians in the horizontal plane and meters along the vertical axis
     */
    public Vector3D convertLos(final GeodeticPoint point, final Vector3D los) {

        // Cartesian coordinates of the topocentric frame origin
        final Vector3D p3D = transform(point);

        // local radius of curvature in the East-West direction (parallel)
        final double r     = FastMath.hypot(p3D.getX(), p3D.getY());

        // local radius of curvature in the North-South direction (meridian)
        final double b2r   = b2 * r;
        final double b4r2  = b2r * b2r;
        final double a2z   = a2 * p3D.getZ();
        final double a4z2  = a2z * a2z;
        final double q     = a4z2 + b4r2;
        final double rho   = q * FastMath.sqrt(q) / (b2 * a4z2 + a2 * b4r2);

        final double norm = los.getNorm();
        return new Vector3D(Vector3D.dotProduct(los, point.getEast())   / (norm * r),
                            Vector3D.dotProduct(los, point.getNorth())  / (norm * rho),
                            Vector3D.dotProduct(los, point.getZenith()) / norm);

    }

    /** Convert a line-of-sight from Cartesian to topocentric.
     * @param primary reference point on the line-of-sight (in body frame and Cartesian coordinates)
     * @param secondary secondary point on the line-of-sight, only used to define a direction
     * with respect to the primary point (in body frame and Cartesian coordinates)
     * @return line-of-sight in topocentric frame (East, North, Zenith) of the point,
     * scaled to match radians in the horizontal plane and meters along the vertical axis
     */
    public Vector3D convertLos(final Vector3D primary, final Vector3D secondary) {

        // switch to geodetic coordinates using primary point as reference
        final GeodeticPoint point = transform(primary, getBodyFrame(), null);
        final Vector3D      los   = secondary.subtract(primary);

        // convert line of sight
        return convertLos(point, los);
    }

    /** Transform a cartesian point to a surface-relative point.
     * @param point cartesian point (m)
     * @param frame frame in which cartesian point is expressed
     * @param date date of the computation (used for frames conversions)
     * @param centralLongitude reference longitude lc such that the point longitude will
     * be normalized between lc-π and lc+π (rad)
     * @return point at the same location but as a surface-relative point
     */
    public NormalizedGeodeticPoint transform(final Vector3D point, final Frame frame, final AbsoluteDate date,
                                             final double centralLongitude) {
        final GeodeticPoint gp = transform(point, frame, date);
        return new NormalizedGeodeticPoint(gp.getLatitude(), gp.getLongitude(), gp.getAltitude(),
                                           centralLongitude);
    }

}