movingai 2.2.0

MovingAI Benchmark Map/Scen File Parser
Documentation 
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

use std::cmp::Ordering;
use std::collections::BinaryHeap;
use std::path::Path;
use std::time::Instant;

use movingai::parser::parse_map_file;
use movingai::parser::parse_scen_file;
use movingai::Coords2D;
use movingai::Map2D;
use movingai::MovingAiMap;

// Let's define the search nodes.

#[derive(Debug)]
struct SearchNode {
    pub f: f64,
    pub h: f64,
    pub g: f64,
    pub current: Coords2D,
}

impl PartialEq for SearchNode {
    fn eq(&self, other: &SearchNode) -> bool {
        self.current == other.current
    }
}

impl Eq for SearchNode {
    // add code here
}

impl PartialOrd for SearchNode {
    fn partial_cmp(&self, other: &SearchNode) -> Option<Ordering> {
        Some(self.cmp(other))
    }
}

impl Ord for SearchNode {
    fn cmp(&self, other: &SearchNode) -> Ordering {
        // This is reversed on purpose to make the max-heap into min-heap.
        if self.f < other.f {
            Ordering::Greater
        } else if self.f > other.f {
            Ordering::Less
        } else {
            Ordering::Equal
        }
    }
}

fn distance(a: Coords2D, b: Coords2D) -> f64 {
    let (x, y) = (a.0 as f64, a.1 as f64);
    let (p, q) = (b.0 as f64, b.1 as f64);
    ((x - p) * (x - p) + (y - q) * (y - q)).sqrt()
}

// A* shortest path algorithm.

// This implementation isn't memory-efficient as it may leave duplicate
// nodes in the queue.
fn shortest_path(map: &MovingAiMap, start: Coords2D, goal: Coords2D) -> Option<f64> {
    let mut heap = BinaryHeap::new();
    let mut visited = Vec::<Coords2D>::new();

    // We're at `start`, with a zero cost
    heap.push(SearchNode {
        f: 0.0,
        g: 0.0,
        h: distance(start, goal),
        current: start,
    });

    // Examine the frontier with lower cost nodes first (min-heap)
    while let Some(SearchNode {
        f: _f,
        g,
        h: _h,
        current,
    }) = heap.pop()
    {
        // Alternatively we could have continued to find all shortest paths

        // println!("G: {:?} H.size: {:?}", g, heap.len());

        if current == goal {
            return Some(g);
        }

        if visited.contains(&current) {
            continue;
        }

        visited.push(current);

        // For each node we can reach, see if we can find a way with
        // a lower cost going through this node
        for neigh in map.neighbors(current) {
            // println!("Current {:?} Neigh {:?}", current,  neigh);
            let new_h = distance(neigh, goal);
            let i = distance(neigh, current);
            let next = SearchNode {
                f: g + i + new_h,
                g: g + i,
                h: new_h,
                current: neigh,
            };
            heap.push(next);
        }
    }

    // Goal not reachable
    None
}

fn main() {
    let map = parse_map_file(Path::new("./tests/maze512-32-9.map")).unwrap();
    let scenes = parse_scen_file(Path::new("./tests/maze512-32-9.map.scen")).unwrap();
    for scene in scenes {
        let start = scene.start_pos;
        let goal = scene.goal_pos;
        let optimal = scene.optimal_length;
        let t = Instant::now();
        match shortest_path(&map, start, goal) {
            Some(g) => {
                let duration = t.elapsed();
                let millis = duration.as_millis();
                let delta = (optimal - g).abs();
                if delta > 0.00001 {
                    println!(
                        "{:?} -> {:?} = {:.5} \t\tin {:.5} ms (Δ{:.5})",
                        start, goal, g, millis, delta
                    );
                } else {
                    println!(
                        "{:?} -> {:?} = {:.5} \t\tin {:.5} ms",
                        start, goal, g, millis
                    );
                }
            }
            None => println!("None"),
        }
    }
}