copc_core/
hierarchy.rs

1use crate::{Error, Result};
2
3pub const HIERARCHY_ENTRY_BYTES: usize = 32;
4
5/// COPC octree voxel key.
6#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash, PartialOrd, Ord)]
7pub struct VoxelKey {
8    pub level: i32,
9    pub x: i32,
10    pub y: i32,
11    pub z: i32,
12}
13
14impl VoxelKey {
15    pub const fn root() -> Self {
16        Self {
17            level: 0,
18            x: 0,
19            y: 0,
20            z: 0,
21        }
22    }
23
24    pub fn child(self, octant: u8) -> Self {
25        Self {
26            level: self.level + 1,
27            x: (self.x << 1) | i32::from(octant & 1),
28            y: (self.y << 1) | i32::from((octant >> 1) & 1),
29            z: (self.z << 1) | i32::from((octant >> 2) & 1),
30        }
31    }
32}
33
34/// One 32-byte COPC hierarchy entry.
35#[derive(Clone, Copy, Debug, PartialEq, Eq)]
36pub struct Entry {
37    pub key: VoxelKey,
38    pub offset: u64,
39    pub byte_size: i32,
40    pub point_count: i32,
41}
42
43/// Availability represented by a COPC hierarchy entry.
44#[derive(Clone, Copy, Debug, PartialEq, Eq)]
45pub enum EntryAvailability {
46    Empty,
47    PointData { point_count: u32 },
48    ChildPage,
49}
50
51impl Entry {
52    pub fn availability(self) -> Result<EntryAvailability> {
53        match self.point_count {
54            -1 => Ok(EntryAvailability::ChildPage),
55            0 => Ok(EntryAvailability::Empty),
56            count if count > 0 => {
57                let point_count = u32::try_from(count).map_err(|_| {
58                    Error::InvalidData(format!(
59                        "hierarchy entry {:?} point count {} is out of range",
60                        self.key, self.point_count
61                    ))
62                })?;
63                Ok(EntryAvailability::PointData { point_count })
64            }
65            _ => Err(Error::InvalidData(format!(
66                "hierarchy entry {:?} has invalid point count {}",
67                self.key, self.point_count
68            ))),
69        }
70    }
71
72    pub fn has_point_data(self) -> bool {
73        self.point_count > 0
74    }
75
76    pub fn is_empty(self) -> bool {
77        self.point_count == 0
78    }
79
80    pub fn is_child_page(self) -> bool {
81        self.point_count == -1
82    }
83
84    pub fn write_le(self, dst: &mut [u8]) -> Result<()> {
85        if dst.len() != HIERARCHY_ENTRY_BYTES {
86            return Err(Error::InvalidInput(format!(
87                "hierarchy entry destination is {} bytes, expected {}",
88                dst.len(),
89                HIERARCHY_ENTRY_BYTES
90            )));
91        }
92        dst[0..4].copy_from_slice(&self.key.level.to_le_bytes());
93        dst[4..8].copy_from_slice(&self.key.x.to_le_bytes());
94        dst[8..12].copy_from_slice(&self.key.y.to_le_bytes());
95        dst[12..16].copy_from_slice(&self.key.z.to_le_bytes());
96        dst[16..24].copy_from_slice(&self.offset.to_le_bytes());
97        dst[24..28].copy_from_slice(&self.byte_size.to_le_bytes());
98        dst[28..32].copy_from_slice(&self.point_count.to_le_bytes());
99        Ok(())
100    }
101
102    pub fn from_le(src: &[u8]) -> Result<Self> {
103        if src.len() != HIERARCHY_ENTRY_BYTES {
104            return Err(Error::InvalidData(format!(
105                "hierarchy entry is {} bytes, expected {}",
106                src.len(),
107                HIERARCHY_ENTRY_BYTES
108            )));
109        }
110        Ok(Self {
111            key: VoxelKey {
112                level: i32::from_le_bytes(src[0..4].try_into().expect("level width")),
113                x: i32::from_le_bytes(src[4..8].try_into().expect("x width")),
114                y: i32::from_le_bytes(src[8..12].try_into().expect("y width")),
115                z: i32::from_le_bytes(src[12..16].try_into().expect("z width")),
116            },
117            offset: u64::from_le_bytes(src[16..24].try_into().expect("offset width")),
118            byte_size: i32::from_le_bytes(src[24..28].try_into().expect("byte_size width")),
119            point_count: i32::from_le_bytes(src[28..32].try_into().expect("point_count width")),
120        })
121    }
122}
123
124/// A COPC hierarchy page.
125#[derive(Clone, Debug, Default, PartialEq, Eq)]
126pub struct HierarchyPage {
127    entries: Vec<Entry>,
128}
129
130impl HierarchyPage {
131    pub fn new(entries: Vec<Entry>) -> Self {
132        Self { entries }
133    }
134
135    pub fn entries(&self) -> &[Entry] {
136        &self.entries
137    }
138
139    pub fn into_entries(self) -> Vec<Entry> {
140        self.entries
141    }
142
143    pub fn from_le_bytes(bytes: &[u8]) -> Result<Self> {
144        if bytes.len() % HIERARCHY_ENTRY_BYTES != 0 {
145            return Err(Error::InvalidData(format!(
146                "hierarchy page is {} bytes, not a multiple of {}",
147                bytes.len(),
148                HIERARCHY_ENTRY_BYTES
149            )));
150        }
151        let entries = bytes
152            .chunks_exact(HIERARCHY_ENTRY_BYTES)
153            .map(Entry::from_le)
154            .collect::<Result<Vec<_>>>()?;
155        Ok(Self { entries })
156    }
157
158    pub fn write_le_bytes(&self) -> Result<Vec<u8>> {
159        let mut out = vec![0u8; self.entries.len() * HIERARCHY_ENTRY_BYTES];
160        for (entry, chunk) in self
161            .entries
162            .iter()
163            .copied()
164            .zip(out.chunks_exact_mut(HIERARCHY_ENTRY_BYTES))
165        {
166            entry.write_le(chunk)?;
167        }
168        Ok(out)
169    }
170}
171
172#[cfg(test)]
173mod tests {
174    use super::*;
175
176    #[test]
177    fn hierarchy_entry_round_trips() {
178        let entry = Entry {
179            key: VoxelKey {
180                level: 3,
181                x: 4,
182                y: 5,
183                z: 6,
184            },
185            offset: 123_456,
186            byte_size: 789,
187            point_count: 42,
188        };
189        let mut bytes = [0u8; HIERARCHY_ENTRY_BYTES];
190        entry.write_le(&mut bytes).unwrap();
191        assert_eq!(Entry::from_le(&bytes).unwrap(), entry);
192    }
193
194    #[test]
195    fn voxel_child_maps_octant_bits() {
196        let child = VoxelKey::root().child(0b101);
197        assert_eq!(
198            child,
199            VoxelKey {
200                level: 1,
201                x: 1,
202                y: 0,
203                z: 1,
204            }
205        );
206    }
207
208    #[test]
209    fn entry_availability_classifies_point_count() {
210        let key = VoxelKey::root();
211        assert_eq!(
212            Entry {
213                key,
214                offset: 0,
215                byte_size: 0,
216                point_count: 0
217            }
218            .availability()
219            .unwrap(),
220            EntryAvailability::Empty
221        );
222        assert_eq!(
223            Entry {
224                key,
225                offset: 64,
226                byte_size: 128,
227                point_count: 42
228            }
229            .availability()
230            .unwrap(),
231            EntryAvailability::PointData { point_count: 42 }
232        );
233        assert_eq!(
234            Entry {
235                key,
236                offset: 64,
237                byte_size: 128,
238                point_count: -1
239            }
240            .availability()
241            .unwrap(),
242            EntryAvailability::ChildPage
243        );
244    }
245}
copc_core/hierarchy.rs

copc_core/
hierarchy.rs
