1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86

use std::error::Error;
use std::fmt;
use std::fs::File;
use std::io::{BufRead, BufReader};

use ordered_float::NotNan;
use serde::{Deserialize, Serialize};

use crate::Distance;

// longitude is x, latitude is y
#[derive(Copy, Clone, PartialEq, Eq, PartialOrd, Ord, Debug, Serialize, Deserialize)]
pub struct LonLat {
    longitude: NotNan<f64>,
    latitude: NotNan<f64>,
}

impl LonLat {
    pub fn new(lon: f64, lat: f64) -> LonLat {
        LonLat {
            longitude: NotNan::new(lon).unwrap(),
            latitude: NotNan::new(lat).unwrap(),
        }
    }

    pub fn x(self) -> f64 {
        self.longitude.into_inner()
    }

    pub fn y(self) -> f64 {
        self.latitude.into_inner()
    }

    pub fn gps_dist_meters(self, other: LonLat) -> Distance {
        // Haversine distance
        let earth_radius_m = 6_371_000.0;
        let lon1 = self.x().to_radians();
        let lon2 = other.x().to_radians();
        let lat1 = self.y().to_radians();
        let lat2 = other.y().to_radians();

        let delta_lat = lat2 - lat1;
        let delta_lon = lon2 - lon1;

        let a = (delta_lat / 2.0).sin().powi(2)
            + (delta_lon / 2.0).sin().powi(2) * lat1.cos() * lat2.cos();
        let c = 2.0 * a.sqrt().atan2((1.0 - a).sqrt());
        Distance::meters(earth_radius_m * c)
    }

    // Pretty meaningless units, for comparing distances very roughly
    pub fn fast_dist(self, other: LonLat) -> NotNan<f64> {
        NotNan::new((self.x() - other.x()).powi(2) + (self.y() - other.y()).powi(2)).unwrap()
    }

    pub(crate) fn approx_eq(self, other: LonLat) -> bool {
        let epsilon = 1e-8;
        (self.x() - other.x()).abs() < epsilon && (self.y() - other.y()).abs() < epsilon
    }

    pub fn read_osmosis_polygon(path: String) -> Result<Vec<LonLat>, Box<dyn Error>> {
        let f = File::open(&path)?;
        let mut pts = Vec::new();
        for (idx, line) in BufReader::new(f).lines().enumerate() {
            if idx < 2 {
                continue;
            }
            let line = line?;
            if line == "END" {
                break;
            }
            let parts = line.trim().split("    ").collect::<Vec<_>>();
            pts.push(LonLat::new(
                parts[0].parse::<f64>()?,
                parts[1].parse::<f64>()?,
            ));
        }
        Ok(pts)
    }
}

impl fmt::Display for LonLat {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        write!(f, "LonLat({0}, {1})", self.x(), self.y())
    }
}