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

//! Implementation of a point linked list with additional methods used for path
//! finding.

use std::rc::Rc;

use de_types::path::Path;
use parry2d::math::Point;

/// A linked list of points which keeps track of its length in meters.
#[derive(Clone)]
pub(crate) struct PointChain {
    prev: Option<Rc<Self>>,
    point: Point<f32>,
    length: f32,
}

impl PointChain {
    /// Creates a new point with a given predecessor.
    ///
    /// # Arguments
    ///
    /// * `chain` - reference to a previous point to be used as an immediate
    ///   predecessor. The reference is cloned with [`Rc::clone`].
    ///
    /// * `point` - extension point. This point must differ from the last point
    ///   in `chain`
    pub(crate) fn extended(chain: &Rc<Self>, point: Point<f32>) -> Self {
        let length = chain.length() + (point - chain.point()).magnitude();
        Self::new(Some(Rc::clone(chain)), point, length)
    }

    /// Creates a new point with no predecessors.
    pub(crate) fn first(point: Point<f32>) -> Self {
        Self::new(None, point, 0.)
    }

    fn new(prev: Option<Rc<Self>>, point: Point<f32>, length: f32) -> Self {
        Self {
            prev,
            point,
            length,
        }
    }

    /// Returns previous point or None if this is the first point.
    pub(crate) fn prev(&self) -> Option<&Rc<Self>> {
        self.prev.as_ref()
    }

    pub(crate) fn point(&self) -> Point<f32> {
        self.point
    }

    /// Returns length of the point chain in meters. It is equal to the sum of
    /// distances of individual points.
    pub(crate) fn length(&self) -> f32 {
        self.length
    }

    /// Returns an iterator over points in this linked list. The iterator
    /// starts at `self` and traverses all predecessors.
    pub(crate) fn iter(&self) -> Predecessors {
        Predecessors::new(self)
    }

    /// Converts point chain to a path.
    pub(crate) fn to_path(&self) -> Path {
        Path::new(
            self.length(),
            self.iter().map(|t| t.point().into()).collect(),
        )
    }
}

pub(crate) struct Predecessors<'a> {
    chain: Option<&'a PointChain>,
}

impl<'a> Predecessors<'a> {
    fn new(chain: &'a PointChain) -> Self {
        Self { chain: Some(chain) }
    }
}

impl<'a> Iterator for Predecessors<'a> {
    type Item = &'a PointChain;

    fn next(&mut self) -> Option<Self::Item> {
        let next = self.chain;
        self.chain = self.chain.and_then(|c| c.prev()).map(Rc::as_ref);
        next
    }
}

#[cfg(test)]
mod tests {
    use glam::Vec2;

    use super::*;

    #[test]
    fn test_chain() {
        let chain = PointChain::first(Point::new(1., 2.));
        assert!(chain.prev().is_none());
        assert_eq!(chain.point(), Point::new(1., 2.));
        assert_eq!(chain.length(), 0.);
        let collected: Vec<Point<f32>> = chain.iter().map(|p| p.point()).collect();
        assert_eq!(collected, vec![Point::new(1., 2.)]);

        let chain = PointChain::extended(&Rc::new(chain), Point::new(3., 2.));
        assert!(chain.prev().is_some());
        assert_eq!(chain.point(), Point::new(3., 2.));
        assert_eq!(chain.length(), 2.);
        let collected: Vec<Point<f32>> = chain.iter().map(|p| p.point()).collect();
        assert_eq!(collected, vec![Point::new(3., 2.), Point::new(1., 2.)]);
    }

    #[test]
    fn test_to_path() {
        let chain = PointChain::extended(
            &Rc::new(PointChain::first(Point::new(1., 2.))),
            Point::new(3., 2.),
        );
        let path = chain.to_path();
        assert_eq!(path.length(), 2.);
        assert_eq!(path.waypoints(), &[Vec2::new(3., 2.), Vec2::new(1., 2.)]);
    }
}