xref: /external/jmonkeyengine/engine/src/desktop/jme3tools/navigation/NavCalculator.java

/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package jme3tools.navigation;



/**
 * A utlity class for performing position calculations
 *
 * @author Benjamin Jakobus, based on JMarine (by Cormac Gebruers and Benjamin
 *          Jakobus)
 * @version 1.0
 * @since 1.0
 */
public class NavCalculator {

    private double distance;
    private double trueCourse;

    /* The earth's radius in meters */
    public static final int WGS84_EARTH_RADIUS = 6378137;
    private String strCourse;

    /* The sailing calculation type */
    public static final int MERCATOR = 0;
    public static final int GC = 1;

    /* The degree precision to use for courses */
    public static final int RL_CRS_PRECISION = 1;

    /* The distance precision to use for distances */
    public static final int RL_DIST_PRECISION = 1;
    public static final int METERS_PER_MINUTE = 1852;

    /**
     * Constructor
     * @param P1
     * @param P2
     * @param calcType
     * @since 1.0
     */
    public NavCalculator(Position P1, Position P2, int calcType) {
        switch (calcType) {
            case MERCATOR:
                mercatorSailing(P1, P2);
                break;
            case GC:
                greatCircleSailing(P1, P2);
                break;
        }
    }

    /**
     * Constructor
     * @since 1.0
     */
    public NavCalculator() {
    }

    /**
     * Determines a great circle track between two positions
     * @param p1 origin position
     * @param p2 destination position
     */
    public GCSailing greatCircleSailing(Position p1, Position p2) {
        return new GCSailing(new int[0], new float[0]);
    }

    /**
     * Determines a Rhumb Line course and distance between two points
     * @param p1 origin position
     * @param p2 destination position
     */
    public RLSailing rhumbLineSailing(Position p1, Position p2) {
        RLSailing rl = mercatorSailing(p1, p2);
        return rl;
    }

    /**
     * Determines the rhumb line course  and distance between two positions
     * @param p1 origin position
     * @param p2 destination position
     */
    public RLSailing mercatorSailing(Position p1, Position p2) {

        double dLat = computeDLat(p1.getLatitude(), p2.getLatitude());
        //plane sailing...
        if (dLat == 0) {
            RLSailing rl = planeSailing(p1, p2);
            return rl;
        }

        double dLong = computeDLong(p1.getLongitude(), p2.getLongitude());
        double dmp = (float) computeDMPClarkeSpheroid(p1.getLatitude(), p2.getLatitude());

        trueCourse = (float) Math.toDegrees(Math.atan(dLong / dmp));
        double degCrs = convertCourse((float) trueCourse, p1, p2);
        distance = (float) Math.abs(dLat / Math.cos(Math.toRadians(trueCourse)));

        RLSailing rl = new RLSailing(degCrs, (float) distance);
        trueCourse = rl.getCourse();
        strCourse = (dLat < 0 ? "S" : "N");
        strCourse += " " + trueCourse;
        strCourse += " " + (dLong < 0 ? "W" : "E");
        return rl;

    }

    /**
     * Calculate a plane sailing situation - i.e. where Lats are the same
     * @param p1
     * @param p2
     * @return
     * @since 1.0
     */
    public RLSailing planeSailing(Position p1, Position p2) {
        double dLong = computeDLong(p1.getLongitude(), p2.getLongitude());

        double sgnDLong = 0 - (dLong / Math.abs(dLong));
        if (Math.abs(dLong) > 180 * 60) {
            dLong = (360 * 60 - Math.abs(dLong)) * sgnDLong;
        }

        double redist = 0;
        double recourse = 0;
        if (p1.getLatitude() == 0) {
            redist = Math.abs(dLong);
        } else {
            redist = Math.abs(dLong * (float) Math.cos(p1.getLatitude() * 2 * Math.PI / 360));
        }
        recourse = (float) Math.asin(0 - sgnDLong);
        recourse = recourse * 360 / 2 / (float) Math.PI;

        if (recourse < 0) {
            recourse = recourse + 360;
        }
        return new RLSailing(recourse, redist);
    }

    /**
     * Converts a course from cardinal XddY to ddd notation
     * @param tc
     * @param p1 position one
     * @param p2 position two
     * @return
     * @since 1.0
     */
    public static double convertCourse(float tc, Position p1, Position p2) {

        double dLat = p1.getLatitude() - p2.getLatitude();
        double dLong = p1.getLongitude() - p2.getLongitude();
        //NE
        if (dLong >= 0 & dLat >= 0) {
            return Math.abs(tc);
        }

        //SE
        if (dLong >= 0 & dLat < 0) {
            return 180 - Math.abs(tc);
        }

        //SW
        if (dLong < 0 & dLat < 0) {
            return 180 + Math.abs(tc);
        }

        //NW
        if (dLong < 0 & dLat >= 0) {
            return 360 - Math.abs(tc);
        }
        return -1;
    }

