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
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162

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

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

use crate::{Distance, GPSBounds, Pt2D};

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

impl LonLat {
    /// Note the order of arguments!
    pub fn new(lon: f64, lat: f64) -> LonLat {
        LonLat {
            longitude: NotNan::new(lon).unwrap(),
            latitude: NotNan::new(lat).unwrap(),
        }
    }

    /// Returns the longitude of this point.
    pub fn x(self) -> f64 {
        self.longitude.into_inner()
    }

    /// Returns the latitude of this point.
    pub fn y(self) -> f64 {
        self.latitude.into_inner()
    }

    /// Transform this to a world-space point. Can go out of bounds.
    pub fn to_pt(self, b: &GPSBounds) -> Pt2D {
        let (width, height) = {
            let pt = b.get_max_world_pt();
            (pt.x(), pt.y())
        };

        let x = (self.x() - b.min_lon) / (b.max_lon - b.min_lon) * width;
        // Invert y, so that the northernmost latitude is 0. Screen drawing order, not Cartesian
        // grid.
        let y = height - ((self.y() - b.min_lat) / (b.max_lat - b.min_lat) * height);
        Pt2D::new(x, y)
    }

    /// Returns the Haversine distance to another point.
    pub(crate) fn gps_dist(self, other: LonLat) -> 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()
    }

    /// Parses a file in the https://wiki.openstreetmap.org/wiki/Osmosis/Polygon_Filter_File_Format
    /// and returns all points.
    pub fn read_osmosis_polygon(path: &str) -> Result<Vec<LonLat>> {
        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)
    }

    /// Writes a set of points to a file in the
    /// https://wiki.openstreetmap.org/wiki/Osmosis/Polygon_Filter_File_Format. The input should
    /// be a closed ring, with the first and last point matching.
    pub fn write_osmosis_polygon(path: &str, pts: &[LonLat]) -> Result<()> {
        let mut f = File::create(path)?;
        writeln!(f, "boundary")?;
        writeln!(f, "1")?;
        for pt in pts {
            writeln!(f, "     {}    {}", pt.x(), pt.y())?;
        }
        writeln!(f, "END")?;
        writeln!(f, "END")?;
        Ok(())
    }

    /// Finds the average of a set of coordinates.
    pub fn center(pts: &[LonLat]) -> LonLat {
        if pts.is_empty() {
            panic!("Can't find center of 0 points");
        }
        let mut x = 0.0;
        let mut y = 0.0;
        for pt in pts {
            x += pt.x();
            y += pt.y();
        }
        let len = pts.len() as f64;
        LonLat::new(x / len, y / len)
    }

    /// Parses a WKT-style line-string into a list of coordinates.
    pub fn parse_wkt_linestring(raw: &str) -> Option<Vec<LonLat>> {
        // Input is something like LINESTRING (-111.9263026 33.4245036, -111.9275146 33.4245016,
        // -111.9278751 33.4233106)
        let mut pts = Vec::new();
        // -111.9446 33.425474, -111.9442814 33.4254737, -111.9442762 33.426894
        for pair in raw
            .strip_prefix("LINESTRING (")?
            .strip_suffix(')')?
            .split(", ")
        {
            let mut nums = Vec::new();
            for x in pair.split(' ') {
                nums.push(x.parse::<f64>().ok()?);
            }
            if nums.len() != 2 {
                return None;
            }
            pts.push(LonLat::new(nums[0], nums[1]));
        }
        if pts.len() < 2 {
            return None;
        }
        Some(pts)
    }
}

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

impl From<LonLat> for geo::Point<f64> {
    fn from(pt: LonLat) -> Self {
        geo::Point::new(pt.x(), pt.y())
    }
}