cu_spatial_payloads/
geometry.rs

1//! The shared point vocabulary: unit-typed points and axis-aligned bounding
2//! boxes spatial components compose over.
3
4use bincode::{Decode, Encode};
5use core::fmt::Debug;
6use cu29::prelude::*;
7use cu29::units::si::area::square_meter;
8use cu29::units::si::f32::Area as Area32;
9use cu29::units::si::f32::Length as Length32;
10use cu29::units::si::f64::Area as Area64;
11use cu29::units::si::f64::Length as Length64;
12use cu29::units::si::length::meter;
13use cu29_soa_derive::Soa;
14use serde::{Deserialize, Serialize};
15
16/// A 2D point with unit-typed coordinates.
17///
18/// This is a scalar type; for large batches use [`Point2Soa`] so bulk
19/// operations vectorize.
20#[derive(
21    Default, Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect, Soa,
22)]
23pub struct Point2<L: Copy + Debug + 'static> {
24    pub x: L,
25    pub y: L,
26}
27
28/// A 3D point with unit-typed coordinates.
29///
30/// This is a scalar type; for large batches use [`Point3Soa`] so bulk
31/// operations vectorize.
32#[derive(
33    Default, Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect, Soa,
34)]
35pub struct Point3<L: Copy + Debug + 'static> {
36    pub x: L,
37    pub y: L,
38    pub z: L,
39}
40
41pub type Point2f = Point2<Length32>;
42pub type Point2d = Point2<Length64>;
43pub type Point3f = Point3<Length32>;
44pub type Point3d = Point3<Length64>;
45
46/// A 2D point in pixel coordinates, for image-space geometry.
47pub type Point2u = Point2<u32>;
48/// A 2D point in signed pixel coordinates, for image-space offsets.
49pub type Point2i = Point2<i32>;
50
51pub type Point2fSoa<const N: usize> = Point2Soa<Length32, N>;
52pub type Point2dSoa<const N: usize> = Point2Soa<Length64, N>;
53pub type Point3fSoa<const N: usize> = Point3Soa<Length32, N>;
54pub type Point3dSoa<const N: usize> = Point3Soa<Length64, N>;
55pub type Point2uSoa<const N: usize> = Point2Soa<u32, N>;
56pub type Point2iSoa<const N: usize> = Point2Soa<i32, N>;
57
58impl<L: Copy + Debug + 'static> Point2<L> {
59    pub const fn new(x: L, y: L) -> Self {
60        Self { x, y }
61    }
62
63    /// The point lifted to 3D at height `z`.
64    pub fn with_z(self, z: L) -> Point3<L> {
65        Point3::new(self.x, self.y, z)
66    }
67}
68
69impl<L: Copy + Debug + 'static> Point3<L> {
70    pub const fn new(x: L, y: L, z: L) -> Self {
71        Self { x, y, z }
72    }
73
74    /// The planar projection: z dropped.
75    pub fn xy(self) -> Point2<L> {
76        Point2::new(self.x, self.y)
77    }
78}
79
80macro_rules! impl_point_metrics {
81    ($len:ty, $area:ty, $scalar:ty, $sqrt:path) => {
82        impl Point2<$len> {
83            pub fn from_meters(x: $scalar, y: $scalar) -> Self {
84                Self::new(<$len>::new::<meter>(x), <$len>::new::<meter>(y))
85            }
86
87            /// Euclidean distance to `other`.
88            pub fn distance(self, other: Self) -> $len {
89                let (dx, dy) = ((self.x - other.x).raw(), (self.y - other.y).raw());
90                <$len>::new::<meter>($sqrt(dx * dx + dy * dy))
91            }
92
93            /// The point at `ratio` of the way toward `other`. Ratio 0
94            /// returns `self` exactly; ratio 1 returns `other` up to rounding.
95            pub fn lerp(self, other: Self, ratio: $scalar) -> Self {
96                Self::new(
97                    self.x + (other.x - self.x) * ratio,
98                    self.y + (other.y - self.y) * ratio,
99                )
100            }
101        }
102
103        impl Point3<$len> {
104            pub fn from_meters(x: $scalar, y: $scalar, z: $scalar) -> Self {
105                Self::new(
106                    <$len>::new::<meter>(x),
107                    <$len>::new::<meter>(y),
108                    <$len>::new::<meter>(z),
109                )
110            }
111
112            /// Euclidean distance to `other`.
113            pub fn distance(self, other: Self) -> $len {
114                let (dx, dy, dz) = (
115                    (self.x - other.x).raw(),
116                    (self.y - other.y).raw(),
117                    (self.z - other.z).raw(),
118                );
119                <$len>::new::<meter>($sqrt(dx * dx + dy * dy + dz * dz))
120            }
121
122            /// The point at `ratio` of the way toward `other`. Ratio 0
123            /// returns `self` exactly; ratio 1 returns `other` up to rounding.
124            pub fn lerp(self, other: Self, ratio: $scalar) -> Self {
125                Self::new(
126                    self.x + (other.x - self.x) * ratio,
127                    self.y + (other.y - self.y) * ratio,
128                    self.z + (other.z - self.z) * ratio,
129                )
130            }
131        }
132
133        impl<const N: usize> Point2Soa<$len, N> {
134            /// Squared distance from every point to `target`, written to
135            /// `out[..len]`. Sqrt-free, so the loop vectorizes; enough for
136            /// nearest-neighbor style comparisons. Accurate to 1 ulp.
137            ///
138            /// # Panics
139            /// If `out` is shorter than `self.len()`.
140            pub fn distances_squared(&self, target: Point2<$len>, out: &mut [$area]) {
141                let n = self.len();
142                let (xs, ys, out) = (&self.x[..n], &self.y[..n], &mut out[..n]);
143                let (tx, ty) = (target.x.raw(), target.y.raw());
144                for i in 0..n {
145                    let (dx, dy) = (xs[i].raw() - tx, ys[i].raw() - ty);
146                    out[i] = <$area>::new::<square_meter>(dx * dx + dy * dy);
147                }
148            }
149
150            /// Distance from every point to `target`, written to `out[..len]`.
151            ///
152            /// # Panics
153            /// If `out` is shorter than `self.len()`.
154            pub fn distances(&self, target: Point2<$len>, out: &mut [$len]) {
155                let n = self.len();
156                let (xs, ys, out) = (&self.x[..n], &self.y[..n], &mut out[..n]);
157                let (tx, ty) = (target.x.raw(), target.y.raw());
158                for i in 0..n {
159                    let (dx, dy) = (xs[i].raw() - tx, ys[i].raw() - ty);
160                    out[i] = <$len>::new::<meter>($sqrt(dx * dx + dy * dy));
161                }
162            }
163
164            /// Every point moved `ratio` of the way toward `target`, in place.
165            pub fn lerp_toward(&mut self, target: Point2<$len>, ratio: $scalar) {
166                let n = self.len();
167                for i in 0..n {
168                    self.x[i] = self.x[i] + (target.x - self.x[i]) * ratio;
169                    self.y[i] = self.y[i] + (target.y - self.y[i]) * ratio;
170                }
171            }
172        }
173
174        impl<const N: usize> Point3Soa<$len, N> {
175            /// Squared distance from every point to `target`, written to
176            /// `out[..len]`. Sqrt-free, so the loop vectorizes; enough for
177            /// nearest-neighbor style comparisons. Accurate to 1 ulp.
178            ///
179            /// # Panics
180            /// If `out` is shorter than `self.len()`.
181            pub fn distances_squared(&self, target: Point3<$len>, out: &mut [$area]) {
182                let n = self.len();
183                let (xs, ys, zs) = (&self.x[..n], &self.y[..n], &self.z[..n]);
184                let out = &mut out[..n];
185                let (tx, ty, tz) = (target.x.raw(), target.y.raw(), target.z.raw());
186                for i in 0..n {
187                    let (dx, dy, dz) = (xs[i].raw() - tx, ys[i].raw() - ty, zs[i].raw() - tz);
188                    out[i] = <$area>::new::<square_meter>(dx * dx + dy * dy + dz * dz);
189                }
190            }
191
192            /// Distance from every point to `target`, written to `out[..len]`.
193            ///
194            /// # Panics
195            /// If `out` is shorter than `self.len()`.
196            pub fn distances(&self, target: Point3<$len>, out: &mut [$len]) {
197                let n = self.len();
198                let (xs, ys, zs) = (&self.x[..n], &self.y[..n], &self.z[..n]);
199                let out = &mut out[..n];
200                let (tx, ty, tz) = (target.x.raw(), target.y.raw(), target.z.raw());
201                for i in 0..n {
202                    let (dx, dy, dz) = (xs[i].raw() - tx, ys[i].raw() - ty, zs[i].raw() - tz);
203                    out[i] = <$len>::new::<meter>($sqrt(dx * dx + dy * dy + dz * dz));
204                }
205            }
206
207            /// Every point moved `ratio` of the way toward `target`, in place.
208            pub fn lerp_toward(&mut self, target: Point3<$len>, ratio: $scalar) {
209                let n = self.len();
210                for i in 0..n {
211                    self.x[i] = self.x[i] + (target.x - self.x[i]) * ratio;
212                    self.y[i] = self.y[i] + (target.y - self.y[i]) * ratio;
213                    self.z[i] = self.z[i] + (target.z - self.z[i]) * ratio;
214                }
215            }
216        }
217    };
218}
219
220impl_point_metrics!(Length32, Area32, f32, libm::sqrtf);
221impl_point_metrics!(Length64, Area64, f64, libm::sqrt);
222
223/// An axis-aligned bounding box; the point type carries the dimension.
224///
225/// This is a scalar type; for large batches (e.g. detection anchors) consider
226/// an SoA layout so bulk operations vectorize (see `cu29_soa_derive`).
227#[derive(
228    Default, Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect,
229)]
230pub struct BBox<P: Copy + Debug + 'static> {
231    pub min: P,
232    pub max: P,
233}
234
235pub type BBox2f = BBox<Point2f>;
236pub type BBox2d = BBox<Point2d>;
237pub type BBox3f = BBox<Point3f>;
238pub type BBox3d = BBox<Point3d>;
239
240/// A 2D bounding box in pixel coordinates, for image-space geometry.
241pub type BBox2u = BBox<Point2u>;
242/// A 2D bounding box in signed pixel coordinates.
243pub type BBox2i = BBox<Point2i>;
244
245impl<P: Copy + Debug + 'static> BBox<P> {
246    pub const fn new(min: P, max: P) -> Self {
247        Self { min, max }
248    }
249}
250
251impl<L: Copy + Debug + PartialOrd + 'static> BBox<Point2<L>> {
252    /// True when `p` lies inside the box, boundary included.
253    pub fn contains(&self, p: Point2<L>) -> bool {
254        self.min.x <= p.x && p.x <= self.max.x && self.min.y <= p.y && p.y <= self.max.y
255    }
256
257    /// `contains` for every point in the set, written to `out[..len]`.
258    ///
259    /// # Panics
260    /// If `out` is shorter than `points.len()`.
261    pub fn contains_points<const N: usize>(&self, points: &Point2Soa<L, N>, out: &mut [bool]) {
262        let n = points.len();
263        let (xs, ys, out) = (&points.x[..n], &points.y[..n], &mut out[..n]);
264        for i in 0..n {
265            out[i] = (self.min.x <= xs[i])
266                & (xs[i] <= self.max.x)
267                & (self.min.y <= ys[i])
268                & (ys[i] <= self.max.y);
269        }
270    }
271}
272
273impl<L: Copy + Debug + PartialOrd + 'static> BBox<Point3<L>> {
274    /// True when `p` lies inside the box, boundary included.
275    pub fn contains(&self, p: Point3<L>) -> bool {
276        self.min.x <= p.x
277            && p.x <= self.max.x
278            && self.min.y <= p.y
279            && p.y <= self.max.y
280            && self.min.z <= p.z
281            && p.z <= self.max.z
282    }
283
284    /// `contains` for every point in the set, written to `out[..len]`.
285    ///
286    /// # Panics
287    /// If `out` is shorter than `points.len()`.
288    pub fn contains_points<const N: usize>(&self, points: &Point3Soa<L, N>, out: &mut [bool]) {
289        let n = points.len();
290        let (xs, ys, zs) = (&points.x[..n], &points.y[..n], &points.z[..n]);
291        let out = &mut out[..n];
292        for i in 0..n {
293            out[i] = (self.min.x <= xs[i])
294                & (xs[i] <= self.max.x)
295                & (self.min.y <= ys[i])
296                & (ys[i] <= self.max.y)
297                & (self.min.z <= zs[i])
298                & (zs[i] <= self.max.z);
299        }
300    }
301}
302
303#[cfg(test)]
304mod tests {
305    use super::*;
306
307    #[test]
308    fn point_distance_and_lerp() {
309        let a = Point2f::from_meters(1.0, 2.0);
310        let b = Point2f::from_meters(4.0, 6.0);
311        assert_eq!(a.distance(b).raw(), 5.0);
312        // Ratio 0 is exact; ratio 1 is only exact up to rounding.
313        assert_eq!(a.lerp(b, 0.0), a);
314        assert!((a.lerp(b, 1.0).distance(b)).raw() <= 1e-6);
315        assert_eq!(a.lerp(b, 0.5), Point2f::from_meters(2.5, 4.0));
316
317        let a = Point3d::from_meters(1.0, 2.0, 3.0);
318        let b = Point3d::from_meters(3.0, 5.0, 9.0);
319        assert_eq!(a.distance(b).raw(), 7.0);
320        assert_eq!(a.lerp(b, 0.5), Point3d::from_meters(2.0, 3.5, 6.0));
321    }
322
323    #[test]
324    fn point_dimension_conversions() {
325        let p = Point3f::from_meters(1.0, 2.0, 3.0);
326        assert_eq!(p.xy(), Point2f::from_meters(1.0, 2.0));
327        assert_eq!(p.xy().with_z(p.z), p);
328    }
329
330    #[test]
331    fn bbox_contains_boundary_included() {
332        let b = BBox2f::new(
333            Point2f::from_meters(0.0, 0.0),
334            Point2f::from_meters(2.0, 2.0),
335        );
336        assert!(b.contains(Point2f::from_meters(1.0, 1.0)));
337        assert!(b.contains(Point2f::from_meters(0.0, 2.0)));
338        assert!(!b.contains(Point2f::from_meters(-0.1, 1.0)));
339        assert!(!b.contains(Point2f::from_meters(1.0, 2.1)));
340
341        let b = BBox3f::new(
342            Point3f::from_meters(0.0, 0.0, 0.0),
343            Point3f::from_meters(1.0, 1.0, 1.0),
344        );
345        assert!(b.contains(Point3f::from_meters(0.5, 0.5, 1.0)));
346        assert!(!b.contains(Point3f::from_meters(0.5, 0.5, 1.1)));
347    }
348
349    #[test]
350    fn pixel_bbox_contains() {
351        let b = BBox2u::new(Point2u::new(10, 20), Point2u::new(110, 220));
352        assert!(b.contains(Point2u::new(10, 220)));
353        assert!(b.contains(Point2u::new(60, 120)));
354        assert!(!b.contains(Point2u::new(9, 120)));
355        assert!(!b.contains(Point2u::new(60, 221)));
356    }
357
358    #[test]
359    fn soa_distances_match_scalar() {
360        let mut set = Point2fSoa::<8>::default();
361        let target = Point2f::from_meters(1.0, -2.0);
362        let points = [(0.0, 0.0), (3.0, 2.0), (-1.5, 4.0)];
363        for (x, y) in points {
364            set.push(Point2f::from_meters(x, y));
365        }
366
367        let mut d = [Length32::default(); 3];
368        let mut d2 = [Area32::default(); 3];
369        set.distances(target, &mut d);
370        set.distances_squared(target, &mut d2);
371        for i in 0..set.len() {
372            let scalar = set.get(i).distance(target);
373            assert!((d[i] - scalar).raw().abs() < 1e-6);
374            assert!((d2[i].raw() - scalar.raw() * scalar.raw()).abs() < 1e-5);
375        }
376
377        let mut set3 = Point3dSoa::<4>::default();
378        set3.push(Point3d::from_meters(1.0, 2.0, 3.0));
379        set3.push(Point3d::from_meters(-2.0, 0.5, 1.0));
380        let target3 = Point3d::from_meters(0.0, 1.0, -1.0);
381        let mut d3 = [Length64::default(); 2];
382        set3.distances(target3, &mut d3);
383        for (i, d) in d3.iter().enumerate() {
384            assert!((*d - set3.get(i).distance(target3)).raw().abs() < 1e-12);
385        }
386    }
387
388    #[test]
389    fn soa_lerp_toward_matches_scalar() {
390        let mut set = Point2fSoa::<4>::default();
391        set.push(Point2f::from_meters(0.0, 0.0));
392        set.push(Point2f::from_meters(4.0, -2.0));
393        let target = Point2f::from_meters(2.0, 2.0);
394
395        let expected: [Point2f; 2] = [set.get(0).lerp(target, 0.25), set.get(1).lerp(target, 0.25)];
396        set.lerp_toward(target, 0.25);
397        assert_eq!(set.get(0), expected[0]);
398        assert_eq!(set.get(1), expected[1]);
399    }
400
401    #[test]
402    fn bbox_contains_points_bulk() {
403        let b = BBox2f::new(
404            Point2f::from_meters(0.0, 0.0),
405            Point2f::from_meters(2.0, 2.0),
406        );
407        let mut set = Point2fSoa::<4>::default();
408        set.push(Point2f::from_meters(1.0, 1.0));
409        set.push(Point2f::from_meters(0.0, 2.0));
410        set.push(Point2f::from_meters(-0.1, 1.0));
411        let mut out = [false; 3];
412        b.contains_points(&set, &mut out);
413        assert_eq!(out, [true, true, false]);
414
415        let pix = BBox2u::new(Point2u::new(10, 20), Point2u::new(110, 220));
416        let mut pixels = Point2uSoa::<4>::default();
417        pixels.push(Point2u::new(60, 120));
418        pixels.push(Point2u::new(9, 120));
419        let mut out = [false; 2];
420        pix.contains_points(&pixels, &mut out);
421        assert_eq!(out, [true, false]);
422    }
423
424    #[test]
425    fn bbox3_contains_points_bulk_matches_scalar() {
426        let b = BBox3f::new(
427            Point3f::from_meters(0.0, 0.0, 0.0),
428            Point3f::from_meters(1.0, 1.0, 1.0),
429        );
430        let points = [
431            Point3f::from_meters(0.5, 0.5, 1.0),  // on the z boundary
432            Point3f::from_meters(0.5, 0.5, 1.1),  // outside in z
433            Point3f::from_meters(0.0, 0.0, 0.0),  // corner
434            Point3f::from_meters(-0.1, 0.5, 0.5), // outside in x
435        ];
436        let mut set = Point3fSoa::<8>::default();
437        for p in points {
438            set.push(p);
439        }
440
441        // `out` is longer than the set; the tail stays untouched.
442        let mut out = [true; 6];
443        b.contains_points(&set, &mut out);
444        assert_eq!(out, [true, false, true, false, true, true]);
445        for (i, p) in points.iter().enumerate() {
446            assert_eq!(out[i], b.contains(*p));
447        }
448    }
449
450    #[test]
451    fn bulk_kernels_handle_an_empty_set() {
452        let set = Point2fSoa::<4>::default();
453        assert!(set.is_empty());
454        set.distances(Point2f::from_meters(1.0, 1.0), &mut []);
455        set.distances_squared(Point2f::from_meters(1.0, 1.0), &mut []);
456        let b = BBox2f::new(
457            Point2f::from_meters(0.0, 0.0),
458            Point2f::from_meters(1.0, 1.0),
459        );
460        b.contains_points(&set, &mut []);
461    }
462
463    /// Replaying a log recorded at a larger capacity must error, not panic.
464    #[test]
465    fn decode_rejects_len_over_capacity() {
466        let mut wide = Point2fSoa::<8>::default();
467        for i in 0..8 {
468            wide.push(Point2f::from_meters(i as f32, -(i as f32)));
469        }
470        let cfg = cu29::bincode::config::standard();
471        let bytes = cu29::bincode::encode_to_vec(&wide, cfg).expect("encode");
472
473        let narrow: Result<(Point2fSoa<4>, usize), _> =
474            cu29::bincode::decode_from_slice(&bytes, cfg);
475        assert!(narrow.is_err(), "expected a capacity error");
476
477        let (same, _): (Point2fSoa<8>, _) =
478            cu29::bincode::decode_from_slice(&bytes, cfg).expect("decode");
479        assert_eq!(same.len(), 8);
480        assert_eq!(same.get(7), wide.get(7));
481    }
482}
cu_spatial_payloads/geometry.rs

cu_spatial_payloads/
geometry.rs
