frames 0.1.0

Contextual frames made simple.
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

use nalgebra::Isometry2;
use std::collections::HashMap;
use thiserror::Error;

#[derive(Debug, Default)]
pub struct Frames<'a> {
    frames: HashMap<Frame<'a>, Isometry2<f32>>,
    points: HashMap<Point<'a>, Isometry2<f32>>,
}

impl<'a> Frames<'a> {
    pub fn new() -> Self {
        Self {
            frames: HashMap::new(),
            points: HashMap::new(),
        }
    }

    pub fn add_frame(&mut self, f: Frame<'a>, c: Isometry2<f32>) -> Result<(), FrameError> {
        self.frames
            .insert(f, c)
            .map_or_else(|| Ok(()), |_| Err(FrameError::AlreadyExists))
    }

    /// Adds a point in the master context.
    pub fn add_point(&mut self, p: Point<'a>, c: Isometry2<f32>) -> Result<(), FrameError> {
        self.points
            .insert(p, c)
            .map_or_else(|| Ok(()), |_| Err(FrameError::AlreadyExists))
    }

    /// Adds a point in a specific context.
    pub fn add_point_in_context(
        &mut self,
        p: Point<'a>,
        c: Isometry2<f32>,
        f: Frame<'a>,
    ) -> Result<(), FrameError> {
        self.points
            .insert(p, c * self.get_frame(f)?.to_owned())
            .map_or_else(|| Ok(()), |_| Err(FrameError::AlreadyExists))
    }

    pub fn get_frame(&self, f: Frame<'a>) -> Result<Isometry2<f32>, FrameError> {
        self.frames
            .get(&f)
            .ok_or(FrameError::CouldNotFindFrame)
            .map(ToOwned::to_owned)
    }

    pub fn get_point(&self, p: Point<'a>) -> Result<Isometry2<f32>, FrameError> {
        self.points
            .get(&p)
            .ok_or(FrameError::CouldNotFindPoint)
            .map(ToOwned::to_owned)
    }

    pub fn get_point_in_context(
        &mut self,
        p: Point<'a>,
        f: Frame<'a>,
    ) -> Result<Isometry2<f32>, FrameError> {
        let offset = self.get_frame(f)?;
        self.get_point(p).map(|x| x / offset)
    }
}

#[derive(PartialEq, Eq, Hash, Debug, Copy, Clone)]
pub struct Frame<'a> {
    name: &'a str,
}

impl<'a> Frame<'a> {
    pub fn new(name: &'a str) -> Self {
        Self { name }
    }
}

#[derive(PartialEq, Eq, Hash, Debug, Copy, Clone, Default)]
pub struct Point<'a> {
    name: &'a str,
}

impl<'a> Point<'a> {
    pub fn new(name: &'a str) -> Self {
        Self { name }
    }
}

#[derive(Error, Debug)]
pub enum FrameError {
    #[error("frame already exists")]
    AlreadyExists,
    #[error("could not find frame")]
    CouldNotFindFrame,
    #[error("could not find point")]
    CouldNotFindPoint,
}

#[cfg(test)]
mod tests {
    use nalgebra::Vector2;

    use super::*;

    #[test]
    fn point_from_frame_to_frame() -> Result<(), FrameError> {
        let some_rotation = std::f32::consts::FRAC_PI_2;

        let field = Frame::new("field");
        let robot = Frame::new("robot");
        let mut frames = Frames::new();

        frames.add_frame(field, Isometry2::new(Vector2::new(0., 0.), 0.))?;
        frames.add_frame(robot, Isometry2::new(Vector2::new(1., 1.), some_rotation))?;
        dbg!(&frames);

        let x = Point::new("x");
        frames.add_point_in_context(
            x,
            Isometry2::new(Vector2::new(5., 5.), some_rotation),
            field,
        )?;
        dbg!(&frames);

        assert_eq!(
            frames.get_point_in_context(x, robot)?,
            Isometry2::new(Vector2::new(4., 4.), 0.)
        );

        Ok(())
    }
}