cam_geom/
ray_bundle_types.rs

1use nalgebra::{OMatrix, SMatrix};
2
3use crate::*;
4
5#[cfg(feature = "serde-serialize")]
6use serde::{Deserialize, Serialize};
7
8/// A bundle of rays with the same arbitrary origin
9#[derive(Debug, Clone, PartialEq)]
10#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
11pub struct SharedOriginRayBundle<R: RealField> {
12    center: Point3<R>,
13}
14
15impl<R> SharedOriginRayBundle<R>
16where
17    R: RealField,
18{
19    /// Create a new SharedOriginRayBundle with origin (center) at zero.
20    #[inline]
21    pub fn new_shared_zero_origin() -> Self {
22        // center is at (0,0,0)
23        let zero: R = nalgebra::convert(0.0);
24        Self {
25            center: Point3::new(zero.clone(), zero.clone(), zero),
26        }
27    }
28}
29
30impl<R> Bundle<R> for SharedOriginRayBundle<R>
31where
32    R: RealField,
33{
34    #[inline]
35    fn to_single_ray<Coords>(&self, self_data: &SMatrix<R, 1, 3>) -> Ray<Coords, R>
36    where
37        Coords: CoordinateSystem,
38    {
39        Ray {
40            direction: self_data.clone(),
41            center: self.center.coords.transpose(),
42            c: std::marker::PhantomData,
43        }
44    }
45
46    fn directions<NPTS, StorageIn>(
47        &self,
48        self_data: &Matrix<R, NPTS, U3, StorageIn>,
49    ) -> Matrix<R, NPTS, U3, Owned<R, NPTS, U3>>
50    where
51        NPTS: nalgebra::DimName,
52        StorageIn: Storage<R, NPTS, U3>,
53        DefaultAllocator: Allocator<NPTS, U3>,
54    {
55        // TODO: do this more smartly/efficiently
56        let mut result = nalgebra::OMatrix::<R, NPTS, U3>::zeros();
57        for i in 0..self_data.nrows() {
58            for j in 0..3 {
59                result[(i, j)] = self_data[(i, j)].clone();
60            }
61        }
62        result
63    }
64
65    fn centers<NPTS, StorageIn>(
66        &self,
67        self_data: &Matrix<R, NPTS, U3, StorageIn>,
68    ) -> Matrix<R, NPTS, U3, Owned<R, NPTS, U3>>
69    where
70        NPTS: nalgebra::DimName,
71        StorageIn: Storage<R, NPTS, U3>,
72        DefaultAllocator: Allocator<NPTS, U3>,
73    {
74        // TODO: do this more smartly/efficiently
75        let mut result = nalgebra::OMatrix::<R, NPTS, U3>::zeros();
76        for i in 0..self_data.nrows() {
77            for j in 0..3 {
78                result[(i, j)] = self.center[j].clone();
79            }
80        }
81        result
82    }
83
84    fn point_on_ray<NPTS, StorageIn, OutFrame>(
85        &self,
86        directions: &Matrix<R, NPTS, U3, StorageIn>,
87    ) -> Points<OutFrame, R, NPTS, Owned<R, NPTS, U3>>
88    where
89        Self: Sized,
90        NPTS: Dim,
91        StorageIn: Storage<R, NPTS, U3>,
92        OutFrame: CoordinateSystem,
93        DefaultAllocator: Allocator<NPTS, U3>,
94    {
95        let mut result = Points::new(OMatrix::zeros_generic(
96            NPTS::from_usize(directions.nrows()),
97            U3::from_usize(3),
98        ));
99        let center = [
100            self.center[0].clone(),
101            self.center[1].clone(),
102            self.center[2].clone(),
103        ];
104        for i in 0..directions.nrows() {
105            for j in 0..3 {
106                result.data[(i, j)] = center[j].clone() + directions[(i, j)].clone();
107            }
108        }
109        result
110    }
111
112    fn point_on_ray_at_distance<NPTS, StorageIn, OutFrame>(
113        &self,
114        directions: &Matrix<R, NPTS, U3, StorageIn>,
115        distance: R,
116    ) -> Points<OutFrame, R, NPTS, Owned<R, NPTS, U3>>
117    where
118        Self: Sized,
119        NPTS: Dim,
120        StorageIn: Storage<R, NPTS, U3>,
121        OutFrame: CoordinateSystem,
122        DefaultAllocator: Allocator<NPTS, U3>,
123    {
124        let mut result = Points::new(OMatrix::zeros_generic(
125            NPTS::from_usize(directions.nrows()),
126            U3::from_usize(3),
127        ));
128        let center = [
129            self.center[0].clone(),
130            self.center[1].clone(),
131            self.center[2].clone(),
132        ];
133        for i in 0..directions.nrows() {
134            let dx = directions[(i, 0)].clone();
135            let dy = directions[(i, 1)].clone();
136            let dz = directions[(i, 2)].clone();
137            let mag2 = dx.clone() * dx + dy.clone() * dy + dz.clone() * dz;
138            let mag = mag2.sqrt();
139            let scale = distance.clone() / mag;
140            for j in 0..3 {
141                result.data[(i, j)] =
142                    center[j].clone() + scale.clone() * directions[(i, j)].clone();
143            }
144        }
145        result
146    }
147
148    fn to_pose<NPTS, StorageIn, OutFrame>(
149        &self,
150        pose: Isometry3<R>,
151        self_data: &Matrix<R, NPTS, U3, StorageIn>,
152    ) -> RayBundle<OutFrame, Self, R, NPTS, Owned<R, NPTS, U3>>
153    where
154        NPTS: Dim,
155        StorageIn: Storage<R, NPTS, U3>,
156        OutFrame: CoordinateSystem,
157        DefaultAllocator: Allocator<NPTS, U3>,
158    {
159        let bundle_type = Self::new_shared_zero_origin();
160        let mut reposed = RayBundle::new(
161            bundle_type,
162            OMatrix::zeros_generic(NPTS::from_usize(self_data.nrows()), U3::from_usize(3)),
163        );
164        // transform single bundle center point
165        let new_center = pose.transform_point(&self.center);
166
167        // transform multiple bundle directions
168        for i in 0..self_data.nrows() {
169            let orig_vec = self_data.row(i).transpose();
170            let new_vec = pose.transform_vector(&orig_vec);
171            for j in 0..3 {
172                reposed.data[(i, j)] = new_vec[j].clone();
173            }
174        }
175
176        reposed.bundle_type = SharedOriginRayBundle { center: new_center };
177        reposed
178    }
179}
180
181/// A bundle of rays with the same arbitrary direction
182#[derive(Debug, Clone, PartialEq)]
183#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
184pub struct SharedDirectionRayBundle<R: RealField> {
185    direction: Vector3<R>,
186}
187
188impl<R: RealField> SharedDirectionRayBundle<R> {
189    /// Create a new SharedDirectionRayBundle with direction +z.
190    pub fn new_plusz_shared_direction() -> Self {
191        // in +z direction
192        Self {
193            direction: Vector3::new(R::zero(), R::zero(), R::one()),
194        }
195    }
196}
197
198impl<R: RealField> Bundle<R> for SharedDirectionRayBundle<R> {
199    fn to_single_ray<Coords>(&self, self_data: &SMatrix<R, 1, 3>) -> Ray<Coords, R>
200    where
201        Coords: CoordinateSystem,
202    {
203        Ray {
204            direction: self.direction.transpose(),
205            center: self_data.clone(),
206            c: std::marker::PhantomData,
207        }
208    }
209
210    fn directions<NPTS, StorageIn>(
211        &self,
212        self_data: &Matrix<R, NPTS, U3, StorageIn>,
213    ) -> Matrix<R, NPTS, U3, Owned<R, NPTS, U3>>
214    where
215        NPTS: nalgebra::DimName,
216        StorageIn: Storage<R, NPTS, U3>,
217        DefaultAllocator: Allocator<NPTS, U3>,
218    {
219        // TODO: do this more smartly/efficiently
220        let mut result = nalgebra::OMatrix::<R, NPTS, U3>::zeros();
221        for i in 0..self_data.nrows() {
222            for j in 0..3 {
223                result[(i, j)] = self.direction[j].clone();
224            }
225        }
226        result
227    }
228
229    fn centers<NPTS, StorageIn>(
230        &self,
231        self_data: &Matrix<R, NPTS, U3, StorageIn>,
232    ) -> Matrix<R, NPTS, U3, Owned<R, NPTS, U3>>
233    where
234        NPTS: nalgebra::DimName,
235        StorageIn: Storage<R, NPTS, U3>,
236        DefaultAllocator: Allocator<NPTS, U3>,
237    {
238        // TODO: do this more smartly/efficiently
239        let mut result = nalgebra::OMatrix::<R, NPTS, U3>::zeros();
240        for i in 0..self_data.nrows() {
241            for j in 0..3 {
242                result[(i, j)] = self_data[(i, j)].clone();
243            }
244        }
245        result
246    }
247
248    fn point_on_ray<NPTS, StorageIn, OutFrame>(
249        &self,
250        centers: &Matrix<R, NPTS, U3, StorageIn>,
251    ) -> Points<OutFrame, R, NPTS, Owned<R, NPTS, U3>>
252    where
253        Self: Sized,
254        NPTS: Dim,
255        StorageIn: Storage<R, NPTS, U3>,
256        OutFrame: CoordinateSystem,
257        DefaultAllocator: Allocator<NPTS, U3>,
258    {
259        let mut result = Points::new(OMatrix::zeros_generic(
260            NPTS::from_usize(centers.nrows()),
261            U3::from_usize(3),
262        ));
263        let direction = [
264            self.direction[0].clone(),
265            self.direction[1].clone(),
266            self.direction[2].clone(),
267        ];
268        for i in 0..centers.nrows() {
269            for j in 0..3 {
270                result.data[(i, j)] = direction[j].clone() + centers[(i, j)].clone();
271            }
272        }
273        result
274    }
275
276    fn point_on_ray_at_distance<NPTS, StorageIn, OutFrame>(
277        &self,
278        centers: &Matrix<R, NPTS, U3, StorageIn>,
279        distance: R,
280    ) -> Points<OutFrame, R, NPTS, Owned<R, NPTS, U3>>
281    where
282        Self: Sized,
283        NPTS: Dim,
284        StorageIn: Storage<R, NPTS, U3>,
285        OutFrame: CoordinateSystem,
286        DefaultAllocator: Allocator<NPTS, U3>,
287    {
288        let mut result = Points::new(OMatrix::zeros_generic(
289            NPTS::from_usize(centers.nrows()),
290            U3::from_usize(3),
291        ));
292
293        let d = &self.direction;
294        let dx = d[0].clone();
295        let dy = d[1].clone();
296        let dz = d[2].clone();
297        let mag2 = dx.clone() * dx.clone() + dy.clone() * dy.clone() + dz.clone() * dz.clone();
298        let mag = mag2.sqrt();
299        let scale = distance / mag;
300        let dist_dir = Vector3::new(scale.clone() * dx, scale.clone() * dy, scale * dz);
301
302        for i in 0..centers.nrows() {
303            for j in 0..3 {
304                result.data[(i, j)] = dist_dir[j].clone() + centers[(i, j)].clone();
305            }
306        }
307        result
308    }
309
310    fn to_pose<NPTS, StorageIn, OutFrame>(
311        &self,
312        pose: Isometry3<R>,
313        self_data: &Matrix<R, NPTS, U3, StorageIn>,
314    ) -> RayBundle<OutFrame, Self, R, NPTS, Owned<R, NPTS, U3>>
315    where
316        NPTS: Dim,
317        StorageIn: Storage<R, NPTS, U3>,
318        OutFrame: CoordinateSystem,
319        DefaultAllocator: Allocator<NPTS, U3>,
320    {
321        let bundle_type = Self::new_plusz_shared_direction();
322
323        let mut reposed = RayBundle::new(
324            bundle_type,
325            OMatrix::zeros_generic(NPTS::from_usize(self_data.nrows()), U3::from_usize(3)),
326        );
327
328        // transform single bundle direction
329        let new_direction = pose.transform_vector(&self.direction);
330
331        // transform multiple bundle origins
332        for i in 0..self_data.nrows() {
333            let orig_point = Point3 {
334                coords: self_data.row(i).transpose(),
335            };
336            let new_point = pose.transform_point(&orig_point);
337            for j in 0..3 {
338                reposed.data[(i, j)] = new_point[j].clone();
339            }
340        }
341
342        reposed.bundle_type = SharedDirectionRayBundle {
343            direction: new_direction,
344        };
345        reposed
346    }
347}
cam_geom/ray_bundle_types.rs

cam_geom/
ray_bundle_types.rs
