parry3d/query/contact_manifolds/
contact_manifolds_composite_shape_composite_shape.rs

1use alloc::boxed::Box;
2use alloc::vec::Vec;
3
4use crate::bounding_volume::BoundingVolume;
5use crate::math::{Pose, Real};
6use crate::query::contact_manifolds::contact_manifolds_workspace::{
7    TypedWorkspaceData, WorkspaceData,
8};
9use crate::query::contact_manifolds::ContactManifoldsWorkspace;
10use crate::query::query_dispatcher::PersistentQueryDispatcher;
11use crate::query::ContactManifold;
12use crate::shape::CompositeShape;
13use crate::utils::hashmap::{Entry, HashMap};
14use crate::utils::PoseOpt;
15
16#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
17#[cfg_attr(
18    feature = "rkyv",
19    derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize)
20)]
21#[derive(Clone)]
22struct SubDetector {
23    manifold_id: usize,
24    timestamp: bool,
25}
26
27#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
28#[derive(Clone, Default)]
29pub struct CompositeShapeCompositeShapeContactManifoldsWorkspace {
30    timestamp: bool,
31    sub_detectors: HashMap<(u32, u32), SubDetector>,
32}
33
34impl CompositeShapeCompositeShapeContactManifoldsWorkspace {
35    pub fn new() -> Self {
36        Self::default()
37    }
38}
39
40fn ensure_workspace_exists(workspace: &mut Option<ContactManifoldsWorkspace>) {
41    if workspace
42        .as_ref()
43        .and_then(|w| {
44            w.0.downcast_ref::<CompositeShapeCompositeShapeContactManifoldsWorkspace>()
45        })
46        .is_some()
47    {
48        return;
49    }
50
51    *workspace = Some(ContactManifoldsWorkspace(Box::new(
52        CompositeShapeCompositeShapeContactManifoldsWorkspace::new(),
53    )));
54}
55
56/// Computes the contact manifolds between two composite shapes.
57pub fn contact_manifolds_composite_shape_composite_shape<'a, ManifoldData, ContactData>(
58    dispatcher: &dyn PersistentQueryDispatcher<ManifoldData, ContactData>,
59    pos12: &Pose,
60    mut composite1: &'a dyn CompositeShape,
61    mut composite2: &'a dyn CompositeShape,
62    prediction: Real,
63    manifolds: &mut Vec<ContactManifold<ManifoldData, ContactData>>,
64    workspace: &mut Option<ContactManifoldsWorkspace>,
65) where
66    ManifoldData: Default + Clone,
67    ContactData: Default + Copy,
68{
69    ensure_workspace_exists(workspace);
70    let workspace: &mut CompositeShapeCompositeShapeContactManifoldsWorkspace =
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 mut bvh1 = composite1.bvh();
80    let mut bvh2 = composite2.bvh();
81
82    let mut pos12 = *pos12;
83    let mut pos21 = pos12.inverse();
84
85    let mut ls_aabb1 = bvh1.root_aabb();
86    let mut ls_aabb2 = bvh2.root_aabb();
87    let flipped =
88        ls_aabb1.half_extents().length_squared() < ls_aabb2.half_extents().length_squared();
89
90    if flipped {
91        core::mem::swap(&mut composite1, &mut composite2);
92        core::mem::swap(&mut bvh1, &mut bvh2);
93        core::mem::swap(&mut pos12, &mut pos21);
94        core::mem::swap(&mut ls_aabb1, &mut ls_aabb2);
95    }
96
97    // Traverse bvh1 first.
98    let ls_aabb2_1 = ls_aabb2.transform_by(&pos12).loosened(prediction);
99    let mut old_manifolds = core::mem::take(manifolds);
100    let deformable = composite1.is_deformable() || composite2.is_deformable();
101
102    let mut leaf_fn1 = |leaf1: u32| {
103        composite1.map_part_at(leaf1, &mut |part_pos1, part_shape1, normal_constraints1| {
104            let pos211 = part_pos1.prepend_to(&pos21); // == pos21 * part_pos1
105            let ls_part_aabb1_2 = part_shape1.compute_aabb(&pos211).loosened(prediction);
106            let mut leaf_fn2 = |leaf2: u32| {
107                composite2.map_part_at(
108                    leaf2,
109                    &mut |part_pos2, part_shape2, normal_constraints2| {
110                        let pos2211 = part_pos2.inv_mul(&pos211);
111                        let entry_key = if flipped {
112                            (leaf2, leaf1)
113                        } else {
114                            (leaf1, leaf2)
115                        };
116
117                        let sub_detector = match workspace.sub_detectors.entry(entry_key) {
118                            Entry::Occupied(entry) => {
119                                let sub_detector = entry.into_mut();
120                                let manifold = old_manifolds[sub_detector.manifold_id].take();
121                                sub_detector.manifold_id = manifolds.len();
122                                sub_detector.timestamp = new_timestamp;
123                                manifolds.push(manifold);
124                                sub_detector
125                            }
126                            Entry::Vacant(entry) => {
127                                let sub_detector = SubDetector {
128                                    manifold_id: manifolds.len(),
129                                    timestamp: new_timestamp,
130                                };
131
132                                let mut manifold = ContactManifold::new();
133
134                                if flipped {
135                                    manifold.subshape1 = leaf2;
136                                    manifold.subshape2 = leaf1;
137                                    manifold.set_subshape_pos1(part_pos2.copied());
138                                    manifold.set_subshape_pos2(part_pos1.copied());
139                                } else {
140                                    manifold.subshape1 = leaf1;
141                                    manifold.subshape2 = leaf2;
142                                    manifold.set_subshape_pos1(part_pos1.copied());
143                                    manifold.set_subshape_pos2(part_pos2.copied());
144                                };
145
146                                manifolds.push(manifold);
147                                entry.insert(sub_detector)
148                            }
149                        };
150
151                        let manifold = &mut manifolds[sub_detector.manifold_id];
152                        if deformable {
153                            manifold.mark_shapes_deformed();
154                        }
155
156                        if flipped {
157                            let _ = dispatcher.contact_manifold_convex_convex(
158                                &pos2211,
159                                part_shape2,
160                                part_shape1,
161                                normal_constraints2,
162                                normal_constraints1,
163                                prediction,
164                                manifold,
165                            );
166                        } else {
167                            let _ = dispatcher.contact_manifold_convex_convex(
168                                &pos2211.inverse(),
169                                part_shape1,
170                                part_shape2,
171                                normal_constraints1,
172                                normal_constraints2,
173                                prediction,
174                                manifold,
175                            );
176                        }
177                    },
178                );
179            };
180
181            for leaf_id in composite2.bvh().intersect_aabb(&ls_part_aabb1_2) {
182                leaf_fn2(leaf_id);
183            }
184        });
185    };
186
187    for leaf_id in composite1.bvh().intersect_aabb(&ls_aabb2_1) {
188        leaf_fn1(leaf_id);
189    }
190
191    workspace
192        .sub_detectors
193        .retain(|_, detector| detector.timestamp == new_timestamp)
194}
195
196impl WorkspaceData for CompositeShapeCompositeShapeContactManifoldsWorkspace {
197    fn as_typed_workspace_data(&self) -> TypedWorkspaceData<'_> {
198        TypedWorkspaceData::CompositeShapeCompositeShapeContactManifoldsWorkspace(self)
199    }
200
201    fn clone_dyn(&self) -> Box<dyn WorkspaceData> {
202        Box::new(self.clone())
203    }
204}
parry3d/query/contact_manifolds/contact_manifolds_composite_shape_composite_shape.rs

parry3d/query/contact_manifolds/
contact_manifolds_composite_shape_composite_shape.rs
