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

use std::fmt::Debug;

pub trait States : Clone + Debug + Default {
    fn get_num_dims(&self) -> i32;
    fn get_vals(&self) -> Vec<f32>;
    fn get_vals_3(&self) -> [f32;3];
    fn set_vals(& mut self, vals: &[f32] );
}

#[derive(Clone, Copy, Debug, Default)]
pub struct States1D(pub f32);

impl States for States1D {
    fn get_num_dims(&self) -> i32 {
        1
    }
    fn get_vals(&self) -> Vec<f32> {
        vec![ self.0 ]
    }
    fn get_vals_3(&self) -> [f32;3] {
        [self.0, 0., 0.]
    }
    fn set_vals(& mut self, vals: &[f32] ){
        debug_assert!( vals.len() == 1 );
        self.0 = vals[0];
    }
}

#[derive(Clone, Copy, Debug, Default)]
pub struct States2D(pub[f32;2]);

impl States for States2D {
    fn get_num_dims(&self) -> i32 {
        2
    }
    fn get_vals(&self) -> Vec<f32> {
        self.0.to_vec()
    }
    fn get_vals_3(&self) -> [f32;3] {
        [self.0[0], self.0[1], 0.]
    }
    fn set_vals(& mut self, vals: &[f32] ){
        debug_assert!( vals.len() == 2 );
        for i in 0..2 {
            self.0[i] = vals[i];
        }
    }
}

#[derive(Clone, Copy, Debug, Default)]
pub struct States3D(pub[f32;3]);

impl States for States3D {
    fn get_num_dims(&self) -> i32 {
        3
    }
    fn get_vals(&self) -> Vec<f32> {
        self.0.to_vec()
    }
    fn get_vals_3(&self) -> [f32;3] {
        [self.0[0], self.0[1], self.0[2]]
    }
    fn set_vals(& mut self, vals: &[f32] ){
        debug_assert!( vals.len() == 3 );
        for i in 0..3 {
            self.0[i] = vals[i];
        }
    }
}

#[derive(Clone, Copy, Debug, Default)]
pub struct States4D(pub[f32;4]);

impl States for States4D {
    fn get_num_dims(&self) -> i32 {
        4
    }
    fn get_vals(&self) -> Vec<f32> {
        self.0.to_vec()
    }
    fn get_vals_3(&self) -> [f32;3] {
        [self.0[0], self.0[1], self.0[2]]
    }
    fn set_vals(& mut self, vals: &[f32] ){
        debug_assert!( vals.len() == 4 );
        for i in 0..4 {
            self.0[i] = vals[i];
        }
    }
}