parry2d/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/// Leaves reached by the query from which the sub-shape manifolds are computed in parallel
56/// (`parallel` feature).
57#[cfg(feature = "parallel")]
58const PARALLEL_LEAVES: usize = 256;
59
60/// Computes the contact manifolds between a composite shape and an abstract shape.
61///
62/// The manifolds are in the order of the composite's leaves reached by the query. Under the
63/// `parallel` feature, a query reaching many leaves computes their manifolds across threads
64/// (same manifolds, same order).
65pub fn contact_manifolds_composite_shape_shape<ManifoldData, ContactData>(
66    dispatcher: &dyn PersistentQueryDispatcher<ManifoldData, ContactData>,
67    pos12: &Pose,
68    composite1: &(dyn CompositeShape + Sync),
69    shape2: &dyn Shape,
70    prediction: Real,
71    manifolds: &mut Vec<ContactManifold<ManifoldData, ContactData>>,
72    workspace: &mut Option<ContactManifoldsWorkspace>,
73    flipped: bool,
74) where
75    ManifoldData: Default + Clone + Send + Sync,
76    ContactData: Default + Copy + Send + Sync,
77{
78    ensure_workspace_exists(workspace);
79    let workspace: &mut CompositeShapeShapeContactManifoldsWorkspace =
80        workspace.as_mut().unwrap().0.downcast_mut().unwrap();
81    let new_timestamp = !workspace.timestamp;
82    workspace.timestamp = new_timestamp;
83
84    /*
85     * Compute interferences.
86     */
87
88    let pos12 = *pos12;
89    let pos21 = pos12.inverse();
90    let deformable = composite1.is_deformable();
91
92    // Traverse bvh1 first.
93    let ls_aabb2_1 = shape2.compute_aabb(&pos12).loosened(prediction);
94    let mut old_manifolds = core::mem::take(manifolds);
95
96    // The manifold of a leaf: the one kept from the last query, or a fresh one (its sub-shape
97    // ids and pose set by the first computation), pushed in leaf order.
98    let mut bookkeep =
99        |leaf1: u32, manifolds: &mut Vec<ContactManifold<ManifoldData, ContactData>>| {
100            match workspace.sub_detectors.entry(leaf1) {
101                Entry::Occupied(entry) => {
102                    let sub_detector = entry.into_mut();
103                    let mut manifold = old_manifolds[sub_detector.manifold_id].take();
104                    sub_detector.manifold_id = manifolds.len();
105                    sub_detector.timestamp = new_timestamp;
106                    if deformable {
107                        manifold.mark_shapes_deformed();
108                    }
109                    manifolds.push(manifold);
110                    false
111                }
112                Entry::Vacant(entry) => {
113                    let _ = entry.insert(SubDetector {
114                        manifold_id: manifolds.len(),
115                        timestamp: new_timestamp,
116                    });
117                    let mut manifold = ContactManifold::new();
118                    if flipped {
119                        manifold.subshape1 = 0;
120                        manifold.subshape2 = leaf1;
121                    } else {
122                        manifold.subshape1 = leaf1;
123                        manifold.subshape2 = 0;
124                    }
125                    if deformable {
126                        manifold.mark_shapes_deformed();
127                    }
128                    manifolds.push(manifold);
129                    true
130                }
131            }
132        };
133    // The manifold's sub-shape pose (a fresh manifold) and contacts.
134    let compute =
135        |leaf1: u32, fresh: bool, manifold: &mut ContactManifold<ManifoldData, ContactData>| {
136            composite1.map_part_at(leaf1, &mut |part_pos1, part_shape1, normal_constraints1| {
137                if fresh {
138                    if flipped {
139                        manifold.set_subshape_pos2(part_pos1.copied());
140                    } else {
141                        manifold.set_subshape_pos1(part_pos1.copied());
142                    }
143                }
144                if flipped {
145                    let _ = dispatcher.contact_manifold_convex_convex(
146                        &part_pos1.prepend_to(&pos21),
147                        shape2,
148                        part_shape1,
149                        None,
150                        normal_constraints1,
151                        prediction,
152                        manifold,
153                    );
154                } else {
155                    let _ = dispatcher.contact_manifold_convex_convex(
156                        &part_pos1.inv_mul(&pos12),
157                        part_shape1,
158                        shape2,
159                        normal_constraints1,
160                        None,
161                        prediction,
162                        manifold,
163                    );
164                }
165            });
166        };
167
168    #[cfg(feature = "parallel")]
169    {
170        let leaves: Vec<u32> = composite1.bvh().intersect_aabb(&ls_aabb2_1).collect();
171        if leaves.len() >= PARALLEL_LEAVES {
172            use rayon::prelude::*;
173            let fresh: Vec<bool> = leaves
174                .iter()
175                .map(|&leaf1| bookkeep(leaf1, manifolds))
176                .collect();
177            manifolds
178                .par_iter_mut()
179                .zip(leaves.par_iter())
180                .zip(fresh.par_iter())
181                .for_each(|((manifold, &leaf1), &fresh)| compute(leaf1, fresh, manifold));
182        } else {
183            for leaf1 in leaves {
184                let fresh = bookkeep(leaf1, manifolds);
185                compute(leaf1, fresh, manifolds.last_mut().unwrap());
186            }
187        }
188    }
189    #[cfg(not(feature = "parallel"))]
190    for leaf1 in composite1.bvh().intersect_aabb(&ls_aabb2_1) {
191        let fresh = bookkeep(leaf1, manifolds);
192        compute(leaf1, fresh, manifolds.last_mut().unwrap());
193    }
194
195    workspace
196        .sub_detectors
197        .retain(|_, detector| detector.timestamp == new_timestamp)
198}
199
200impl WorkspaceData for CompositeShapeShapeContactManifoldsWorkspace {
201    fn as_typed_workspace_data(&self) -> TypedWorkspaceData<'_> {
202        TypedWorkspaceData::CompositeShapeShapeContactManifoldsWorkspace(self)
203    }
204
205    fn clone_dyn(&self) -> Box<dyn WorkspaceData> {
206        Box::new(self.clone())
207    }
208}
parry2d/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs

parry2d/query/contact_manifolds/
contact_manifolds_composite_shape_shape.rs
