|
@@ -1,617 +0,0 @@
|
|
|
-/*
|
|
|
- * Copyright (c) 2009-2019 jMonkeyEngine
|
|
|
- * All rights reserved.
|
|
|
- *
|
|
|
- * Redistribution and use in source and binary forms, with or without
|
|
|
- * modification, are permitted provided that the following conditions are
|
|
|
- * met:
|
|
|
- *
|
|
|
- * * Redistributions of source code must retain the above copyright
|
|
|
- * notice, this list of conditions and the following disclaimer.
|
|
|
- *
|
|
|
- * * Redistributions in binary form must reproduce the above copyright
|
|
|
- * notice, this list of conditions and the following disclaimer in the
|
|
|
- * documentation and/or other materials provided with the distribution.
|
|
|
- *
|
|
|
- * * Neither the name of 'jMonkeyEngine' nor the names of its contributors
|
|
|
- * may be used to endorse or promote products derived from this software
|
|
|
- * without specific prior written permission.
|
|
|
- *
|
|
|
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
|
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
|
|
|
- * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
|
|
- * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
|
|
|
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
|
|
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
|
|
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
|
|
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
|
|
|
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
|
|
|
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
|
|
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
|
- */
|
|
|
-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 a new instance
|
|
|
- * @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 angle (in degrees)
|
|
|
- * @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 initialPos
|
|
|
- * @param heading
|
|
|
- * @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 bearing (in degrees)
|
|
|
- * @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 angle (in degrees)
|
|
|
- */
|
|
|
- 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) {
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- }
|
|
|
-}
|