Azimuth/Distance calculator

compass.rs

// Algorithm from http://cosinekitty.com/compass.html
// Code ported to Rust by @aeshirey

use core::f64::consts::PI;
#[derive(Debug, Copy, Clone)]
pub struct Location {
    latitude: f64,
    longitude: f64,
    elevation_meters: f64,
}

impl From<(f64, f64)> for Location {
    fn from(coords: (f64, f64)) -> Self {
        Location {
            latitude: coords.0,
            longitude: coords.1,
            elevation_meters: 0.,
        }
    }
}

impl From<(f64, f64, f64)> for Location {
    fn from(coords: (f64, f64, f64)) -> Self {
        Location {
            latitude: coords.0,
            longitude: coords.1,
            elevation_meters: coords.2,
        }
    }
}

impl Location {
    fn earth_radius_meters(latitude_radians: f64) -> f64 {
        // latitude is geodetic, i.e. that reported by GPS
        // http://en.wikipedia.org/wiki/Earth_radius
        const EQUATOR: f64 = 6378137.0; // equatorial radius in meters
        const POLE: f64 = 6356752.3; // polar radius in meters
        let cos = latitude_radians.cos();
        let sin = latitude_radians.sin();
        let t1 = EQUATOR * EQUATOR * cos;
        let t2 = POLE * POLE * sin;
        let t3 = EQUATOR * cos;
        let t4 = POLE * sin;
        ((t1 * t1 + t2 * t2) / (t3 * t3 + t4 * t4)).sqrt()
    }
    fn to_point(self: Location, oblate: bool) -> Point {
        // Convert (lat, lon, elv) to (x, y, z).
        let lat = self.latitude * PI / 180.0;
        let lon = self.longitude * PI / 180.0;

        let (radius, clat) = if oblate {
            (Location::earth_radius_meters(lat), geocentric_latitude(lat))
        } else {
            (6371009.0, lat)
        };

        let cos_lon = lon.cos();
        let sin_lon = lon.sin();
        let cos_lat = clat.cos();
        let sin_lat = clat.sin();

        // We used geocentric latitude to calculate (x,y,z) on the Earth's ellipsoid.
        // Now we use geodetic latitude to calculate normal vector from the surface, to correct for elevation.
        let cos_glat = lat.cos();
        let sin_glat = lat.sin();

        let nx = cos_glat * cos_lon;
        let ny = cos_glat * sin_lon;
        let nz = sin_glat;

        let x = radius * cos_lon * cos_lat + self.elevation_meters * nx;
        let y = radius * sin_lon * cos_lat + self.elevation_meters * ny;
        let z = radius * sin_lat + self.elevation_meters * nz;

        Point {
            x,
            y,
            z,
            radius,
            nx,
            ny,
            nz,
        }
    }
}

#[derive(Debug, Copy, Clone)]
pub struct Point {
    x: f64,
    y: f64,
    z: f64,
    radius: f64,

    nx: f64,
    ny: f64,
    nz: f64,
}

impl Point {
    pub fn distance_km(&self, other: Point) -> f64 {
        let dx = self.x - other.x;
        let dy = self.y - other.y;
        let dz = self.z - other.z;
        (dx * dx + dy * dy + dz * dz).sqrt()
    }

    fn normalize_vector_diff(&self, a: Point) -> Option<Point> {
        // Calculate norm(b-a), where norm divides a vector by its length to produce a unit vector.
        let dx = self.x - a.x;
        let dy = self.y - a.y;
        let dz = self.z - a.z;
        let dist = dx * dx + dy * dy + dz * dz;

        if dist == 0.0 {
            return None;
        }

        let dist = dist.sqrt();

        Some(Point {
            x: dx / dist,
            y: dy / dist,
            z: dz / dist,
            radius: 1.,
            // ??
            nx: f64::NAN,
            ny: f64::NAN,
            nz: f64::NAN,
        })
    }
}

#[derive(Debug, Copy, Clone, PartialEq)]
pub struct Results {
    distance_km: f64,
    altitude: Option<f64>,
    azimuth: Option<f64>,
}

/// Convert geodetic latitude 'lat' to a geocentric latitude 'clat'.
/// Geodetic latitude is the latitude as given by GPS.
/// Geocentric latitude is the angle measured from center of Earth between a point and the equator.
/// https://en.wikipedia.org/wiki/Latitude#Geocentric_latitude
fn geocentric_latitude(lat: f64) -> f64 {
    const E2: f64 = 0.00669437999014;
    ((1.0 - E2) * lat.tan()).atan()
}

fn rotate_globe(b: Location, a: Location, bradius: f64, _aradius: f64, oblate: bool) -> Point {
    // Get modified coordinates of 'b' by rotating the globe so that 'a' is at lat=0, lon=0.
    let br = Location {
        latitude: b.latitude,
        longitude: b.longitude - a.longitude,
        elevation_meters: b.elevation_meters,
    };
    let brp = br.to_point(oblate);

    // Rotate brp cartesian coordinates around the z-axis by a.lon degrees,
    // then around the y-axis by a.lat degrees.
    // Though we are decreasing by a.lat degrees, as seen above the y-axis,
    // this is a positive (counterclockwise) rotation (if B's longitude is east of A's).
    // However, from this point of view the x-axis is pointing left.
    // So we will look the other way making the x-axis pointing right, the z-axis
    // pointing up, and the rotation treated as negative.

    let mut alat = -a.latitude * PI / 180.0;
    if oblate {
        alat = geocentric_latitude(alat);
    }
    let acos = alat.cos();
    let asin = alat.sin();

    let bx = (brp.x * acos) - (brp.z * asin);
    let by = brp.y;
    let bz = (brp.x * asin) + (brp.z * acos);

    Point {
        x: bx,
        y: by,
        z: bz,
        radius: bradius,
        nx: f64::NAN,
        ny: f64::NAN,
        nz: f64::NAN,
    }
}