    /**
     * Getter method for the distance between two points
     * @return distance
     * @since 1.0
     */
    public double getDistance() {
        return distance;
    }

    /**
     * Getter method for the true course
     * @return true course
     * @since 1.0
     */
    public double getTrueCourse() {
        return trueCourse;
    }

    /**
     * Getter method for the true course
     * @return true course
     * @since 1.0
     */
    public String getStrCourse() {
        return strCourse;
    }

    /**
     * Computes the difference in meridional parts for two latitudes in minutes
     * (based on Clark 1880 spheroid)
     * @param lat1
     * @param lat2
     * @return difference in minutes
     * @since 1.0
     */
    public static double computeDMPClarkeSpheroid(double lat1, double lat2) {
        double absLat1 = Math.abs(lat1);
        double absLat2 = Math.abs(lat2);

        double m1 = (7915.704468 * (Math.log(Math.tan(Math.toRadians(45
                + (absLat1 / 2)))) / Math.log(10))
                - 23.268932 * Math.sin(Math.toRadians(absLat1))
                - 0.052500 * Math.pow(Math.sin(Math.toRadians(absLat1)), 3)
                - 0.000213 * Math.pow(Math.sin(Math.toRadians(absLat1)), 5));

        double m2 = (7915.704468 * (Math.log(Math.tan(Math.toRadians(45
                + (absLat2 / 2)))) / Math.log(10))
                - 23.268932 * Math.sin(Math.toRadians(absLat2))
                - 0.052500 * Math.pow(Math.sin(Math.toRadians(absLat2)), 3)
                - 0.000213 * Math.pow(Math.sin(Math.toRadians(absLat2)), 5));
        if ((lat1 <= 0 && lat2 <= 0) || (lat1 > 0 && lat2 > 0)) {
            return Math.abs(m1 - m2);
        } else {
            return m1 + m2;
        }
    }

    /**
     * Computes the difference in meridional parts for a perfect sphere between
     * two degrees of latitude
     * @param lat1
     * @param lat2
     * @return difference in meridional parts between lat1 and lat2 in minutes
     * @since 1.0
     */
    public static float computeDMPWGS84Spheroid(float lat1, float lat2) {
        float absLat1 = Math.abs(lat1);
        float absLat2 = Math.abs(lat2);

        float m1 = (float) (7915.7045 * Math.log10(Math.tan(Math.toRadians(45 + (absLat1 / 2))))
                - 23.01358 * Math.sin(absLat1 - 0.05135) * Math.pow(Math.sin(absLat1), 3));

        float m2 = (float) (7915.7045 * Math.log10(Math.tan(Math.toRadians(45 + (absLat2 / 2))))
                - 23.01358 * Math.sin(absLat2 - 0.05135) * Math.pow(Math.sin(absLat2), 3));

        if (lat1 <= 0 & lat2 <= 0 || lat1 > 0 & lat2 > 0) {
            return Math.abs(m1 - m2);
        } else {
            return m1 + m2;
        }
    }

    /**
     * Predicts the position of a target for a given time in the future
     * @param time the number of seconds from now for which to predict the future
     *        position
     * @param speed the miles per minute that the target is traveling
     * @param currentLat the target's current latitude
     * @param currentLong the target's current longitude
     * @param course the target's current course in degrees
     * @return the predicted future position
     * @since 1.0
     */
    public static Position predictPosition(int time, double speed,
            double currentLat, double currentLong, double course) {
        Position futurePosition = null;
        course = Math.toRadians(course);
        double futureLong = currentLong + speed * time * Math.sin(course);
        double futureLat = currentLat + speed * time * Math.cos(course);
        try {
            futurePosition = new Position(futureLat, futureLong);
        } catch (InvalidPositionException ipe) {
            ipe.printStackTrace();
        }
        return futurePosition;

    }

