oxmpl 0.6.0

The Open Motion-Planning Library but Oxidised
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

// Copyright (c) 2025 Junior Sundar
//
// SPDX-License-Identifier: BSD-3-Clause

use std::{any::Any, ops::Deref};

use crate::base::state::{CompoundState, RealVectorState, SO2State, State};

/// A state representing a 2D rigid body transformation, an element of the Special Euclidean group
/// SE(2).
///
/// This state is composed of a 2D translation (x, y) and a 2D rotation (yaw). It is internally
/// represented as a `CompoundState`.
#[derive(Clone, Debug)]
pub struct SE2State(pub CompoundState);

impl State for SE2State {
    fn as_any(&self) -> &dyn std::any::Any {
        self
    }
}

impl SE2State {
    /// Creates a new `SE2State` from x, y, and yaw components.
    ///
    /// # Examples
    ///
    /// ```
    /// use std::f64::consts::PI;
    /// use oxmpl::base::state::SE2State;
    ///
    /// let state1 = SE2State::new(1.0, 2.0, PI / 2.0);
    /// assert_eq!(state1.get_x(), 1.0);
    /// assert_eq!(state1.get_y(), 2.0);
    /// assert!((state1.get_yaw() - (PI / 2.0)).abs() < 1e-9);
    ///
    /// let state2 = SE2State::new(3.0, 4.0, 3.0 * PI); // equivalent to PI
    /// assert!((state2.get_yaw() + PI).abs() < 1e-9);
    /// ```
    pub fn new(x: f64, y: f64, yaw: f64) -> Self {
        SE2State(CompoundState {
            components: vec![
                Box::new(RealVectorState::new(vec![x, y])),
                Box::new(SO2State::new(yaw)),
            ],
        })
    }

    /// Returns a reference to the translational component (x, y) of the state.
    pub fn get_translation(&self) -> &RealVectorState {
        (self.0.components[0].deref() as &dyn Any)
            .downcast_ref::<RealVectorState>()
            .expect("Issue found in retreiving the translation vector.")
    }

    /// Returns a reference to the rotational component (yaw) of the state.
    pub fn get_rotation(&self) -> &SO2State {
        (self.0.components[1].deref() as &dyn Any)
            .downcast_ref::<SO2State>()
            .expect("Issue found in retreiving the rotation.")
    }

    /// Returns the x-coordinate of the state.
    pub fn get_x(&self) -> f64 {
        (self.0.components[0].deref() as &dyn Any)
            .downcast_ref::<RealVectorState>()
            .expect("Issue found in retreiving the translation vector.")
            .values[0]
    }

    /// Returns the y-coordinate of the state.
    pub fn get_y(&self) -> f64 {
        (self.0.components[0].deref() as &dyn Any)
            .downcast_ref::<RealVectorState>()
            .expect("Issue found in retreiving the translation vector.")
            .values[1]
    }

    /// Returns the yaw (rotation) of the state in radians.
    pub fn get_yaw(&self) -> f64 {
        (self.0.components[1].deref() as &dyn Any)
            .downcast_ref::<SO2State>()
            .expect("Issue found in retreiving the rotation.")
            .value
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::base::state::{RealVectorState, SO2State};
    use std::f64::consts::PI;

    #[test]
    fn test_se2_state_creation_and_getters() {
        let state = SE2State::new(1.5, -2.5, PI / 2.0);

        assert_eq!(state.get_x(), 1.5);
        assert_eq!(state.get_y(), -2.5);
        assert_eq!(state.get_yaw(), PI / 2.0);

        assert_eq!(
            state.get_translation(),
            &RealVectorState::new(vec![1.5, -2.5])
        );
        assert_eq!(state.get_rotation(), &SO2State::new(PI / 2.0));
    }

    #[test]
    fn test_se2_state_yaw_normalization() {
        let state1 = SE2State::new(1.0, 2.0, 3.0 * PI / 2.0);
        assert!((state1.get_yaw() - (-PI / 2.0)).abs() < 1e-9);

        let state2 = SE2State::new(1.0, 2.0, 5.0 * PI);
        assert!((state2.get_yaw() + PI).abs() < 1e-9);

        let state3 = SE2State::new(1.0, 2.0, -7.0 * PI / 2.0);
        assert!((state3.get_yaw() - (PI / 2.0)).abs() < 1e-9);
    }

    #[test]
    fn test_se2_state_clone() {
        let state1 = SE2State::new(10.0, 20.0, PI / 4.0);
        let state2 = state1.clone();

        assert_eq!(state1.get_x(), state2.get_x());
        assert_eq!(state1.get_y(), state2.get_y());
        assert_eq!(state1.get_yaw(), state2.get_yaw());

        assert_eq!(state1.get_translation(), state2.get_translation());
        assert_eq!(state1.get_rotation(), state2.get_rotation());
    }
}