From: Tilman Sauerbeck Date: Sun, 19 Jan 2020 18:57:42 +0000 (+0100) Subject: common: Implement TimeAndPos::distance_cm(). X-Git-Url: http://git.code-monkey.de/?a=commitdiff_plain;h=80ab8c86d93777354b2d780784ebb6e025d6147e;p=gps-watch.git common: Implement TimeAndPos::distance_cm(). Calculates the distance between two points in cm. --- diff --git a/src/common/gps.rs b/src/common/gps.rs index 7e635fc..85e1dd4 100644 --- a/src/common/gps.rs +++ b/src/common/gps.rs @@ -63,6 +63,25 @@ impl TimeAndPos { longitude_rad: Fixed::from_i64(0), } } + + pub fn distance_cm(&self, other: &TimeAndPos) -> i64 { + let a_lat = self.latitude_rad; + let a_lon = self.longitude_rad; + + let b_lat = other.latitude_rad; + let b_lon = other.longitude_rad; + + let x = (b_lon - a_lon) * ((b_lat + a_lat) / Fixed::from_i64(2)).cos(); + let y = b_lat - a_lat; + + let d_km = (x * x + y * y).sqrt() * Fixed::from_i64(6371); + + let hundred = Fixed::from_i64(100); + let thousand = Fixed::from_i64(1000); + + // Convert from km to cm. + (d_km * thousand * hundred).to_i64() + } } fn to_lower(c: u8) -> u8 { diff --git a/test/gps_test.rs b/test/gps_test.rs index 1cff088..7a71a76 100644 --- a/test/gps_test.rs +++ b/test/gps_test.rs @@ -21,8 +21,11 @@ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +use common::fixed15_49; use common::gps; +type Fixed = fixed15_49::Fixed15_49; + // Verifies that a single RMC message with position, date and time // does not suffice for getting a valid TimeAndPos result. #[test] @@ -87,3 +90,26 @@ $GPGGA,110338.000,1234.5678,N,12345.6789,E,\ assert_eq!(0.21949487565883447, tap.latitude_rad.to_f32()); assert_eq!(2.160042433347846, tap.longitude_rad.to_f32()); } + +#[test] +fn distance_cm0() { + let tap0 = gps::TimeAndPos { + system_time: 0, + unix_time: 0, + latitude: 0, + longitude: 0, + latitude_rad: Fixed::from_f32(49.02541333).to_radians(), + longitude_rad: Fixed::from_f32(8.79440167).to_radians(), + }; + + let tap1 = gps::TimeAndPos { + system_time: 1, + unix_time: 1, + latitude: 0, + longitude: 0, + latitude_rad: Fixed::from_f32(49.02541000).to_radians(), + longitude_rad: Fixed::from_f32(8.79443667).to_radians(), + }; + + assert_eq!(349, tap1.distance_cm(&tap0)); +}