    /**
     * Computes the coordinate of position B relative to an offset given
     * a distance and an angle.
     *
     * @param offset        The offset position.
     * @param bearing       The bearing between the offset and the coordinate
     *                      that you want to calculate.
     * @param distance      The distance, in meters, between the offset
     *                      and point B.
     * @return              The position of point B that is located from
     *                      given offset at given distance and angle.
     * @since 1.0
     */
    public static Position computePosition(Position initialPos, double heading,
            double distance) {
        if (initialPos == null) {
            return null;
        }
        double angle;
        if (heading < 90) {
            angle = heading;
        } else if (heading > 90 && heading < 180) {
            angle = 180 - heading;
        } else if (heading > 180 && heading < 270) {
            angle = heading - 180;
        } else {
            angle = 360 - heading;
        }

        Position newPosition = null;

        // Convert meters into nautical miles
        distance = distance * 0.000539956803;
        angle = Math.toRadians(angle);
        double initialLat = initialPos.getLatitude();
        double initialLong = initialPos.getLongitude();
        double dlat = distance * Math.cos(angle);
        dlat = dlat / 60;
        dlat = Math.abs(dlat);
        double newLat = 0;
        if ((heading > 270 && heading < 360) || (heading > 0 && heading < 90)) {
            newLat = initialLat + dlat;
        } else if (heading < 270 && heading > 90) {
            newLat = initialLat - dlat;
        }
        double meanLat = (Math.abs(dlat) / 2.0) + newLat;
        double dep = (Math.abs(dlat * 60)) * Math.tan(angle);
        double dlong = dep * (1.0 / Math.cos(Math.toRadians(meanLat)));
        dlong = dlong / 60;
        dlong = Math.abs(dlong);
        double newLong;
        if (heading > 180 && heading < 360) {
            newLong = initialLong - dlong;
        } else {
            newLong = initialLong + dlong;
        }

        if (newLong < -180) {
            double diff = Math.abs(newLong + 180);
            newLong = 180 - diff;
        }

        if (newLong > 180) {
            double diff = Math.abs(newLong + 180);
            newLong = (180 - diff) * -1;
        }

        if (heading == 0 || heading == 360 || heading == 180) {
            newLong = initialLong;
            newLat = initialLat + dlat;
        } else if (heading == 90 || heading == 270) {
            newLat = initialLat;
//            newLong = initialLong + dlong; THIS WAS THE ORIGINAL (IT WORKED)
            newLong = initialLong - dlong;
        }
        try {
            newPosition = new Position(newLat,
                    newLong);
        } catch (InvalidPositionException ipe) {
            ipe.printStackTrace();
            System.out.println(newLat + "," + newLong);
        }
        return newPosition;
    }

    /**
     * Computes the difference in Longitude between two positions and assigns the
     * correct sign -westwards travel, + eastwards travel
     * @param lng1
     * @param lng2
     * @return difference in longitude
     * @since 1.0
     */
    public static double computeDLong(double lng1, double lng2) {
        if (lng1 - lng2 == 0) {
            return 0;
        }

        // both easterly
        if (lng1 >= 0 & lng2 >= 0) {
            return -(lng1 - lng2) * 60;
        }
        //both westerly
        if (lng1 < 0 & lng2 < 0) {
            return -(lng1 - lng2) * 60;
        }

        //opposite sides of Date line meridian

        //sum less than 180
        if (Math.abs(lng1) + Math.abs(lng2) < 180) {
            if (lng1 < 0 & lng2 > 0) {
                return -(Math.abs(lng1) + Math.abs(lng2)) * 60;
            } else {
                return Math.abs(lng1) + Math.abs(lng2) * 60;
            }
        } else {
            //sum greater than 180
            if (lng1 < 0 & lng2 > 0) {
                return -(360 - (Math.abs(lng1) + Math.abs(lng2))) * 60;
            } else {
                return (360 - (Math.abs(lng1) + Math.abs(lng2))) * 60;
            }
        }
    }

    /**
     * Computes the difference in Longitude between two positions and assigns the
     * correct sign -westwards travel, + eastwards travel
     * @param lng1
     * @param lng2
     * @return difference in longitude
     * @since 1.0
     */
    public static double computeLongDiff(double lng1, double lng2) {
        if (lng1 - lng2 == 0) {
            return 0;
        }

        // both easterly
        if (lng1 >= 0 & lng2 >= 0) {
            return Math.abs(-(lng1 - lng2) * 60);
        }
        //both westerly
        if (lng1 < 0 & lng2 < 0) {
            return Math.abs(-(lng1 - lng2) * 60);
        }

        if (lng1 == 0) {
            return Math.abs(lng2 * 60);
        }

        if (lng2 == 0) {
            return Math.abs(lng1 * 60);
        }

        return (Math.abs(lng1) + Math.abs(lng2)) * 60;
    }

