optimization_engine 0.3.1

A pure Rust framework for embedded nonconvex optimization. Ideal 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
48
49
50
51
52
53
54
55
56
57

use super::Constraint;

/// A Eucledian ball
pub struct Ball2 {
    centre: Option<Vec<f64>>,
    radius: f64,
}

impl Ball2 {
    ///
    /// Construct a new ball centered at the origin with given radius
    pub fn new_at_origin_with_radius(radius_: f64) -> Ball2 {
        assert!(radius_ > 0.0);

        Ball2 {
            centre: None,
            radius: radius_,
        }
    }

    ///
    /// Construct a new Eucledian ball with given centre and radius
    pub fn new(centre_: Vec<f64>, radius_: f64) -> Ball2 {
        assert!(radius_ > 0.0);

        Ball2 {
            centre: Some(centre_),
            radius: radius_,
        }
    }
}

impl Constraint for Ball2 {
    fn project(&self, x: &mut [f64]) {
        if let Some(centre) = &self.centre {
            let mut norm_difference = 0.0;
            x.iter().zip(centre.iter()).for_each(|(a, b)| {
                let diff_ = *a - *b;
                norm_difference += diff_ * diff_
            });

            norm_difference = norm_difference.sqrt();

            if norm_difference > self.radius {
                x.iter_mut().zip(centre.iter()).for_each(|(x, c)| {
                    *x = *c + (*x - *c) / norm_difference;
                });
            }
        } else {
            let norm_x = crate::matrix_operations::norm2(x);
            if norm_x > self.radius {
                let norm_over_radius = norm_x / self.radius;
                x.iter_mut().for_each(|x_| *x_ /= norm_over_radius);
            }
        }
    }
}