You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
617 lines
20 KiB
617 lines
20 KiB
/*
|
|
* 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) {
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
}
|
|
|