package au.gov.amsa.util.navigation;
import static java.lang.Math.PI;
import static java.lang.Math.abs;
import static java.lang.Math.acos;
import static java.lang.Math.asin;
import static java.lang.Math.atan;
import static java.lang.Math.atan2;
import static java.lang.Math.cos;
import static java.lang.Math.floor;
import static java.lang.Math.round;
import static java.lang.Math.signum;
import static java.lang.Math.sin;
import static java.lang.Math.sqrt;
import static java.lang.Math.toRadians;
import java.awt.Polygon;
import java.text.DecimalFormat;
import java.util.ArrayList;
import java.util.List;
import org.apache.commons.math3.util.FastMath;
/**
* Can use commons-math3 FastMath for most trig functions exception for
* atan,atan2 (see <a
* href="https://issues.apache.org/jira/browse/MATH-740">here</a>).
*
* @author dxm
*
*/
public class Position {
private final double lat;
private final double lon;
private final double alt;
private static final double radiusEarthKm = 6371.01;
private static final double circumferenceEarthKm = 2.0 * PI * radiusEarthKm;
/**
* @param lat
* in degrees
* @param lon
* in degrees
*/
public Position(double lat, double lon) {
this.lat = lat;
this.lon = lon;
this.alt = 0.0;
}
/**
* @param lat
* in degrees
* @param lon
* in degrees
* @param alt
* in metres
*/
public Position(double lat, double lon, double alt) {
this.lat = lat;
this.lon = lon;
this.alt = alt;
}
public static Position create(double lat, double lon) {
return new Position(lat, lon);
}
public static Position create(double lat, double lon, double alt) {
return new Position(lat, lon, alt);
}
public final double getLat() {
return lat;
}
public final double getLon() {
return lon;
}
public final double getAlt() {
return alt;
}
@Override
public final String toString() {
return "[" + lat + "," + lon + "]";
}
/**
* Predicts position travelling along a great circle arc based on the
* Haversine formula.
*
* From http://www.movable-type.co.uk/scripts/latlong.html
*
* @param distanceKm
* @param courseDegrees
* @return
*/
public final Position predict(double distanceKm, double courseDegrees) {
assertWithMsg(alt == 0.0, "Predictions only valid for Earth's surface");
double dr = distanceKm / radiusEarthKm;
double latR = toRadians(lat);
double lonR = toRadians(lon);
double courseR = toRadians(courseDegrees);
double lat2Radians = asin(sin(latR) * cos(dr) + cos(latR) * sin(dr)
* cos(courseR));
double lon2Radians = atan2(sin(courseR) * sin(dr) * cos(latR), cos(dr)
- sin(latR) * sin(lat2Radians));
double lon3Radians = mod(lonR + lon2Radians + PI, 2 * PI) - PI;
return new Position(FastMath.toDegrees(lat2Radians),
FastMath.toDegrees(lon3Radians));
}
public static double toDegrees(double degrees, double minutes,
double seconds) {
return degrees + minutes / 60.0 + seconds / 3600.0;
}
/**
* From http://williams.best.vwh.net/avform.htm (Latitude of point on GC).
*
* @param position
* @param longitudeDegrees
* @return
*/
public Double getLatitudeOnGreatCircle(Position position,
double longitudeDegrees) {
double lonR = toRadians(longitudeDegrees);
double lat1R = toRadians(lat);
double lon1R = toRadians(lon);
double lat2R = toRadians(position.getLat());
double lon2R = toRadians(position.getLon());
double sinDiffLon1RLon2R = sin(lon1R - lon2R);
if (abs(sinDiffLon1RLon2R) < 0.00000001) {
return null;
} else {
double cosLat1R = cos(lat1R);
double cosLat2R = cos(lat2R);
double numerator = sin(lat1R) * cosLat2R * sin(lonR - lon2R)
- sin(lat2R) * cosLat1R * sin(lonR - lon1R);
double denominator = cosLat1R * cosLat2R * sinDiffLon1RLon2R;
double radians = atan(numerator / denominator);
return FastMath.toDegrees(radians);
}
}
public static class LongitudePair {
private final double lon1, lon2;
public LongitudePair(double lon1, double lon2) {
this.lon1 = lon1;
this.lon2 = lon2;
}
public double getLon1() {
return lon1;
}
public double getLon2() {
return lon2;
}
@Override
public String toString() {
return "LongitudePair [lon1=" + lon1 + ", lon2=" + lon2 + "]";
}
}
/**
* Returns null if no crossing of latitude otherwise return two longitude
* candidates. From http://williams.best.vwh.net/avform.htm (Crossing
* parallels).
*
* @param position
* @param latitudeDegrees
* @return
*/
// TODO add unit test
public LongitudePair getLongitudeOnGreatCircle(Position position,
double latitudeDegrees) {
double lat3 = toRadians(latitudeDegrees);
double lat1 = toRadians(lat);
double lon1 = toRadians(lon);
double lat2 = toRadians(position.getLat());
double lon2 = toRadians(position.getLon());
double l12 = lon1 - lon2;
double sinLat1 = sin(lat1);
double cosLat2 = cos(lat2);
double cosLat3 = cos(lat3);
double cosLat1 = cos(lat1);
double sinL12 = sin(l12);
double A = sinLat1 * cosLat2 * cosLat3 * sinL12;
double B = sinLat1 * cosLat2 * cosLat3 * cos(l12) - cosLat1 * sin(lat2)
* cosLat3;
double C = cosLat1 * cosLat2 * sin(lat3) * sinL12;
double longitude = atan2(B, A);
double v = sqrt(sqr(A) + sqr(B));
if (abs(C) >= v) {
// not found!
return null;
} else {
double dlon = acos(C / v);
double lonCandidate1Degrees = to180(FastMath.toDegrees(lon1 + dlon
+ longitude));
double lonCandidate2Degrees = to180(FastMath.toDegrees(lon1 - dlon
+ longitude));
return new LongitudePair(lonCandidate1Degrees, lonCandidate2Degrees);
}
}
private double sqr(double d) {
return d * d;
}
/**
* Return an array of Positions representing the earths limb (aka: horizon)
* as viewed from this Position in space. This position must have altitude >
* 0
*
* The array returned will have the specified number of elements (radials).
*
*
* This method is useful for the calculation of satellite footprints or the
* position of the Earth's day/night terminator.
*
*
* This formula from Aviation Formula by Ed Williams
* (http://williams.best.vwh.net/avform.htm)
*
* @param radials
* the number of radials to calculated (evenly spaced around the
* circumference of the circle
*
* @return An array of radial points a fixed distance from this point
* representing the Earth's limb as viewed from this point in space.
*
*/
public final Position[] getEarthLimb(int radials) {
Position[] result = new Position[radials];
double radialDegrees = 0.0;
double incDegrees = 360.0 / radials;
double quarterEarthKm = circumferenceEarthKm / 4.0;
Position surfacePosition = new Position(this.lat, this.lon, 0.0);
// Assert( this.alt>0.0, "getEarthLimb() requires Position a positive
// altitude");
for (int i = 0; i < radials; i++) {
// TODO: base the distance on the altitude above the Earth
result[i] = surfacePosition.predict(quarterEarthKm, radialDegrees);
radialDegrees += incDegrees;
}
return result;
}
/**
* returns distance between two WGS84 positions according to Vincenty's
* formula from Wikipedia
*
* @param position
* @return
*/
public final double getDistanceToKm(Position position) {
double lat1 = toRadians(lat);
double lat2 = toRadians(position.lat);
double lon1 = toRadians(lon);
double lon2 = toRadians(position.lon);
double deltaLon = lon2 - lon1;
double cosLat2 = cos(lat2);
double cosLat1 = cos(lat1);
double sinLat1 = sin(lat1);
double sinLat2 = sin(lat2);
double cosDeltaLon = cos(deltaLon);
double top = sqrt(sqr(cosLat2 * sin(deltaLon))
+ sqr(cosLat1 * sinLat2 - sinLat1 * cosLat2 * cosDeltaLon));
double bottom = sinLat1 * sinLat2 + cosLat1 * cosLat2 * cosDeltaLon;
double distance = radiusEarthKm * atan2(top, bottom);
return abs(distance);
}
/**
* Returns a great circle bearing in degrees in the range 0 to 360.
*
* @param position
* @return
*/
public final double getBearingDegrees(Position position) {
double lat1 = toRadians(lat);
double lat2 = toRadians(position.lat);
double lon1 = toRadians(lon);
double lon2 = toRadians(position.lon);
double dLon = lon2 - lon1;
double sinDLon = sin(dLon);
double cosLat2 = cos(lat2);
double y = sinDLon * cosLat2;
double x = cos(lat1) * sin(lat2) - sin(lat1) * cosLat2 * cos(dLon);
double course = FastMath.toDegrees(atan2(y, x));
if (course < 0)
course += 360;
return course;
}
/**
* returns difference in degrees in the range -180 to 180
*
* @param bearing1
* degrees between -360 and 360
* @param bearing2
* degrees between -360 and 360
* @return
*/
public static double getBearingDifferenceDegrees(double bearing1,
double bearing2) {
if (bearing1 < 0)
bearing1 += 360;
if (bearing2 > 180)
bearing2 -= 360;
double result = bearing1 - bearing2;
if (result > 180)
result -= 360;
return result;
}
/**
* calculates the distance of a point to the great circle path between p1
* and p2.
*
* Formula from: http://www.movable-type.co.uk/scripts/latlong.html
*
* @param p1
* @param p2
* @return
*/
public final double getDistanceKmToPath(Position p1, Position p2) {
double d = radiusEarthKm
* asin(sin(getDistanceToKm(p1) / radiusEarthKm)
* sin(toRadians(getBearingDegrees(p1)
- p1.getBearingDegrees(p2))));
return abs(d);
}
public static String toDegreesMinutesDecimalMinutesLatitude(double lat) {
long degrees = round(signum(lat) * floor(abs(lat)));
double remaining = abs(lat - degrees);
remaining *= 60;
String result = abs(degrees) + "" + (char) 0x00B0
+ new DecimalFormat("00.00").format(remaining) + "'"
+ (lat < 0 ? "S" : "N");
return result;
}
public static String toDegreesMinutesDecimalMinutesLongitude(double lon) {
long degrees = round(signum(lon) * floor(abs(lon)));
double remaining = abs(lon - degrees);
remaining *= 60;
String result = abs(degrees) + "" + (char) 0x00B0
+ new DecimalFormat("00.00").format(remaining) + "'"
+ (lon < 0 ? "W" : "E");
return result;
}
private static double mod(double y, double x) {
x = abs(x);
int n = (int) (y / x);
double mod = y - x * n;
if (mod < 0) {
mod += x;
}
return mod;
}
public static void assertWithMsg(boolean assertion, String msg) {
if (!assertion)
throw new RuntimeException("Assertion failed: " + msg);
}
/**
* Returns a position along a path according to the proportion value
*
* @param position
* @param proportion
* is between 0 and 1 inclusive
* @return
*/
public final Position getPositionAlongPath(Position position,
double proportion) {
if (proportion >= 0 && proportion <= 1) {
// Get bearing degrees for course
double courseDegrees = this.getBearingDegrees(position);
// Get distance from position arg and this objects location
double distanceKm = this.getDistanceToKm(position);
// Predict the position for a proportion of the course
// where this object is the start position and the arg
// is the destination position.
Position retPosition = this.predict(proportion * distanceKm,
courseDegrees);
return retPosition;
} else
throw new RuntimeException(
"Proportion must be between 0 and 1 inclusive");
}
public final List<Position> getPositionsAlongPath(Position position,
double maxSegmentLengthKm) {
// Get distance from this to position
double distanceKm = this.getDistanceToKm(position);
List<Position> positions = new ArrayList<Position>();
long numSegments = round(floor(distanceKm / maxSegmentLengthKm)) + 1;
positions.add(this);
for (int i = 1; i < numSegments; i++)
positions.add(getPositionAlongPath(position, i
/ (double) numSegments));
positions.add(position);
return positions;
}
public final Position to360() {
double lat = this.lat;
double lon = this.lon;
if (lon < 0)
lon += 360;
return new Position(lat, lon);
}
/**
* normalize the lat lon values of this to ensure that no large longitude
* jumps are made from lastPosition (e.g. 179 to -180)
*
* @param lastPosition
*/
public final Position ensureContinuous(Position lastPosition) {
double lon = this.lon;
if (abs(lon - lastPosition.lon) > 180) {
if (lastPosition.lon < 0)
lon -= 360;
else
lon += 360;
return new Position(lat, lon);
} else
return this;
}
public final boolean isWithin(List<Position> positions) {
Polygon polygon = new Polygon();
for (Position p : positions) {
polygon.addPoint(degreesToArbitraryInteger(p.lon),
degreesToArbitraryInteger(p.lat));
}
int x = degreesToArbitraryInteger(this.lon);
int y = degreesToArbitraryInteger(this.lat);
return polygon.contains(x, y);
}
private int degreesToArbitraryInteger(double d) {
return (int) round(d * 3600);
}
@Override
public final boolean equals(Object o) {
if (o == null)
return false;
else if (o instanceof Position) {
Position p = (Position) o;
return p.lat == lat && p.lon == lon;
} else
return false;
}
@Override
public final int hashCode() {
return (int) (lat + lon);
}
public final double getDistanceToPathKm(List<Position> positions) {
if (positions.size() == 0)
throw new RuntimeException("positions must not be empty");
else if (positions.size() == 1)
return this.getDistanceToKm(positions.get(0));
else {
Double distance = null;
for (int i = 0; i < positions.size() - 1; i++) {
double d = getDistanceToSegmentKm(positions.get(i),
positions.get(i + 1));
if (distance == null || d < distance)
distance = d;
}
return distance;
}
}
public final double getDistanceToSegmentKm(Position p1, Position p2) {
return getDistanceToKm(getClosestIntersectionWithSegment(p1, p2));
}
public final Position getClosestIntersectionWithSegment(Position p1,
Position p2) {
if (p1.equals(p2))
return p1;
double d = getDistanceToKm(p1);
double bearing1 = p1.getBearingDegrees(this);
double bearing2 = p1.getBearingDegrees(p2);
double bearingDiff = bearing1 - bearing2;
double proportion = d * cos(toRadians(bearingDiff))
/ p1.getDistanceToKm(p2);
if (proportion < 0 || proportion > 1) {
if (d < getDistanceToKm(p2))
return p1;
else
return p2;
} else
return p1.getPositionAlongPath(p2, proportion);
}
/**
* @param path
* @param minDistanceKm
* @return
*/
public boolean isOutside(List<Position> path, double minDistanceKm) {
if (isWithin(path))
return false;
else {
double distance = getDistanceToPathKm(path);
return distance >= minDistanceKm;
}
}
/**
* Returns the difference between two longitude values. The returned value
* is always >=0.
*
* @param a
* @param b
* @return
*/
public static double longitudeDiff(double a, double b) {
a = to180(a);
b = to180(b);
if (a < b)
return a - b + 360;
else
return a - b;
}
/**
* Converts an angle in degrees to range -180< x <= 180.
*
* @param d
* @return
*/
public static double to180(double d) {
if (d < 0)
return -to180(abs(d));
else {
if (d > 180) {
long n = round(floor((d + 180) / 360.0));
return d - n * 360;
} else
return d;
}
}
public Position normalizeLongitude() {
return new Position(lat, to180(lon), alt);
}
}