altoslib: Add range and elevation to AltosGreatCircle

Move the computations from AltosState here so they can be re-used elsewhere.

Signed-off-by: Keith Packard <keithp@keithp.com>

2 files changed, 26 insertions, 16 deletions

diff --git a/altoslib/AltosGreatCircle.java b/altoslib/AltosGreatCircle.java
index 921356a5..f1cf0ae9 100644
--- a/altoslib/AltosGreatCircle.java
+++ b/altoslib/AltosGreatCircle.java
@@ -22,6 +22,8 @@ import java.lang.Math;
 public class AltosGreatCircle {
 	public double	distance;
 	public double	bearing;
+	public double	range;
+	public double	elevation;
 
 	double sqr(double a) { return a * a; }
 
@@ -54,9 +56,8 @@ public class AltosGreatCircle {
 		return bearing_string[length][(int)((bearing / 90 * 8 + 1) / 2)%16];
 	}
 
-	public AltosGreatCircle (double start_lat, double start_lon,
-				 double end_lat, double end_lon)
-	{
+	public AltosGreatCircle (double start_lat, double start_lon, double start_alt,
+				 double end_lat, double end_lon, double end_alt) {
 		double lat1 = rad * start_lat;
 		double lon1 = rad * -start_lon;
 		double lat2 = rad * end_lat;
@@ -88,14 +89,25 @@ public class AltosGreatCircle {
 		}
 		distance = d * earth_radius;
 		bearing = course * 180/Math.PI;
+
+		double height_diff = end_alt - start_alt;
+		range = Math.sqrt(distance * distance + height_diff * height_diff);
+		elevation = Math.atan2(height_diff, distance) * 180 / Math.PI;
+	}
+
+	public AltosGreatCircle (double start_lat, double start_lon,
+				 double end_lat, double end_lon) {
+		this(start_lat, start_lon, 0, end_lat, end_lon, 0);
 	}
 
 	public AltosGreatCircle(AltosGPS start, AltosGPS end) {
-		this(start.lat, start.lon, end.lat, end.lon);
+		this(start.lat, start.lon, start.alt, end.lat, end.lon, end.alt);
 	}
 
 	public AltosGreatCircle() {
 		distance = 0;
 		bearing = 0;
+		range = 0;
+		elevation = 0;
 	}
 }
diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java
index f1bcb1c1..a3b9a8c0 100644
--- a/altoslib/AltosState.java
+++ b/altoslib/AltosState.java
@@ -248,23 +248,21 @@ public class AltosState {
 
 		if (height != AltosRecord.MISSING && height > max_height)
 			max_height = height;
+		elevation = 0;
+		range = -1;
+		gps_height = 0;
 		if (data.gps != null) {
 			if (gps == null || !gps.locked || data.gps.locked)
 				gps = data.gps;
 			if (ngps > 0 && gps.locked) {
-				from_pad = new AltosGreatCircle(pad_lat, pad_lon, gps.lat, gps.lon);
-			}
-		}
-		elevation = 0;
-		range = -1;
-		if (ngps > 0) {
-			gps_height = gps.alt - pad_alt;
-			if (from_pad != null) {
-				elevation = Math.atan2(height, from_pad.distance) * 180 / Math.PI;
-				range = Math.sqrt(height * height + from_pad.distance * from_pad.distance);
+				double h = height;
+
+				if (h == AltosRecord.MISSING) h = 0;
+				from_pad = new AltosGreatCircle(pad_lat, pad_lon, 0, gps.lat, gps.lon, h);
+				elevation = from_pad.elevation;
+				range = from_pad.range;
+				gps_height = gps.alt - pad_alt;
 			}
-		} else {
-			gps_height = 0;
 		}
 	}