parry3d/query/contact_manifolds/
contact_manifolds_composite_shape_shape.rs

1use alloc::{boxed::Box, vec::Vec};
2
3use crate::bounding_volume::BoundingVolume;
4use crate::math::{Pose, Real};
5use crate::query::contact_manifolds::contact_manifolds_workspace::{
6    TypedWorkspaceData, WorkspaceData,
7};
8use crate::query::contact_manifolds::ContactManifoldsWorkspace;
9use crate::query::query_dispatcher::PersistentQueryDispatcher;
10use crate::query::ContactManifold;
11use crate::shape::{CompositeShape, Shape};
12use crate::utils::hashmap::{Entry, HashMap};
13use crate::utils::PoseOpt;
14
15#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
16#[cfg_attr(
17    feature = "rkyv",
18    derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize)
19)]
20#[derive(Clone)]
21struct SubDetector {
22    manifold_id: usize,
23    timestamp: bool,
24}
25
26#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
27#[derive(Clone, Default)]
28pub struct CompositeShapeShapeContactManifoldsWorkspace {
29    timestamp: bool,
30    sub_detectors: HashMap<u32, SubDetector>,
31}
32
33impl CompositeShapeShapeContactManifoldsWorkspace {
34    pub fn new() -> Self {
35        Self::default()
36    }
37}
38
39fn ensure_workspace_exists(workspace: &mut Option<ContactManifoldsWorkspace>) {
40    if workspace
41        .as_ref()
42        .and_then(|w| {
43            w.0.downcast_ref::<CompositeShapeShapeContactManifoldsWorkspace>()
44        })
45        .is_some()
46    {
47        return;
48    }
49
50    *workspace = Some(ContactManifoldsWorkspace(Box::new(
51        CompositeShapeShapeContactManifoldsWorkspace::new(),
52    )));
53}
54
55/// Computes the contact manifolds between a composite shape and an abstract shape.
56pub fn contact_manifolds_composite_shape_shape<ManifoldData, ContactData>(
57    dispatcher: &dyn PersistentQueryDispatcher<ManifoldData, ContactData>,
58    pos12: &Pose,
59    composite1: &dyn CompositeShape,
60    shape2: &dyn Shape,
61    prediction: Real,
62    manifolds: &mut Vec<ContactManifold<ManifoldData, ContactData>>,
63    workspace: &mut Option<ContactManifoldsWorkspace>,
64    flipped: bool,
65) where
66    ManifoldData: Default + Clone,
67    ContactData: Default + Copy,
68{
69    ensure_workspace_exists(workspace);
70    let workspace: &mut CompositeShapeShapeContactManifoldsWorkspace =
71        workspace.as_mut().unwrap().0.downcast_mut().unwrap();
72    let new_timestamp = !workspace.timestamp;
73    workspace.timestamp = new_timestamp;
74
75    /*
76     * Compute interferences.
77     */
78
79    let pos12 = *pos12;
80    let pos21 = pos12.inverse();
81
82    // Traverse bvh1 first.
83    let ls_aabb2_1 = shape2.compute_aabb(&pos12).loosened(prediction);
84    let mut old_manifolds = core::mem::take(manifolds);
85
86    let mut leaf1_fn = |leaf1: u32| {
87        composite1.map_part_at(leaf1, &mut |part_pos1, part_shape1, normal_constraints1| {
88            let sub_detector = match workspace.sub_detectors.entry(leaf1) {
89                Entry::Occupied(entry) => {
90                    let sub_detector = entry.into_mut();
91                    let manifold = old_manifolds[sub_detector.manifold_id].take();
92                    sub_detector.manifold_id = manifolds.len();
93                    sub_detector.timestamp = new_timestamp;
94                    manifolds.push(manifold);
95                    sub_detector
96                }
97                Entry::Vacant(entry) => {
98                    let sub_detector = SubDetector {
99                        manifold_id: manifolds.len(),
100                        timestamp: new_timestamp,
101                    };
102
103                    let mut manifold = ContactManifold::new();
104
105                    if flipped {
106                        manifold.subshape1 = 0;
107                        manifold.subshape2 = leaf1;
108                        manifold.subshape_pos2 = part_pos1.copied();
109                    } else {
110                        manifold.subshape1 = leaf1;
111                        manifold.subshape2 = 0;
112                        manifold.subshape_pos1 = part_pos1.copied();
113                    };
114
115                    manifolds.push(manifold);
116                    entry.insert(sub_detector)
117                }
118            };
119
120            let manifold = &mut manifolds[sub_detector.manifold_id];
121
122            if flipped {
123                let _ = dispatcher.contact_manifold_convex_convex(
124                    &part_pos1.prepend_to(&pos21),
125                    shape2,
126                    part_shape1,
127                    None,
128                    normal_constraints1,
129                    prediction,
130                    manifold,
131                );
132            } else {
133                let _ = dispatcher.contact_manifold_convex_convex(
134                    &part_pos1.inv_mul(&pos12),
135                    part_shape1,
136                    shape2,
137                    normal_constraints1,
138                    None,
139                    prediction,
140                    manifold,
141                );
142            }
143        });
144    };
145
146    for leaf_id in composite1.bvh().intersect_aabb(&ls_aabb2_1) {
147        leaf1_fn(leaf_id);
148    }
149
150    workspace
151        .sub_detectors
152        .retain(|_, detector| detector.timestamp == new_timestamp)
153}
154
155impl WorkspaceData for CompositeShapeShapeContactManifoldsWorkspace {
156    fn as_typed_workspace_data(&self) -> TypedWorkspaceData<'_> {
157        TypedWorkspaceData::CompositeShapeShapeContactManifoldsWorkspace(self)
158    }
159
160    fn clone_dyn(&self) -> Box<dyn WorkspaceData> {
161        Box::new(self.clone())
162    }
163}
parry3d/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs

parry3d/query/contact_manifolds/
contact_manifolds_composite_shape_shape.rs
