cu_sensor_payloads/
pointcloud.rs

1use crate::{CuHandlePayload, CuHandlePayloadInit, CuHandlePayloadMeta};
2use alloc::alloc::{Layout, alloc_zeroed, handle_alloc_error};
3use alloc::boxed::Box;
4use bincode::de::Decoder;
5use bincode::error::DecodeError;
6use bincode::{Decode, Encode};
7use cu29::prelude::*;
8use cu29::units::si::f32::{Length, Ratio};
9use cu29::units::si::length::meter;
10use cu29::units::si::ratio::percent;
11use cu29_clock::CuTime;
12use cu29_soa_derive::Soa;
13use serde::{Deserialize, Serialize};
14
15pub type Distance = Length;
16pub type Reflectivity = Ratio;
17
18/// Standardized PointCloud.
19/// note: the derive(Soa) will generate a PointCloudSoa struct that will store the data in a SoA format.
20/// The Soa format is appropriate for early pipeline operations like changing their frame of reference.
21/// important: The ToV of the points are not assumed to be sorted.
22#[derive(
23    Default, Clone, PartialEq, Debug, Soa, Serialize, Deserialize, Encode, Decode, Reflect,
24)]
25#[reflect(from_reflect = false)]
26pub struct PointCloud {
27    pub tov: CuTime, // Time of Validity, not sorted.
28    pub x: Distance,
29    pub y: Distance,
30    pub z: Distance,
31    pub i: Reflectivity,
32    pub return_order: u8, // 0 for first return, 1 for second return, etc.
33}
34
35impl PointCloud {
36    pub fn new(tov: CuTime, x: f32, y: f32, z: f32, i: f32, return_order: Option<u8>) -> Self {
37        Self {
38            tov,
39            x: Distance::new::<meter>(x),
40            y: Distance::new::<meter>(y),
41            z: Distance::new::<meter>(z),
42            i: Reflectivity::new::<percent>(i),
43            return_order: return_order.unwrap_or(0),
44        }
45    }
46
47    pub fn new_units(
48        tov: CuTime,
49        x: Length,
50        y: Length,
51        z: Length,
52        i: Ratio,
53        return_order: Option<u8>,
54    ) -> Self {
55        Self {
56            tov,
57            x,
58            y,
59            z,
60            i,
61            return_order: return_order.unwrap_or(0),
62        }
63    }
64}
65
66pub struct PointCloudSoaHandleMeta;
67
68impl CuHandlePayloadMeta for PointCloudSoaHandleMeta {
69    const TYPE_PATH: &'static str = "cu_sensor_payloads::PointCloudSoaHandle";
70    const SHORT_TYPE_PATH: &'static str = "PointCloudSoaHandle";
71    const TYPE_IDENT: Option<&'static str> = Some("PointCloudSoaHandle");
72    const CRATE_NAME: Option<&'static str> = Some("cu_sensor_payloads");
73    const MODULE_PATH: Option<&'static str> = Some("cu_sensor_payloads");
74}
75
76/// Copper point-cloud payload stored behind a `CuHandle` so large clouds do not live inline on
77/// the thread stack.
78pub type PointCloudSoaHandle<const N: usize> =
79    CuHandlePayload<PointCloudSoa<N>, PointCloudSoaHandleMeta>;
80
81impl<const N: usize> PointCloudSoa<N> {
82    /// Allocate a zeroed point cloud directly on the heap.
83    ///
84    /// SAFETY: `PointCloudSoa<N>` is generated from `PointCloud`, whose fields are `usize`, `u8`,
85    /// `u64` (`CuTime`), and `uom::Quantity<f32>` wrappers. The quantity type is
86    /// `#[repr(transparent)]` over its scalar plus `PhantomData`, so an all-zero bit pattern is a
87    /// valid semantic zero for every field in the generated SoA storage.
88    pub fn boxed_zeroed() -> Box<Self> {
89        let layout = Layout::new::<Self>();
90        let ptr = unsafe { alloc_zeroed(layout) };
91        if ptr.is_null() {
92            handle_alloc_error(layout);
93        }
94        unsafe { Box::from_raw(ptr.cast()) }
95    }
96
97    /// Sort in place the point cloud so it can be ready for merge sorts for example
98    pub fn sort(&mut self) {
99        self.quicksort(0, N - 1);
100    }
101
102    /// Implementation of the sort
103    fn quicksort(&mut self, low: usize, high: usize) {
104        if low < high {
105            let pivot_index = self.partition(low, high);
106            if pivot_index > 0 {
107                self.quicksort(low, pivot_index - 1);
108            }
109            self.quicksort(pivot_index + 1, high);
110        }
111    }
112
113    /// Used by quicksort.
114    fn partition(&mut self, low: usize, high: usize) -> usize {
115        let pivot = self.tov[high];
116        let mut i = low;
117        for j in low..high {
118            if self.tov[j] <= pivot {
119                self.swap(i, j);
120                i += 1;
121            }
122        }
123        self.swap(i, high);
124        i
125    }
126
127    /// swap the elements at index a and b
128    pub fn swap(&mut self, a: usize, b: usize) {
129        self.tov.swap(a, b);
130        self.x.swap(a, b);
131        self.y.swap(a, b);
132        self.z.swap(a, b);
133        self.i.swap(a, b);
134    }
135}
136
137impl<const N: usize> CuHandlePayloadInit for PointCloudSoa<N> {
138    fn boxed_init() -> Box<Self> {
139        Self::boxed_zeroed()
140    }
141
142    fn decode_boxed<D>(decoder: &mut D) -> Result<Box<Self>, DecodeError>
143    where
144        D: Decoder<Context = ()>,
145    {
146        let mut result = Self::boxed_zeroed();
147        result.len = Decode::decode(decoder)?;
148        if result.len > N {
149            return Err(DecodeError::ArrayLengthMismatch {
150                required: N,
151                found: result.len,
152            });
153        }
154
155        for idx in 0..result.len {
156            result.tov[idx] = Decode::decode(decoder)?;
157        }
158        for idx in 0..result.len {
159            result.x[idx] = Decode::decode(decoder)?;
160        }
161        for idx in 0..result.len {
162            result.y[idx] = Decode::decode(decoder)?;
163        }
164        for idx in 0..result.len {
165            result.z[idx] = Decode::decode(decoder)?;
166        }
167        for idx in 0..result.len {
168            result.i[idx] = Decode::decode(decoder)?;
169        }
170        for idx in 0..result.len {
171            result.return_order[idx] = Decode::decode(decoder)?;
172        }
173
174        Ok(result)
175    }
176}
177
178#[cfg(test)]
179mod tests {
180    use super::*;
181    use cu29_clock::CuTime;
182
183    #[test]
184    fn test_point_payload() {
185        let payload = PointCloud::new(CuTime::from_nanos(1), 1.0, 2.0, 3.0, 0.0, None);
186        assert_eq!(payload.x.value, 1.0);
187        assert_eq!(payload.y.value, 2.0);
188        assert_eq!(payload.z.value, 3.0);
189    }
190
191    #[test]
192    fn test_length_add_sub() {
193        let a = Distance::new::<meter>(1.0);
194        let b = Distance::new::<meter>(2.0);
195        let c = a + b;
196        assert_eq!(c.value, 3.0);
197        let d = c - a;
198        assert_eq!(d.value, 2.0);
199    }
200
201    #[test]
202    fn test_encoding_length() {
203        let a = Distance::new::<meter>(1.0);
204        let mut encoded = vec![0u8; 1024]; // Reserve a buffer with sufficient capacity
205
206        let length =
207            bincode::encode_into_slice(a, &mut encoded, bincode::config::standard()).unwrap();
208        assert_eq!(length, 4);
209    }
210}
cu_sensor_payloads/pointcloud.rs

cu_sensor_payloads/
pointcloud.rs
