repose_collision/
scene.rs

1//! Fixed-capacity, versioned planning scene storage using deterministic collision primitives.
2//!
3//! A planning scene maintains the set of active collision objects in the robot environment:
4//! - Static/free obstacles in world coordinates.
5//! - Attached objects rigidly affixed to robot frames (or the base) with local geometry.
6//!
7//! All storage is pre-allocated with compile-time const capacities (`MAX_OBSTACLES`, `MAX_ATTACHED`),
8//! strictly allocation-free on the execution path, and versioned via monotonic [`CuSceneRevision`].
9//! Mutations are transactional: failed operations (capacity overflow, missing ID, invalid links,
10//! invalid geometry, or revision exhaustion) leave state unchanged, while successful modifications
11//! advance the scene revision without wrapping.
12
13use crate::env::CuCollisionEnv;
14use crate::shapes::CuCollisionShape;
15use crate::shapes::transform_shape;
16use bincode::Decode;
17use bincode::Encode;
18use cu_spatial_payloads::Transform3D;
19use cu29::prelude::*;
20use serde::Deserialize;
21use serde::Serialize;
22
23/// Default obstacle capacity of [`CuPlanningScene`] and `CuCollisionCheckTask`.
24pub const DEFAULT_MAX_OBSTACLES: usize = 32;
25
26/// Default attached shape capacity of [`CuPlanningScene`] and `CuCollisionCheckTask`.
27pub const DEFAULT_MAX_ATTACHED: usize = 8;
28
29/// Returns `true` if `frame` has strictly finite 4x4 matrix components.
30fn is_finite_transform(frame: &Transform3D<f64>) -> bool {
31    frame.to_matrix().iter().flatten().all(|v| v.is_finite())
32}
33
34/// Strongly typed, stable object identifier for obstacles and attached shapes in a planning scene.
35#[derive(
36    Default,
37    Debug,
38    Clone,
39    Copy,
40    PartialEq,
41    Eq,
42    PartialOrd,
43    Ord,
44    Hash,
45    Encode,
46    Decode,
47    Serialize,
48    Deserialize,
49    Reflect,
50)]
51pub struct CuObjectId(u32);
52
53impl CuObjectId {
54    /// Sentinel identifier representing an unassigned or invalid object ID.
55    pub const NIL: Self = Self(0);
56
57    /// Creates a new object identifier from a raw numeric value.
58    pub const fn new(id: u32) -> Self {
59        Self(id)
60    }
61
62    /// Returns the raw numeric value.
63    pub const fn as_u32(self) -> u32 {
64        self.0
65    }
66
67    /// Returns `true` if this identifier is non-zero, not reserved, and valid.
68    pub const fn is_valid(self) -> bool {
69        self.0 != 0 && self.0 < u32::MAX
70    }
71}
72
73impl core::fmt::Display for CuObjectId {
74    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
75        write!(f, "ObjectId({})", self.0)
76    }
77}
78
79/// Monotonically increasing revision counter tracking changes to the planning scene.
80#[derive(
81    Default,
82    Debug,
83    Clone,
84    Copy,
85    PartialEq,
86    Eq,
87    PartialOrd,
88    Ord,
89    Hash,
90    Encode,
91    Decode,
92    Serialize,
93    Deserialize,
94    Reflect,
95)]
96pub struct CuSceneRevision(u64);
97
98impl CuSceneRevision {
99    /// Initial revision for an empty or newly initialized planning scene.
100    pub const INITIAL: Self = Self(0);
101
102    /// Creates a new revision counter with the given raw value.
103    pub const fn new(revision: u64) -> Self {
104        Self(revision)
105    }
106
107    /// Returns the underlying raw revision number.
108    pub const fn as_u64(self) -> u64 {
109        self.0
110    }
111
112    /// Returns the next revision in sequence, or `None` if the counter has reached maximum.
113    #[must_use]
114    pub const fn checked_next(self) -> Option<Self> {
115        match self.0.checked_add(1) {
116            Some(v) => Some(Self(v)),
117            None => None,
118        }
119    }
120
121    /// Returns the next revision in sequence, saturating at [`u64::MAX`] to avoid wrapping.
122    #[must_use]
123    pub const fn next(self) -> Self {
124        Self(self.0.saturating_add(1))
125    }
126}
127
128impl core::fmt::Display for CuSceneRevision {
129    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
130        write!(f, "Revision({})", self.0)
131    }
132}
133
134/// Errors that can occur during planning scene modifications and queries.
135#[derive(Debug, Clone, Copy, PartialEq, Eq)]
136pub enum CuSceneError {
137    /// Attempted to add an obstacle when obstacle storage is full.
138    ObstacleCapacityExceeded,
139    /// Attempted to add an attached shape when attached storage is full.
140    AttachedCapacityExceeded,
141    /// The specified object identifier was not found in the scene.
142    ObjectNotFound(CuObjectId),
143    /// The specified object identifier is invalid or reserved.
144    InvalidObjectId(CuObjectId),
145    /// An object with the specified identifier already exists.
146    DuplicateObjectId(CuObjectId),
147    /// Specified robot link index is out of bounds for the kinematic chain.
148    InvalidLinkIndex { link_index: usize, max_links: usize },
149    /// Target destination capacity is too small to receive the exported data.
150    OutputCapacityExceeded,
151    /// Scene revision counter has reached maximum value and cannot advance.
152    RevisionExhausted,
153    /// Collision shape contains invalid or non-finite geometry.
154    InvalidGeometry,
155}
156
157impl core::fmt::Display for CuSceneError {
158    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
159        match self {
160            Self::ObstacleCapacityExceeded => {
161                write!(f, "planning scene obstacle capacity exceeded")
162            }
163            Self::AttachedCapacityExceeded => {
164                write!(f, "planning scene attached shape capacity exceeded")
165            }
166            Self::ObjectNotFound(id) => {
167                write!(f, "planning scene object {id} not found")
168            }
169            Self::InvalidObjectId(id) => {
170                write!(f, "planning scene object {id} is invalid")
171            }
172            Self::DuplicateObjectId(id) => {
173                write!(f, "planning scene object {id} already exists")
174            }
175            Self::InvalidLinkIndex {
176                link_index,
177                max_links,
178            } => {
179                write!(
180                    f,
181                    "invalid robot link index {link_index} (chain has {max_links} links)"
182                )
183            }
184            Self::OutputCapacityExceeded => {
185                write!(f, "target output buffer capacity exceeded")
186            }
187            Self::RevisionExhausted => {
188                write!(f, "planning scene revision counter exhausted")
189            }
190            Self::InvalidGeometry => {
191                write!(
192                    f,
193                    "planning scene collision shape contains invalid or non-finite geometry"
194                )
195            }
196        }
197    }
198}
199
200impl CuSceneError {
201    /// Static description, used where no formatting is available.
202    pub const fn as_str(&self) -> &'static str {
203        match self {
204            Self::ObstacleCapacityExceeded => {
205                "repose_collision: planning scene obstacle capacity exceeded"
206            }
207            Self::AttachedCapacityExceeded => {
208                "repose_collision: planning scene attached shape capacity exceeded"
209            }
210            Self::ObjectNotFound(_) => "repose_collision: planning scene object not found",
211            Self::InvalidObjectId(_) => "repose_collision: planning scene object id is invalid",
212            Self::DuplicateObjectId(_) => "repose_collision: planning scene duplicate object id",
213            Self::InvalidLinkIndex { .. } => "repose_collision: invalid robot link index",
214            Self::OutputCapacityExceeded => {
215                "repose_collision: target output buffer capacity exceeded"
216            }
217            Self::RevisionExhausted => "repose_collision: planning scene revision exhausted",
218            Self::InvalidGeometry => {
219                "repose_collision: invalid or non-finite collision shape geometry"
220            }
221        }
222    }
223}
224
225impl From<CuSceneError> for CuError {
226    fn from(err: CuSceneError) -> Self {
227        CuError::from(err.as_str())
228    }
229}
230
231/// An environment obstacle in world coordinates, as stored in a planning scene.
232#[derive(
233    Default, Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect,
234)]
235pub struct CuSceneObstacle {
236    /// Stable typed identifier of this obstacle.
237    pub id: CuObjectId,
238    /// Collision geometry in world coordinates.
239    pub shape: CuCollisionShape,
240}
241
242impl CuSceneObstacle {
243    /// Creates a scene obstacle, validating ID and geometry.
244    pub fn new(id: CuObjectId, shape: CuCollisionShape) -> CuResult<Self> {
245        if !id.is_valid() {
246            return Err(CuSceneError::InvalidObjectId(id).into());
247        }
248        shape.validate()?;
249        Ok(Self { id, shape })
250    }
251}
252
253/// A collision shape rigidly attached to a robot frame or the base.
254#[derive(
255    Default, Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect,
256)]
257pub struct CuAttachedShape {
258    /// Stable typed identifier of this attached object.
259    pub id: CuObjectId,
260    /// Joint-frame index this shape is attached to (`0..N`), or `None` for the base frame.
261    pub frame: Option<usize>,
262    /// Collision geometry expressed in the local coordinates of `frame`.
263    pub shape: CuCollisionShape,
264}
265
266impl CuAttachedShape {
267    /// Creates an attached shape, validating ID and geometry. The frame index is checked against
268    /// the robot when the shape is used.
269    pub fn new(id: CuObjectId, frame: Option<usize>, shape: CuCollisionShape) -> CuResult<Self> {
270        if !id.is_valid() {
271            return Err(CuSceneError::InvalidObjectId(id).into());
272        }
273        shape.validate()?;
274        Ok(Self { id, frame, shape })
275    }
276
277    /// Re-expresses the shape through a frame-relative transform (`frame_from_shape`),
278    /// rejecting non-finite transforms and results.
279    pub fn with_transform(mut self, frame_from_shape: &Transform3D<f64>) -> CuResult<Self> {
280        if !is_finite_transform(frame_from_shape) {
281            return Err(CuSceneError::InvalidGeometry.into());
282        }
283        let local = self.shape;
284        transform_shape(frame_from_shape, &local, &mut self.shape);
285        self.shape.validate()?;
286        Ok(self)
287    }
288}
289
290/// Changes applied by [`CuPlanningScene::update_attached`]; unset parts are kept.
291#[derive(Debug, Clone, Copy, Default, PartialEq)]
292pub struct CuAttachedUpdate {
293    frame: Option<Option<usize>>,
294    shape: Option<CuCollisionShape>,
295}
296
297impl CuAttachedUpdate {
298    /// An update that changes nothing.
299    pub const fn new() -> Self {
300        Self {
301            frame: None,
302            shape: None,
303        }
304    }
305
306    /// Moves the shape to `frame` (`None` = base frame).
307    pub const fn with_frame(mut self, frame: Option<usize>) -> Self {
308        self.frame = Some(frame);
309        self
310    }
311
312    /// Replaces the frame-local geometry.
313    pub const fn with_shape(mut self, shape: CuCollisionShape) -> Self {
314        self.shape = Some(shape);
315        self
316    }
317}
318
319#[derive(Serialize, Deserialize)]
320struct CuPlanningSceneSnapshot<const MAX_OBSTACLES: usize, const MAX_ATTACHED: usize> {
321    revision: CuSceneRevision,
322    next_id: u32,
323    #[serde(with = "crate::serde_arrays::obstacle_array")]
324    obstacles: [CuSceneObstacle; MAX_OBSTACLES],
325    obstacle_count: u32,
326    #[serde(with = "crate::serde_arrays::attached_array")]
327    attached_shapes: [CuAttachedShape; MAX_ATTACHED],
328    attached_count: u32,
329}
330
331/// Fixed-capacity, versioned planning scene storage (`CuMsgPayload`-compliant).
332///
333/// Holds environment obstacles in world frame and attached shapes in robot frames.
334/// Every mutation is transactional and increments [`CuSceneRevision`] on success.
335/// [`collision_env`](Self::collision_env) borrows the scene as a zero-copy [`CuCollisionEnv`].
336#[derive(Debug, Clone, PartialEq, Reflect)]
337#[reflect(no_field_bounds, from_reflect = false)]
338pub struct CuPlanningScene<
339    const MAX_OBSTACLES: usize = DEFAULT_MAX_OBSTACLES,
340    const MAX_ATTACHED: usize = DEFAULT_MAX_ATTACHED,
341> {
342    revision: CuSceneRevision,
343    next_id: u32,
344    obstacle_ids: [CuObjectId; MAX_OBSTACLES],
345    obstacle_shapes: [CuCollisionShape; MAX_OBSTACLES],
346    obstacle_count: u32,
347    attached_shapes: [CuAttachedShape; MAX_ATTACHED],
348    attached_count: u32,
349}
350
351impl<const MAX_OBSTACLES: usize, const MAX_ATTACHED: usize> Default
352    for CuPlanningScene<MAX_OBSTACLES, MAX_ATTACHED>
353{
354    fn default() -> Self {
355        Self::new()
356    }
357}
358
359impl<const MAX_OBSTACLES: usize, const MAX_ATTACHED: usize> Serialize
360    for CuPlanningScene<MAX_OBSTACLES, MAX_ATTACHED>
361{
362    fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
363    where
364        S: serde::Serializer,
365    {
366        if let Err(err) = self.validate_invariants() {
367            return Err(serde::ser::Error::custom(err.as_str()));
368        }
369        let snapshot = CuPlanningSceneSnapshot::<MAX_OBSTACLES, MAX_ATTACHED> {
370            revision: self.revision,
371            next_id: self.next_id,
372            obstacles: core::array::from_fn(|i| CuSceneObstacle {
373                id: self.obstacle_ids[i],
374                shape: self.obstacle_shapes[i],
375            }),
376            obstacle_count: self.obstacle_count,
377            attached_shapes: self.attached_shapes,
378            attached_count: self.attached_count,
379        };
380        snapshot.serialize(serializer)
381    }
382}
383
384impl<'de, const MAX_OBSTACLES: usize, const MAX_ATTACHED: usize> Deserialize<'de>
385    for CuPlanningScene<MAX_OBSTACLES, MAX_ATTACHED>
386{
387    fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
388    where
389        D: serde::Deserializer<'de>,
390    {
391        let raw =
392            CuPlanningSceneSnapshot::<MAX_OBSTACLES, MAX_ATTACHED>::deserialize(deserializer)?;
393        let scene = Self {
394            revision: raw.revision,
395            next_id: raw.next_id,
396            obstacle_ids: raw.obstacles.map(|o| o.id),
397            obstacle_shapes: raw.obstacles.map(|o| o.shape),
398            obstacle_count: raw.obstacle_count,
399            attached_shapes: raw.attached_shapes,
400            attached_count: raw.attached_count,
401        };
402        if let Err(err) = scene.validate_invariants() {
403            return Err(serde::de::Error::custom(err.as_str()));
404        }
405        Ok(scene)
406    }
407}
408
409impl<const MAX_OBSTACLES: usize, const MAX_ATTACHED: usize> Encode
410    for CuPlanningScene<MAX_OBSTACLES, MAX_ATTACHED>
411{
412    fn encode<E: bincode::enc::Encoder>(
413        &self,
414        encoder: &mut E,
415    ) -> Result<(), bincode::error::EncodeError> {
416        if let Err(err) = self.validate_invariants() {
417            return Err(bincode::error::EncodeError::Other(err.as_str()));
418        }
419        Encode::encode(&self.revision, encoder)?;
420        Encode::encode(&self.next_id, encoder)?;
421        Encode::encode(&self.obstacle_count, encoder)?;
422        Encode::encode(&self.obstacle_ids, encoder)?;
423        Encode::encode(&self.obstacle_shapes, encoder)?;
424        Encode::encode(&self.attached_count, encoder)?;
425        Encode::encode(&self.attached_shapes, encoder)
426    }
427}
428
429impl<C, const MAX_OBSTACLES: usize, const MAX_ATTACHED: usize> Decode<C>
430    for CuPlanningScene<MAX_OBSTACLES, MAX_ATTACHED>
431{
432    fn decode<D: bincode::de::Decoder<Context = C>>(
433        decoder: &mut D,
434    ) -> Result<Self, bincode::error::DecodeError> {
435        let scene = Self {
436            revision: Decode::decode(decoder)?,
437            next_id: Decode::decode(decoder)?,
438            obstacle_count: Decode::decode(decoder)?,
439            obstacle_ids: Decode::decode(decoder)?,
440            obstacle_shapes: Decode::decode(decoder)?,
441            attached_count: Decode::decode(decoder)?,
442            attached_shapes: Decode::decode(decoder)?,
443        };
444        if let Err(err) = scene.validate_invariants() {
445            return Err(bincode::error::DecodeError::Other(err.as_str()));
446        }
447        Ok(scene)
448    }
449}
450
451impl<'de, C, const MAX_OBSTACLES: usize, const MAX_ATTACHED: usize> bincode::BorrowDecode<'de, C>
452    for CuPlanningScene<MAX_OBSTACLES, MAX_ATTACHED>
453{
454    fn borrow_decode<D: bincode::de::BorrowDecoder<'de, Context = C>>(
455        decoder: &mut D,
456    ) -> Result<Self, bincode::error::DecodeError> {
457        Decode::decode(decoder)
458    }
459}
460
461impl<const MAX_OBSTACLES: usize, const MAX_ATTACHED: usize>
462    CuPlanningScene<MAX_OBSTACLES, MAX_ATTACHED>
463{
464    /// Maximum number of environment obstacles.
465    pub const OBSTACLE_CAPACITY: usize = MAX_OBSTACLES;
466    /// Maximum number of attached shapes.
467    pub const ATTACHED_CAPACITY: usize = MAX_ATTACHED;
468
469    /// Creates an empty planning scene initialized at [`CuSceneRevision::INITIAL`].
470    pub fn new() -> Self {
471        Self {
472            revision: CuSceneRevision::INITIAL,
473            next_id: 1,
474            obstacle_ids: [CuObjectId::NIL; MAX_OBSTACLES],
475            obstacle_shapes: [CuCollisionShape::default(); MAX_OBSTACLES],
476            obstacle_count: 0,
477            attached_shapes: [CuAttachedShape::default(); MAX_ATTACHED],
478            attached_count: 0,
479        }
480    }
481
482    /// Validates structural, identity, and geometric invariants:
483    /// counts within capacity, non-zero `next_id` greater than every active ID, valid and
484    /// pairwise-unique IDs across obstacles and attached shapes, and finite geometry.
485    pub(crate) fn validate_invariants(&self) -> Result<(), CuSceneError> {
486        let obs_count = self.obstacle_count as usize;
487        if obs_count > MAX_OBSTACLES {
488            return Err(CuSceneError::ObstacleCapacityExceeded);
489        }
490        let att_count = self.attached_count as usize;
491        if att_count > MAX_ATTACHED {
492            return Err(CuSceneError::AttachedCapacityExceeded);
493        }
494        if self.next_id == 0 {
495            return Err(CuSceneError::InvalidObjectId(CuObjectId::NIL));
496        }
497        let obstacle_ids = &self.obstacle_ids[..obs_count];
498        let check = |id: CuObjectId, shape: &CuCollisionShape| {
499            if !id.is_valid() || id.as_u32() >= self.next_id {
500                return Err(CuSceneError::InvalidObjectId(id));
501            }
502            if shape.validate().is_err() {
503                return Err(CuSceneError::InvalidGeometry);
504            }
505            Ok(())
506        };
507        for (i, (&id, shape)) in obstacle_ids
508            .iter()
509            .zip(&self.obstacle_shapes[..obs_count])
510            .enumerate()
511        {
512            check(id, shape)?;
513            if obstacle_ids[i + 1..].contains(&id) {
514                return Err(CuSceneError::DuplicateObjectId(id));
515            }
516        }
517        let attached = self.attached_shapes();
518        for (i, att) in attached.iter().enumerate() {
519            check(att.id, &att.shape)?;
520            if attached[i + 1..].iter().any(|o| o.id == att.id) || obstacle_ids.contains(&att.id) {
521                return Err(CuSceneError::DuplicateObjectId(att.id));
522            }
523        }
524        Ok(())
525    }
526
527    /// Returns the current scene revision.
528    pub const fn revision(&self) -> CuSceneRevision {
529        self.revision
530    }
531
532    /// Returns `true` if the scene has neither obstacles nor attached shapes.
533    pub const fn is_empty(&self) -> bool {
534        self.obstacle_count == 0 && self.attached_count == 0
535    }
536
537    /// Active obstacle shapes in world coordinates; the count is `obstacle_shapes().len()`.
538    pub fn obstacle_shapes(&self) -> &[CuCollisionShape] {
539        &self.obstacle_shapes[..self.obstacle_len()]
540    }
541
542    /// Stable IDs of the active obstacles, parallel to [`obstacle_shapes`](Self::obstacle_shapes).
543    pub fn obstacle_ids(&self) -> &[CuObjectId] {
544        &self.obstacle_ids[..self.obstacle_len()]
545    }
546
547    /// Iterates over the active obstacles with their IDs.
548    pub fn obstacles(&self) -> impl Iterator<Item = CuSceneObstacle> + '_ {
549        self.obstacle_ids()
550            .iter()
551            .zip(self.obstacle_shapes())
552            .map(|(&id, &shape)| CuSceneObstacle { id, shape })
553    }
554
555    /// Active attached shapes; the count is `attached_shapes().len()`.
556    pub fn attached_shapes(&self) -> &[CuAttachedShape] {
557        let count = (self.attached_count as usize).min(MAX_ATTACHED);
558        &self.attached_shapes[..count]
559    }
560
561    /// Looks up an obstacle shape by its typed object ID.
562    pub fn obstacle(&self, id: CuObjectId) -> Option<&CuCollisionShape> {
563        self.find_obstacle_index(id)
564            .map(|idx| &self.obstacle_shapes[idx])
565    }
566
567    /// Looks up an attached shape by its typed object ID.
568    pub fn attached_shape(&self, id: CuObjectId) -> Option<&CuAttachedShape> {
569        self.find_attached_index(id)
570            .map(|idx| &self.attached_shapes[idx])
571    }
572
573    /// Returns `true` if the specified object identifier is present in the scene.
574    pub fn contains(&self, id: CuObjectId) -> bool {
575        self.find_obstacle_index(id).is_some() || self.find_attached_index(id).is_some()
576    }
577
578    /// Zero-copy view of the obstacles (with IDs) and attached shapes for collision checks.
579    /// Add exclusions with [`CuCollisionEnv::with_exclusions`].
580    pub fn collision_env(&self) -> CuCollisionEnv<'_> {
581        CuCollisionEnv::with_obstacle_ids(self.obstacle_shapes(), self.obstacle_ids())
582            .with_attached(self.attached_shapes())
583    }
584
585    fn obstacle_len(&self) -> usize {
586        (self.obstacle_count as usize).min(MAX_OBSTACLES)
587    }
588
589    pub(crate) fn find_obstacle_index(&self, id: CuObjectId) -> Option<usize> {
590        if !id.is_valid() {
591            return None;
592        }
593        self.obstacle_ids().iter().position(|&o| o == id)
594    }
595
596    pub(crate) fn find_attached_index(&self, id: CuObjectId) -> Option<usize> {
597        if !id.is_valid() {
598            return None;
599        }
600        self.attached_shapes().iter().position(|a| a.id == id)
601    }
602
603    fn next_revision(&self) -> CuResult<CuSceneRevision> {
604        Ok(self
605            .revision
606            .checked_next()
607            .ok_or(CuSceneError::RevisionExhausted)?)
608    }
609
610    /// Chooses the ID for a new object and the resulting `next_id`, without mutating.
611    fn plan_id(&self, id: Option<CuObjectId>) -> CuResult<(CuObjectId, u32)> {
612        let overflow = || CuError::from("repose_collision: object id overflow");
613        match id {
614            Some(id) => {
615                if !id.is_valid() {
616                    return Err(CuSceneError::InvalidObjectId(id).into());
617                }
618                if self.contains(id) {
619                    return Err(CuSceneError::DuplicateObjectId(id).into());
620                }
621                let next = if id.as_u32() >= self.next_id {
622                    id.as_u32()
623                        .checked_add(1)
624                        .ok_or(CuSceneError::InvalidObjectId(id))?
625                } else {
626                    self.next_id
627                };
628                Ok((id, next))
629            }
630            None => {
631                let mut candidate = self.next_id;
632                while candidate == 0 || self.contains(CuObjectId::new(candidate)) {
633                    candidate = candidate.checked_add(1).ok_or_else(overflow)?;
634                }
635                let id = CuObjectId::new(candidate);
636                if !id.is_valid() {
637                    return Err(overflow());
638                }
639                Ok((id, candidate.checked_add(1).ok_or_else(overflow)?))
640            }
641        }
642    }
643
644    fn push_obstacle(&mut self, id: CuObjectId, shape: CuCollisionShape) {
645        let idx = self.obstacle_count as usize;
646        self.obstacle_ids[idx] = id;
647        self.obstacle_shapes[idx] = shape;
648        self.obstacle_count += 1;
649    }
650
651    fn take_obstacle(&mut self, idx: usize) -> CuCollisionShape {
652        let count = self.obstacle_len();
653        let removed = self.obstacle_shapes[idx];
654        self.obstacle_ids.copy_within(idx + 1..count, idx);
655        self.obstacle_shapes.copy_within(idx + 1..count, idx);
656        self.obstacle_ids[count - 1] = CuObjectId::NIL;
657        self.obstacle_shapes[count - 1] = CuCollisionShape::default();
658        self.obstacle_count -= 1;
659        removed
660    }
661
662    fn push_attached(&mut self, attached: CuAttachedShape) {
663        self.attached_shapes[self.attached_count as usize] = attached;
664        self.attached_count += 1;
665    }
666
667    fn take_attached(&mut self, idx: usize) -> CuAttachedShape {
668        let count = self.attached_shapes().len();
669        let removed = self.attached_shapes[idx];
670        self.attached_shapes.copy_within(idx + 1..count, idx);
671        self.attached_shapes[count - 1] = CuAttachedShape::default();
672        self.attached_count -= 1;
673        removed
674    }
675
676    /// Adds a world-frame obstacle and returns its ID.
677    ///
678    /// With `id = None` a fresh unique ID is assigned; with `Some(id)` that ID is used and must be
679    /// valid and unused. Fails on capacity, invalid geometry, invalid or duplicate ID, or revision
680    /// exhaustion; on failure, state is unmodified.
681    pub fn add_obstacle(
682        &mut self,
683        shape: CuCollisionShape,
684        id: Option<CuObjectId>,
685    ) -> CuResult<CuObjectId> {
686        if self.obstacle_len() >= MAX_OBSTACLES {
687            return Err(CuSceneError::ObstacleCapacityExceeded.into());
688        }
689        if shape.validate().is_err() {
690            return Err(CuSceneError::InvalidGeometry.into());
691        }
692        let revision = self.next_revision()?;
693        let (id, next_id) = self.plan_id(id)?;
694        self.push_obstacle(id, shape);
695        self.next_id = next_id;
696        self.revision = revision;
697        Ok(id)
698    }
699
700    /// Replaces the geometry of an existing obstacle. On failure, state is unmodified.
701    pub fn update_obstacle(&mut self, id: CuObjectId, shape: CuCollisionShape) -> CuResult<()> {
702        if !id.is_valid() {
703            return Err(CuSceneError::InvalidObjectId(id).into());
704        }
705        if shape.validate().is_err() {
706            return Err(CuSceneError::InvalidGeometry.into());
707        }
708        let idx = self
709            .find_obstacle_index(id)
710            .ok_or(CuSceneError::ObjectNotFound(id))?;
711        self.revision = self.next_revision()?;
712        self.obstacle_shapes[idx] = shape;
713        Ok(())
714    }
715
716    /// Removes an obstacle, preserving the order of the others. On failure, state is unmodified.
717    pub fn remove_obstacle(&mut self, id: CuObjectId) -> CuResult<CuCollisionShape> {
718        if !id.is_valid() {
719            return Err(CuSceneError::InvalidObjectId(id).into());
720        }
721        let idx = self
722            .find_obstacle_index(id)
723            .ok_or(CuSceneError::ObjectNotFound(id))?;
724        self.revision = self.next_revision()?;
725        Ok(self.take_obstacle(idx))
726    }
727
728    /// Adds a shape attached to `frame` (`None` = base frame) and returns its ID.
729    ///
730    /// ID assignment follows [`add_obstacle`](Self::add_obstacle). On failure, state is unmodified.
731    pub fn add_attached(
732        &mut self,
733        frame: Option<usize>,
734        shape: CuCollisionShape,
735        id: Option<CuObjectId>,
736    ) -> CuResult<CuObjectId> {
737        if self.attached_shapes().len() >= MAX_ATTACHED {
738            return Err(CuSceneError::AttachedCapacityExceeded.into());
739        }
740        if shape.validate().is_err() {
741            return Err(CuSceneError::InvalidGeometry.into());
742        }
743        let revision = self.next_revision()?;
744        let (id, next_id) = self.plan_id(id)?;
745        self.push_attached(CuAttachedShape { id, frame, shape });
746        self.next_id = next_id;
747        self.revision = revision;
748        Ok(id)
749    }
750
751    /// Changes the frame and/or geometry of an attached shape. On failure, state is unmodified.
752    pub fn update_attached(&mut self, id: CuObjectId, update: CuAttachedUpdate) -> CuResult<()> {
753        if !id.is_valid() {
754            return Err(CuSceneError::InvalidObjectId(id).into());
755        }
756        if update.shape.is_some_and(|s| s.validate().is_err()) {
757            return Err(CuSceneError::InvalidGeometry.into());
758        }
759        let idx = self
760            .find_attached_index(id)
761            .ok_or(CuSceneError::ObjectNotFound(id))?;
762        self.revision = self.next_revision()?;
763        let attached = &mut self.attached_shapes[idx];
764        if let Some(frame) = update.frame {
765            attached.frame = frame;
766        }
767        if let Some(shape) = update.shape {
768            attached.shape = shape;
769        }
770        Ok(())
771    }
772
773    /// Removes an attached shape, preserving the order of the others. On failure, state is
774    /// unmodified.
775    pub fn remove_attached(&mut self, id: CuObjectId) -> CuResult<CuAttachedShape> {
776        if !id.is_valid() {
777            return Err(CuSceneError::InvalidObjectId(id).into());
778        }
779        let idx = self
780            .find_attached_index(id)
781            .ok_or(CuSceneError::ObjectNotFound(id))?;
782        self.revision = self.next_revision()?;
783        Ok(self.take_attached(idx))
784    }
785
786    /// Turns an existing obstacle into a shape attached to `frame` (e.g. object grasp), keeping
787    /// its [`CuObjectId`]. `local_shape` is the geometry in `frame` coordinates.
788    ///
789    /// Fails if the obstacle is not found, geometry is invalid, attached storage is full, or the
790    /// revision is exhausted. On failure, state is unmodified.
791    pub fn attach(
792        &mut self,
793        id: CuObjectId,
794        frame: Option<usize>,
795        local_shape: CuCollisionShape,
796    ) -> CuResult<()> {
797        if !id.is_valid() {
798            return Err(CuSceneError::InvalidObjectId(id).into());
799        }
800        if local_shape.validate().is_err() {
801            return Err(CuSceneError::InvalidGeometry.into());
802        }
803        if self.attached_shapes().len() >= MAX_ATTACHED {
804            return Err(CuSceneError::AttachedCapacityExceeded.into());
805        }
806        let idx = self
807            .find_obstacle_index(id)
808            .ok_or(CuSceneError::ObjectNotFound(id))?;
809        self.revision = self.next_revision()?;
810        self.take_obstacle(idx);
811        self.push_attached(CuAttachedShape {
812            id,
813            frame,
814            shape: local_shape,
815        });
816        Ok(())
817    }
818
819    /// Turns an attached shape into a world-frame obstacle (e.g. object release), keeping its
820    /// [`CuObjectId`].
821    ///
822    /// Fails if the attached shape is not found, geometry is invalid, obstacle storage is full,
823    /// or the revision is exhausted. On failure, state is unmodified.
824    pub fn detach(&mut self, id: CuObjectId, world_shape: CuCollisionShape) -> CuResult<()> {
825        if !id.is_valid() {
826            return Err(CuSceneError::InvalidObjectId(id).into());
827        }
828        if world_shape.validate().is_err() {
829            return Err(CuSceneError::InvalidGeometry.into());
830        }
831        if self.obstacle_len() >= MAX_OBSTACLES {
832            return Err(CuSceneError::ObstacleCapacityExceeded.into());
833        }
834        let idx = self
835            .find_attached_index(id)
836            .ok_or(CuSceneError::ObjectNotFound(id))?;
837        self.revision = self.next_revision()?;
838        self.take_attached(idx);
839        self.push_obstacle(id, world_shape);
840        Ok(())
841    }
842
843    /// Clears all obstacles and attached shapes, advancing the revision if the scene was not
844    /// empty. On failure (revision exhaustion), state is unmodified.
845    pub fn clear(&mut self) -> CuResult<()> {
846        if !self.is_empty() {
847            self.revision = self.next_revision()?;
848            *self = Self {
849                revision: self.revision,
850                next_id: self.next_id,
851                ..Self::new()
852            };
853        }
854        Ok(())
855    }
856
857    /// Clears all obstacles, advancing the revision if any were present. On failure, state is
858    /// unmodified.
859    pub fn clear_obstacles(&mut self) -> CuResult<()> {
860        if self.obstacle_count > 0 {
861            self.revision = self.next_revision()?;
862            self.obstacle_ids = [CuObjectId::NIL; MAX_OBSTACLES];
863            self.obstacle_shapes = [CuCollisionShape::default(); MAX_OBSTACLES];
864            self.obstacle_count = 0;
865        }
866        Ok(())
867    }
868
869    /// Clears all attached shapes, advancing the revision if any were present. On failure, state
870    /// is unmodified.
871    pub fn clear_attached(&mut self) -> CuResult<()> {
872        if self.attached_count > 0 {
873            self.revision = self.next_revision()?;
874            self.attached_shapes = [CuAttachedShape::default(); MAX_ATTACHED];
875            self.attached_count = 0;
876        }
877        Ok(())
878    }
879
880    /// Executes a transaction block on the planning scene.
881    ///
882    /// If the closure `f` succeeds, modifications are committed and the revision advances once
883    /// if state was modified without advancing it. If `f` returns an error or revision
884    /// exhaustion occurs, the scene rolls back to its pre-transaction state.
885    pub fn transaction<F, R>(&mut self, f: F) -> CuResult<R>
886    where
887        F: FnOnce(&mut Self) -> CuResult<R>,
888    {
889        let backup = self.clone();
890        match f(self) {
891            Ok(result) => {
892                if *self != backup && self.revision == backup.revision {
893                    match self.revision.checked_next() {
894                        Some(rev) => self.revision = rev,
895                        None => {
896                            *self = backup;
897                            return Err(CuSceneError::RevisionExhausted.into());
898                        }
899                    }
900                }
901                Ok(result)
902            }
903            Err(e) => {
904                *self = backup;
905                Err(e)
906            }
907        }
908    }
909
910    /// Writes world-space copies of the active attached shapes for `link_frames` into
911    /// `out_shapes` and returns how many were written.
912    ///
913    /// Validates capacity, frame indices, transforms, and resulting geometry first: on error,
914    /// `out_shapes` is unmodified.
915    pub fn world_attached_shapes<const N: usize, const MAX_OUT: usize>(
916        &self,
917        link_frames: &[Transform3D<f64>; N],
918        out_shapes: &mut [CuCollisionShape; MAX_OUT],
919    ) -> CuResult<usize> {
920        let attached = self.attached_shapes();
921        if attached.len() > MAX_OUT {
922            return Err(CuSceneError::OutputCapacityExceeded.into());
923        }
924        let world = |att: &CuAttachedShape| -> CuResult<CuCollisionShape> {
925            let Some(frame) = att.frame else {
926                return Ok(att.shape);
927            };
928            let Some(transform) = link_frames.get(frame) else {
929                return Err(CuSceneError::InvalidLinkIndex {
930                    link_index: frame,
931                    max_links: N,
932                }
933                .into());
934            };
935            if !is_finite_transform(transform) {
936                return Err(CuSceneError::InvalidGeometry.into());
937            }
938            let mut out = att.shape;
939            transform_shape(transform, &att.shape, &mut out);
940            if out.validate().is_err() {
941                return Err(CuSceneError::InvalidGeometry.into());
942            }
943            Ok(out)
944        };
945        for att in attached {
946            world(att)?;
947        }
948        for (att, out) in attached.iter().zip(out_shapes.iter_mut()) {
949            *out = world(att)?;
950        }
951        Ok(attached.len())
952    }
953}
repose_collision/scene.rs

repose_collision/
scene.rs
