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 {
* 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]
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));
+}