loam_physics/
geometry.rs

1use std::collections::hash_map::{DefaultHasher, Entry as MapEntry};
2use std::collections::HashMap;
3use std::hash::Hasher;
4
5use crate::collider::{Collider, ColliderKind};
6use crate::edit::EditError;
7
8#[cfg_attr(feature = "persist", derive(serde::Serialize, serde::Deserialize))]
9#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash, PartialOrd, Ord)]
10pub struct GeometryId(u32);
11
12impl GeometryId {
13    pub fn index(self) -> u32 {
14        self.0
15    }
16}
17
18#[cfg_attr(feature = "persist", derive(serde::Serialize, serde::Deserialize))]
19#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
20pub struct GeometryRef {
21    id: GeometryId,
22    version: u32,
23}
24
25impl GeometryRef {
26    pub fn id(self) -> GeometryId {
27        self.id
28    }
29
30    pub fn version(self) -> u32 {
31        self.version
32    }
33}
34
35#[cfg_attr(feature = "persist", derive(serde::Serialize, serde::Deserialize))]
36#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
37pub struct ColliderRef {
38    kind: ColliderKind,
39    geometry: GeometryRef,
40}
41
42impl ColliderRef {
43    pub fn kind(self) -> ColliderKind {
44        self.kind
45    }
46
47    pub fn geometry(self) -> GeometryRef {
48        self.geometry
49    }
50}
51
52#[cfg_attr(feature = "persist", derive(serde::Serialize, serde::Deserialize))]
53#[derive(Clone)]
54struct Entry {
55    version: u32,
56    uses: u32,
57    hash: u64,
58    shape: Option<Collider>,
59}
60
61const RELEASED_BUFFERS: usize = 2;
62
63#[cfg_attr(feature = "persist", derive(serde::Serialize, serde::Deserialize))]
64#[derive(Clone, Default)]
65pub struct GeometryStore {
66    entries: Vec<Entry>,
67    free: Vec<u32>,
68    released: Vec<Collider>,
69    index: HashMap<u64, Vec<GeometryId>>,
70    buckets: Vec<Vec<GeometryId>>,
71}
72
73impl GeometryStore {
74    /// A shape equal to one already held shares its entry, and the duplicate joins the released pool.
75    pub fn prepare(&mut self, shape: Collider) -> ColliderRef {
76        let kind = shape.kind();
77        let hash = shape_hash(&shape);
78        let entries = &mut self.entries;
79        let shared = self.index.get(&hash).and_then(|bucket| {
80            bucket.iter().copied().find(|id| {
81                entries[id.0 as usize]
82                    .shape
83                    .as_ref()
84                    .is_some_and(|prepared| same_shape(prepared, &shape))
85            })
86        });
87        if let Some(id) = shared {
88            let entry = &mut self.entries[id.0 as usize];
89            entry.uses += 1;
90            let version = entry.version;
91            self.stash(shape);
92            return ColliderRef {
93                kind,
94                geometry: GeometryRef { id, version },
95            };
96        }
97
98        let slot = match self.free.pop() {
99            Some(slot) => {
100                let entry = &mut self.entries[slot as usize];
101                entry.uses = 1;
102                entry.hash = hash;
103                entry.shape = Some(shape);
104                slot
105            }
106            None => {
107                self.entries.push(Entry {
108                    version: 0,
109                    uses: 1,
110                    hash,
111                    shape: Some(shape),
112                });
113                (self.entries.len() - 1) as u32
114            }
115        };
116        let id = GeometryId(slot);
117        let spare = self.buckets.pop().unwrap_or_default();
118        match self.index.entry(hash) {
119            MapEntry::Occupied(mut held) => {
120                held.get_mut().push(id);
121                self.buckets.push(spare);
122            }
123            MapEntry::Vacant(empty) => {
124                let mut bucket = spare;
125                bucket.push(id);
126                empty.insert(bucket);
127            }
128        }
129        ColliderRef {
130            kind,
131            geometry: GeometryRef {
132                id,
133                version: self.entries[slot as usize].version,
134            },
135        }
136    }
137
138    pub fn release(&mut self, collider: ColliderRef) {
139        let slot = collider.geometry.id.0 as usize;
140        let Some(entry) = self.entries.get_mut(slot) else {
141            return;
142        };
143        if entry.version != collider.geometry.version || entry.uses == 0 {
144            return;
145        }
146        entry.uses -= 1;
147        if entry.uses > 0 {
148            return;
149        }
150        let freed = entry.shape.take();
151        let hash = entry.hash;
152        if let Some(bucket) = self.index.get_mut(&hash) {
153            bucket.retain(|held| *held != collider.geometry.id);
154            if bucket.is_empty() {
155                if let Some(spare) = self.index.remove(&hash) {
156                    self.buckets.push(spare);
157                }
158            }
159        }
160        let entry = &mut self.entries[slot];
161        if let Some(next) = entry.version.checked_add(1) {
162            entry.version = next;
163            self.free.push(slot as u32);
164        }
165        if let Some(freed) = freed {
166            self.stash(freed);
167        }
168    }
169
170    pub(crate) fn stash(&mut self, shape: Collider) {
171        if self.released.len() == RELEASED_BUFFERS {
172            self.released.remove(0);
173        }
174        self.released.push(shape);
175    }
176
177    pub fn take_released(&mut self) -> Option<Collider> {
178        self.released.pop()
179    }
180
181    pub fn get(&self, collider: ColliderRef) -> Option<&Collider> {
182        let entry = self.entries.get(collider.geometry.id.0 as usize)?;
183        if entry.version != collider.geometry.version {
184            return None;
185        }
186        entry.shape.as_ref()
187    }
188
189    pub fn prepared(&self) -> usize {
190        self.entries
191            .iter()
192            .filter(|entry| entry.shape.is_some())
193            .count()
194    }
195
196    pub(crate) fn validate(
197        &self,
198        colliders: impl Iterator<Item = ColliderRef>,
199    ) -> Result<(), EditError> {
200        if self.released.len() > RELEASED_BUFFERS
201            || self.buckets.iter().any(|bucket| !bucket.is_empty())
202        {
203            return Err(EditError::InvalidGeometry);
204        }
205        let mut free = vec![false; self.entries.len()];
206        for &slot in &self.free {
207            let Some(entry) = self.entries.get(slot as usize) else {
208                return Err(EditError::InvalidGeometry);
209            };
210            if free[slot as usize] || entry.shape.is_some() || entry.uses != 0 {
211                return Err(EditError::InvalidGeometry);
212            }
213            free[slot as usize] = true;
214        }
215        let mut indexed = vec![false; self.entries.len()];
216        for (&hash, bucket) in &self.index {
217            if bucket.is_empty() {
218                return Err(EditError::InvalidGeometry);
219            }
220            for &id in bucket {
221                let Some(entry) = self.entries.get(id.0 as usize) else {
222                    return Err(EditError::InvalidGeometry);
223                };
224                if indexed[id.0 as usize]
225                    || entry.hash != hash
226                    || entry.shape.is_none()
227                    || entry
228                        .shape
229                        .as_ref()
230                        .is_some_and(|shape| shape_hash(shape) != hash)
231                {
232                    return Err(EditError::InvalidGeometry);
233                }
234                indexed[id.0 as usize] = true;
235            }
236        }
237        let mut uses = vec![0_u32; self.entries.len()];
238        for collider in colliders {
239            let Some(entry) = self.entries.get(collider.geometry.id.0 as usize) else {
240                return Err(EditError::InvalidGeometry);
241            };
242            if entry.version != collider.geometry.version
243                || entry.shape.as_ref().map(Collider::kind) != Some(collider.kind)
244            {
245                return Err(EditError::InvalidGeometry);
246            }
247            let Some(count) = uses[collider.geometry.id.0 as usize].checked_add(1) else {
248                return Err(EditError::InvalidGeometry);
249            };
250            uses[collider.geometry.id.0 as usize] = count;
251        }
252        for (slot, entry) in self.entries.iter().enumerate() {
253            if entry.shape.is_some() {
254                if free[slot] || !indexed[slot] || entry.uses == 0 || uses[slot] != entry.uses {
255                    return Err(EditError::InvalidGeometry);
256                }
257            } else if entry.uses != 0
258                || indexed[slot]
259                || uses[slot] != 0
260                || (entry.version != u32::MAX && !free[slot])
261            {
262                return Err(EditError::InvalidGeometry);
263            }
264        }
265        Ok(())
266    }
267}
268
269fn shape_hash(shape: &Collider) -> u64 {
270    let mut hasher = DefaultHasher::new();
271    hasher.write_u8(kind_tag(shape));
272    match shape {
273        Collider::Sphere { center, radius } => {
274            hash_f32s(&mut hasher, center.as_ref());
275            hasher.write_u32(radius.to_bits());
276        }
277        Collider::HyperSphere4D { center, radius } => {
278            hash_f32s(&mut hasher, center.as_ref());
279            hasher.write_u32(radius.to_bits());
280        }
281        Collider::HalfSpace { normal, offset } => {
282            hash_f32s(&mut hasher, normal.as_ref());
283            hasher.write_u32(offset.to_bits());
284        }
285        Collider::HalfSpace4D { normal, offset } => {
286            hash_f32s(&mut hasher, normal.as_ref());
287            hasher.write_u32(offset.to_bits());
288        }
289        Collider::Box3 { half_extents } => hash_f32s(&mut hasher, half_extents.as_ref()),
290        Collider::Polygon2D { vertices } => {
291            for v in vertices {
292                hash_f32s(&mut hasher, v.as_ref());
293            }
294        }
295        Collider::ConvexPolytope3D { vertices } => {
296            for v in vertices {
297                hash_f32s(&mut hasher, v.as_ref());
298            }
299        }
300        Collider::ConvexPolytope4D { vertices } => {
301            for v in vertices {
302                hash_f32s(&mut hasher, v.as_ref());
303            }
304        }
305    }
306    hasher.finish()
307}
308
309fn hash_f32s(hasher: &mut DefaultHasher, values: &[f32]) {
310    for value in values {
311        hasher.write_u32(value.to_bits());
312    }
313}
314
315fn kind_tag(shape: &Collider) -> u8 {
316    match shape.kind() {
317        ColliderKind::Sphere => 0,
318        ColliderKind::HalfSpace => 1,
319        ColliderKind::HalfSpace4D => 2,
320        ColliderKind::Box3 => 3,
321        ColliderKind::Polygon2D => 4,
322        ColliderKind::ConvexPolytope3D => 5,
323        ColliderKind::ConvexPolytope4D => 6,
324        ColliderKind::HyperSphere4D => 7,
325    }
326}
327
328fn same_shape(a: &Collider, b: &Collider) -> bool {
329    match (a, b) {
330        (
331            Collider::Sphere {
332                center: ca,
333                radius: ra,
334            },
335            Collider::Sphere {
336                center: cb,
337                radius: rb,
338            },
339        ) => ca == cb && ra == rb,
340        (
341            Collider::HyperSphere4D {
342                center: ca,
343                radius: ra,
344            },
345            Collider::HyperSphere4D {
346                center: cb,
347                radius: rb,
348            },
349        ) => ca == cb && ra == rb,
350        (
351            Collider::HalfSpace {
352                normal: na,
353                offset: oa,
354            },
355            Collider::HalfSpace {
356                normal: nb,
357                offset: ob,
358            },
359        ) => na == nb && oa == ob,
360        (
361            Collider::HalfSpace4D {
362                normal: na,
363                offset: oa,
364            },
365            Collider::HalfSpace4D {
366                normal: nb,
367                offset: ob,
368            },
369        ) => na == nb && oa == ob,
370        (Collider::Box3 { half_extents: a }, Collider::Box3 { half_extents: b }) => a == b,
371        (Collider::Polygon2D { vertices: a }, Collider::Polygon2D { vertices: b }) => a == b,
372        (
373            Collider::ConvexPolytope3D { vertices: a },
374            Collider::ConvexPolytope3D { vertices: b },
375        ) => a == b,
376        (
377            Collider::ConvexPolytope4D { vertices: a },
378            Collider::ConvexPolytope4D { vertices: b },
379        ) => a == b,
380        _ => false,
381    }
382}
383
384#[cfg(test)]
385mod tests {
386    use super::*;
387
388    #[test]
389    fn decoded_geometry_with_a_wrong_reference_count_is_rejected() {
390        let mut store = GeometryStore::default();
391        let collider = store.prepare(Collider::sphere_at_origin(1.0));
392        assert!(store.validate(std::iter::once(collider)).is_ok());
393
394        store.entries[0].uses = 2;
395        assert_eq!(
396            store.validate(std::iter::once(collider)),
397            Err(EditError::InvalidGeometry)
398        );
399    }
400
401    #[test]
402    fn a_maximum_version_entry_is_reused_once_then_retired() {
403        let mut store = GeometryStore::default();
404        let first = store.prepare(Collider::sphere_at_origin(1.0));
405        store.entries[0].version = u32::MAX - 1;
406        let first = ColliderRef {
407            kind: first.kind,
408            geometry: GeometryRef {
409                id: first.geometry.id,
410                version: u32::MAX - 1,
411            },
412        };
413
414        store.release(first);
415        assert!(store.validate(std::iter::empty()).is_ok());
416        let last = store.prepare(Collider::sphere_at_origin(2.0));
417        assert_eq!(last.geometry.version, u32::MAX);
418        store.release(last);
419        assert!(store.validate(std::iter::empty()).is_ok());
420
421        let fresh = store.prepare(Collider::sphere_at_origin(3.0));
422        assert_ne!(fresh.geometry.id, last.geometry.id);
423    }
424}
loam_physics/geometry.rs

loam_physics/
geometry.rs