pub fn calculate(a: Location, b: Location, oblate: bool) -> Results {
    let ap = a.to_point(oblate);
    let bp = b.to_point(oblate);

    let distance_km = 0.001 * ap.distance_km(bp);

    // Let's use a trick to calculate azimuth:
    // Rotate the globe so that point A looks like latitude 0, longitude 0.
    // We keep the actual radii calculated based on the oblate geoid,
    // but use angles based on subtraction.
    // Point A will be at x=radius, y=0, z=0.
    // Vector difference B-A will have dz = N/S component, dy = E/W component.
    let br = rotate_globe(b, a, bp.radius, ap.radius, oblate);
    let azimuth = if br.z * br.z + br.y * br.y > 1.0e-6 {
        let theta = br.z.atan2(br.y) * 180.0 / PI;
        let mut azimuth = 90.0 - theta;
        if azimuth < 0.0 {
            azimuth += 360.0;
        } else if azimuth > 360.0 {
            azimuth -= 360.0;
        }
        Some(azimuth)
    } else {
        None
    };

    let altitude = bp.normalize_vector_diff(ap).map(|bma|
                                                   // Calculate altitude, which is the angle above the horizon of B as seen from A.
                                                   // Almost always, B will actually be below the horizon, so the altitude will be negative.
                                                   // The dot product of bma and norm = cos(zenith_angle), and zenith_angle = (90 deg) - altitude.
                                                   // So altitude = 90 - acos(dotprod).
                                                   90.0 - (180.0 / PI)*(bma.x*ap.nx + bma.y*ap.ny + bma.z*ap.nz).acos()
                                                  );

    Results {
        distance_km,
        azimuth,
        altitude,
    }
}

/*
   fn OnGeoCheck()
   {
// The geostationary checkbox was clicked.
let geomode = $('cb_geo').checked;
if (geomode) {
// Save values so user doesn't lose them on accidental/curiosity click.
save_b_lat = $('b_lat').value;
save_b_elv = $('b_elv').value;

// Fill in the values for geostationary orbit.
$('b_lat').value = '0';         // assume satellite is directly above equator.
$('b_elv').value = '35786000';  // 35,786 km above equator.

// Disable editing of point B latitude and elevation while box is checked.
$('b_lat').disabled = true;
$('b_elv').disabled = true;
} else {
// Restore saved values to edit boxes, so user doesn't lose them.
$('b_lat').value = save_b_lat;
$('b_elv').value = save_b_elv;

// Enable editing of point B latitude and elevation while box is checked.
$('b_lat').disabled = false;
$('b_elv').disabled = false;
}
}
*/

#[cfg(test)]
mod tests {
    use super::*;
    #[test]
    fn space_needle_gas_works() {
        let space_needle: Location = (47.6215067, -122.3410866).into();
        let gas_works: Location = (47.6423433, -122.3395575).into();

        let actual = calculate(space_needle, gas_works, true);
        let expected = Results {
            distance_km: 2.319527511701253,
            azimuth: Some(2.8392840177295113),
            altitude: Some(-0.010431166135234093),
        };

        assert!((actual.distance_km - expected.distance_km).abs() < 0.00000001);
        assert!((actual.azimuth.unwrap() - expected.azimuth.unwrap()).abs() < 0.00000001);
        assert!((actual.altitude.unwrap() - expected.altitude.unwrap()).abs() < 0.00000001);
    }

    #[test]
    fn space_needle_rainier() {
        let space_needle: Location = (47.6215067, -122.3410866).into();
        let rainier: Location = (46.852202, -121.760314).into();

        let actual = calculate(space_needle, rainier, true);

        let expected = Results {
            distance_km: 96.16937194459534,
            azimuth: Some(152.57618897472915),
            altitude: Some(-0.4322258796320142),
        };

        assert!((actual.distance_km - expected.distance_km).abs() < 0.00000001);
        assert!((actual.azimuth.unwrap() - expected.azimuth.unwrap()).abs() < 0.00000001);
        assert!((actual.altitude.unwrap() - expected.altitude.unwrap()).abs() < 0.00000001);
    }

    #[test]
    fn space_needle_rainier_with_elevation() {
        let space_needle: Location = (47.6215067, -122.3410866, 20.).into();
        let rainier: Location = (46.852202, -121.760314, 4392.).into();

        let actual = calculate(space_needle, rainier, true);

        let expected = Results {
            distance_km: 96.30194370374271,
            azimuth: Some(152.5722091827188),
            altitude: Some(2.1697583667351807),
        };

        assert!((actual.distance_km - expected.distance_km).abs() < 0.00000001);
        assert!((actual.azimuth.unwrap() - expected.azimuth.unwrap()).abs() < 0.00000001);
        assert!((actual.altitude.unwrap() - expected.altitude.unwrap()).abs() < 0.00000001);
    }

    #[test]
    fn space_needle_to_self() {
        let space_needle: Location = (47.6215067, -122.3410866).into();

        let actual = calculate(space_needle, space_needle, true);

        let expected = Results {
            distance_km: 0.,
            azimuth: None,
            altitude: None,
        };

        assert_eq!(actual, expected);
    }
}