    /**
     * Compute the difference in latitude between two positions
     * @param lat1
     * @param lat2
     * @return difference in latitude
     * @since 1.0
     */
    public static double computeDLat(double lat1, double lat2) {
        //same side of equator

        //plane sailing
        if (lat1 - lat2 == 0) {
            return 0;
        }

        //both northerly
        if (lat1 >= 0 & lat2 >= 0) {
            return -(lat1 - lat2) * 60;
        }
        //both southerly
        if (lat1 < 0 & lat2 < 0) {
            return -(lat1 - lat2) * 60;
        }

        //opposite sides of equator
        if (lat1 >= 0) {
            //heading south
            return -(Math.abs(lat1) + Math.abs(lat2));
        } else {
            //heading north
            return (Math.abs(lat1) + Math.abs(lat2));
        }
    }

    public static class Quadrant {

        private static final Quadrant FIRST = new Quadrant(1, 1);
        private static final Quadrant SECOND = new Quadrant(-1, 1);
        private static final Quadrant THIRD = new Quadrant(-1, -1);
        private static final Quadrant FOURTH = new Quadrant(1, -1);
        private final int lonMultiplier;
        private final int latMultiplier;

        public Quadrant(final int xMultiplier, final int yMultiplier) {
            this.lonMultiplier = xMultiplier;
            this.latMultiplier = yMultiplier;
        }

        static Quadrant getQuadrant(double degrees, boolean invert) {
            if (invert) {
                if (degrees >= 0 && degrees <= 90) {
                    return FOURTH;
                } else if (degrees > 90 && degrees <= 180) {
                    return THIRD;
                } else if (degrees > 180 && degrees <= 270) {
                    return SECOND;
                }
                return FIRST;
            } else {
                if (degrees >= 0 && degrees <= 90) {
                    return FIRST;
                } else if (degrees > 90 && degrees <= 180) {
                    return SECOND;
                } else if (degrees > 180 && degrees <= 270) {
                    return THIRD;
                }
                return FOURTH;
            }
        }
    }

    /**
     * Converts meters to degrees.
     *
     * @param meters            The meters that you want to convert into degrees.
     * @return                  The degree equivalent of the given meters.
     * @since 1.0
     */
    public static double toDegrees(double meters) {
        return (meters / METERS_PER_MINUTE) / 60;
    }

    /**
     * Computes the bearing between two points.
     *
     * @param p1
     * @param p2
     * @return
     * @since 1.0
     */
    public static int computeBearing(Position p1, Position p2) {
        int bearing;
        double dLon = computeDLong(p1.getLongitude(), p2.getLongitude());
        double y = Math.sin(dLon) * Math.cos(p2.getLatitude());
        double x = Math.cos(p1.getLatitude()) * Math.sin(p2.getLatitude())
                - Math.sin(p1.getLatitude()) * Math.cos(p2.getLatitude()) * Math.cos(dLon);
        bearing = (int) Math.toDegrees(Math.atan2(y, x));
        return bearing;
    }

    /**
     * Computes the angle between two points.
     *
     * @param p1
     * @param p2
     * @return
     */
    public static int computeAngle(Position p1, Position p2) {
        // cos (adj / hyp)
        double adj = Math.abs(p1.getLongitude() - p2.getLongitude());
        double opp = Math.abs(p1.getLatitude() - p2.getLatitude());
        return (int) Math.toDegrees(Math.atan(opp / adj));

//        int angle = (int)Math.atan2(p2.getLatitude() - p1.getLatitude(),
//                p2.getLongitude() - p1.getLongitude());
        //Actually it's ATan2(dy , dx) where dy = y2 - y1 and dx = x2 - x1, or ATan(dy / dx)
    }

    public static int computeHeading(Position p1, Position p2) {
        int angle = computeAngle(p1, p2);
        // NE
        if (p2.getLongitude() >= p1.getLongitude() && p2.getLatitude() >= p1.getLatitude()) {
            return angle;
        } else if (p2.getLongitude() >= p1.getLongitude() && p2.getLatitude() <= p1.getLatitude()) {
            // SE
            return 90 + angle;
        } else if (p2.getLongitude() <= p1.getLongitude() && p2.getLatitude() <= p1.getLatitude()) {
            // SW
            return 270 - angle;
        } else {
            // NW
            return 270 + angle;
        }
    }

    public static void main(String[] args) {
        try {
            int pos = NavCalculator.computeHeading(new Position(0, 0), new Position(10, -10));
//            System.out.println(pos.getLatitude() + "," + pos.getLongitude());
            System.out.println(pos);
        } catch (Exception e) {
        }





    }
}