sample_planning 0.0.4

Sample Based Planner
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


use std::collections::{HashMap,HashSet};

/// returns list of vertex coordinates, list of triangle vertex indices, max_x, max_y

pub fn load_map( path_ele: & str, path_nodes: &str ) -> (Vec<(f32,f32)>, Vec<[usize;3]>, f32, f32) {
    
    use std::fs::File;
    use std::io::Read;
    use std::io::BufReader;
    use std::io::BufRead;
    
    let mut f_ele = File::open(path_ele).expect("map file cannot be opened");
    let b_ele = BufReader::new(&f_ele);
    
    let mut f_nodes = File::open(path_nodes).expect("map file cannot be opened");
    let b_nodes = BufReader::new(&f_nodes);

    let mut num_nodes = 0;

    let mut nodes : Vec<(f32,f32)> = vec![];

    let mut max_x = 0.;
    let mut max_y = 0.;

    let mut min_x = 99999.;
    let mut min_y = 99999.;
    
    for(n,line) in b_nodes.lines().enumerate() {
        if n == 0 {
            
            num_nodes = line.unwrap()
                .split_whitespace()
                .nth(0).unwrap()
                .parse::<usize>().expect("num_nodes");
            
            nodes = vec![ (0.,0.); num_nodes ];
            
        } else {
            
            let a = line.expect("line");
            
            let idx = a.split_whitespace().nth(0).unwrap()
                .parse::<usize>().expect("node index");

            assert!(idx<num_nodes);

            let x = a.split_whitespace().nth(1).unwrap()
                .parse::<f32>().expect("node x");
            
            let y = a.split_whitespace().nth(2).unwrap()
                .parse::<f32>().expect("node y");
            
            max_x = if x > max_x { x } else { max_x };
            max_y = if y > max_y { y } else { max_y };

            min_x = if x < min_x { x } else { min_x };
            min_y = if y < min_y { y } else { min_y };
                
            nodes[idx] = (x,y);

            if n >= num_nodes {
                break;
            }
        }
    }

    //shift coordinates by -(min_x,min_y)
    nodes.iter_mut().for_each(|x| { x.0 -= min_x; x.1 -= min_y; } );
    //shift (max_x,max_y) by -(min_x,min_y)
    max_x -= min_x;
    max_y -= min_y;

    //scale coordinates so that longest dimension is 1.0

    let scale = if max_y > max_x { max_y } else { max_x };

    nodes.iter_mut().for_each(|x| { x.0 /= scale; x.1 /= scale; } );

    max_x /= scale;
    max_y /= scale;    
    
    let mut num_tris = 0;

    let mut tris : Vec<[usize;3]> = vec![];
    
    for(n,line) in b_ele.lines().enumerate() {
        if n == 0 {
            num_tris = line.unwrap()
                .split_whitespace()
                .nth(0).unwrap()
                .parse::<usize>().expect("num_tris");
            
            tris = vec![ [0,0,0]; num_tris ];
            
        } else {
            
            let a = line.expect("line");
            
            let idx = a.split_whitespace().nth(0).unwrap()
                .parse::<usize>().expect("tri index");

            assert!(idx<num_tris);

            let n0 = a.split_whitespace().nth(1).unwrap()
                .parse::<usize>().expect("tri n0");
            
            let n1 = a.split_whitespace().nth(2).unwrap()
                .parse::<usize>().expect("tri n1");

            let n2 = a.split_whitespace().nth(3).unwrap()
                .parse::<usize>().expect("tri n2");

            tris[idx] = [n0,n1,n2];

            if n >= num_tris {
                break;
            }
        }
    }
    
    ( nodes, tris, max_x, max_y )
}