optima 0.0.4

An easy to set up and easy optimization and planning toolbox, particularly for robotics.
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

use nalgebra::DVector;

pub struct SimpleInterpolationUtils;
impl SimpleInterpolationUtils {
    pub fn linear_interpolation(start_point: &DVector<f64>,
                                end_point: &DVector<f64>,
                                mode: &LinearInterpolationMode) -> Vec<DVector<f64>> {
        let mut out_vec = vec![];

        let mut dir = end_point - start_point;
        let n = dir.norm();
        dir /= n;

        match mode {
            LinearInterpolationMode::FixedNumKnots { num_knots } => {
                let spacing_between_knots = n / (*num_knots as f64 - 1.0);

                let mut curr_point = start_point.clone();
                for _ in 0..*num_knots-1 {
                    out_vec.push(curr_point.clone());
                    curr_point = &curr_point + spacing_between_knots * &dir;
                }
                out_vec.push(end_point.clone());
            }
            LinearInterpolationMode::FixedL2NormSpacing { spacing } => {
                if *spacing > n { out_vec.push(start_point.clone()); out_vec.push(end_point.clone()); }
                else {
                    let num_knots = (n / *spacing).ceil() as usize;

                    let mut curr_point = start_point.clone();
                    for _ in 0..num_knots {
                        out_vec.push(curr_point.clone());
                        curr_point = &curr_point + *spacing * &dir;
                    }
                    out_vec.push(end_point.clone());
                }
            }
        }

        out_vec
    }
}

pub enum LinearInterpolationMode {
    FixedNumKnots { num_knots: usize },
    FixedL2NormSpacing { spacing: f64 }
}