kalman_rs/geometry/rectangle.rs
1extern crate nalgebra as na;
2use na::{Point3, Matrix4 as Matrix, Vector3 as Vector};
3use super::traits::{self, Transform, Plane};
4
5/// A struct for sensors of rectangular geometry
6pub struct Rectangle {
7 points : [na::Point3<f32>; 4],
8 normal: Option<Vector<f32>>,
9 tfm: na::Affine3<f32>
10}
11
12impl Rectangle {
13 /// This is the constructor for the rectangular geometry. It expects a 4x4 `nalgebra::Matrix4<f32>` that is invertible
14 /// and a 4 element array of `nalgebra::Point3<f32>`. If the matrix is not invertible it will return `Err(&str)`.
15 /// The provided matrix should be an affine transformation for converting from R2->R3
16 ///
17 /// # Examples
18 /// ```
19 /// use nalgebra as na;
20 /// use na::Point3;
21 ///
22 /// let rectangular_points = [Point3::new(0.0, 0.0, 0.0),
23 /// Point3::new(5.0,0.0,0.0),
24 /// Point3::new(0.0,5.0,0.0),
25 /// Point3::new(5.0,5.0,0.0)];
26 /// let tfm_matrix : na::Matrix4<f32>= na::Matrix4::new(1.0,5.0,7.0,2.0, 3.0,5.0,7.0,4.0, 8.0,4.0,1.0,9.0, 2.0,6.0,4.0,8.0);
27 /// let mut rectangle_sensor = kalman_rs::Rectangle::new(rectangular_points, tfm_matrix).unwrap();
28 /// ```
29 pub fn new(mut points: [Point3<f32>; 4], tfm_matrix: Matrix<f32>) -> Result<Rectangle, &'static str>{
30
31 let affine_transform = na::Affine3::from_matrix_unchecked(tfm_matrix);
32
33 match affine_transform.try_inverse(){
34 Some(_x) => {
35 let points = traits::organize_points(&mut points);
36 Ok(Rectangle{points: *points, normal: None,tfm: affine_transform})},
37 None => return Err("matrix was not invertable")
38
39 }
40 }
41
42}
43impl Transform for Rectangle{
44 /// Converts a point in the global reference frame to a point in the local reference frame of the sensor.
45 ///
46 /// # Examples
47 /// ```
48 /// use nalgebra as na;
49 /// use na::Point3;
50 /// use kalman_rs::sensor_traits::Transform;
51 ///
52 /// let rectangular_points = [Point3::new(0.0, 0.0, 0.0),
53 /// Point3::new(5.0,0.0,0.0),
54 /// Point3::new(0.0,5.0,0.0),
55 /// Point3::new(5.0,5.0,0.0)];
56 /// let tfm_matrix : na::Matrix4<f32>= na::Matrix4::new(1.0,5.0,7.0,2.0, 3.0,5.0,7.0,4.0, 8.0,4.0,1.0,9.0, 2.0,6.0,4.0,8.0);
57 /// let mut rectangle_sensor = kalman_rs::Rectangle::new(rectangular_points, tfm_matrix).unwrap();
58 ///
59 /// let global_point = rectangle_sensor.to_global(na::Point3::new(1.0, 2.0, 0.0));
60 /// ```
61 fn to_global(&self, input_point: Point3<f32>)-> na::Point3<f32>{
62 return self.tfm * input_point;
63 }
64
65 /// Converts a point in the local refernce frame of the sensor to the global reference frame.
66 ///
67 /// # Examples
68 ///
69 /// ```
70 /// use nalgebra as na;
71 /// use na::Point3;
72 /// use kalman_rs::sensor_traits::Transform;
73 ///
74 /// let rectangular_points = [Point3::new(0.0, 0.0, 0.0),
75 /// Point3::new(5.0,0.0,0.0),
76 /// Point3::new(0.0,5.0,0.0),
77 /// Point3::new(5.0,5.0,0.0)];
78 /// let tfm_matrix : na::Matrix4<f32>= na::Matrix4::new(1.0,5.0,7.0,2.0, 3.0,5.0,7.0,4.0, 8.0,4.0,1.0,9.0, 2.0,6.0,4.0,8.0);
79 /// let mut rectangle_sensor = kalman_rs::Rectangle::new(rectangular_points, tfm_matrix).unwrap();
80 ///
81 /// let global_point = rectangle_sensor.to_local(na::Point3::new(6.0, 3.0, 5.0));
82 /// ```
83 fn to_local(&self, input_point: Point3<f32>) -> na::Point3<f32>{
84 self.tfm.inverse() * input_point
85 }
86
87
88 /// Checks if a local point is contained within the bounds of a sensor.
89 /// NOTE: `plane()` must be called before checking for bounds of the sensor since the normal
90 /// vector must be calculated first.
91 ///
92 /// # Examples
93 /// ```
94 /// use nalgebra as na;
95 /// use na::Point3;
96 /// use kalman_rs::sensor_traits::Transform;
97 ///
98 /// let rectangular_points = [Point3::new(0.0, 0.0, 0.0),
99 /// Point3::new(5.0,0.0,0.0),
100 /// Point3::new(5.0,5.0,0.0),
101 /// Point3::new(5.0, 0.0, 0.0)];
102 /// let tfm_matrix : na::Matrix4<f32>= na::Matrix4::new(1.0,5.0,7.0,2.0, 3.0,5.0,7.0,4.0, 8.0,4.0,1.0,9.0, 2.0,6.0,4.0,8.0);
103 /// let mut rectangle_sensor = kalman_rs::Rectangle::new(rectangular_points, tfm_matrix).unwrap();
104 ///
105 /// let is_point_on_sensor = rectangle_sensor.contains_from_local(&na::Point3::new(1.0, 6.0, 0.0));
106 /// ```
107 fn contains_from_local(&self, input: &Point3<f32>) ->Result<bool, &'static str> {
108 let xy_contains = traits::quadralateral_contains(&self.points, &input);
109 let z_contains = self.on_plane(&input);
110
111 match z_contains{
112 Ok(x) =>{
113 if xy_contains && x {
114 return Ok(true)
115 }
116 Ok(false)
117 },
118 Err(x) => return Err(x)
119 }
120 }
121}
122
123
124impl Plane for Rectangle{
125 /// Calculate the current normal vector of the plane of the surface.
126 /// NOTE: `plane()` must be called before `contains_from_local` since `contains_from_local`
127 /// requires the normal vector to be defined
128 ///
129 /// # Examples
130 ///
131 /// ```
132 /// use nalgebra as na;
133 /// use na::Point3;
134 /// use kalman_rs::sensor_traits::Plane;
135 ///
136 /// let rectangular_points = [Point3::new(0.0, 0.0, 0.0),
137 /// Point3::new(5.0,0.0,0.0),
138 /// Point3::new(0.0,5.0,0.0),
139 /// Point3::new(5.0,5.0,0.0)];
140 /// let tfm_matrix : na::Matrix4<f32>= na::Matrix4::new(1.0,5.0,7.0,2.0, 3.0,5.0,7.0,4.0, 8.0,4.0,1.0,9.0, 2.0,6.0,4.0,8.0);
141 /// let mut rectangle_sensor = kalman_rs::Rectangle::new(rectangular_points, tfm_matrix).unwrap();
142 ///
143 /// let normal_vector = rectangle_sensor.plane();
144 /// ```
145 fn plane(&mut self) -> Vector<f32>{
146 // calculate the normal vector of the surface if it has not been calculated before
147 // if it has been calculated return the original calculation
148 // this would need to change if the suface moves
149 match self.normal{
150 Some(x)=>x,
151 None =>{
152 let normal_vector = traits::plane_normal_vector(&self.points[0], &self.points[1], &self.points[2]);
153 self.normal = Some(normal_vector);
154 normal_vector
155 },
156 }
157 }
158
159 /// Check if a given point is located on the same plane as the sensor
160 /// NOTE: `plane()` must be called becuase the normal vector is not currently known
161 /// # Examples
162 ///
163 /// ```
164 /// use nalgebra as na;
165 /// use na::Point3;
166 /// use kalman_rs::sensor_traits::Plane;
167 ///
168 /// let rectangular_points = [Point3::new(0.0, 0.0, 0.0),
169 /// Point3::new(5.0,0.0,0.0),
170 /// Point3::new(0.0,5.0,0.0),
171 /// Point3::new(5.0,5.0,0.0)];
172 /// let tfm_matrix : na::Matrix4<f32>= na::Matrix4::new(1.0,5.0,7.0,2.0, 3.0,5.0,7.0,4.0, 8.0,4.0,1.0,9.0, 2.0,6.0,4.0,8.0);
173 /// let mut rectangle_sensor = kalman_rs::Rectangle::new(rectangular_points, tfm_matrix).unwrap();
174 ///
175 /// let on_plane = rectangle_sensor.on_plane(&na::Point3::new(1.0, 3.0, 0.0)); //true
176 /// ```
177 fn on_plane(&self, input_point: &Point3<f32>) -> Result<bool, &'static str> {
178 let pv = traits::vector3_from_points(&self.points[0], &input_point);
179 match self.normal{
180 Some(x) => {
181 if x.dot(&pv) ==0.0 {
182 return Ok(true)
183 }
184 Ok(false)
185 }
186 None => Err("self.plane() method must be called before normal vector can be used")
187 }
188
189 }
190}