/*
* Copyright (c) 2009-2012 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
* @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) {
}
}
}