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::{Isometry, 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::IsometryOpt;
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    archive(check_bytes)
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 CompositeShapeShapeContactManifoldsWorkspace {
30    timestamp: bool,
31    sub_detectors: HashMap<u32, SubDetector>,
32}
33
34impl CompositeShapeShapeContactManifoldsWorkspace {
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::<CompositeShapeShapeContactManifoldsWorkspace>()
45        })
46        .is_some()
47    {
48        return;
49    }
50
51    *workspace = Some(ContactManifoldsWorkspace(Box::new(
52        CompositeShapeShapeContactManifoldsWorkspace::new(),
53    )));
54}
55
56/// Computes the contact manifolds between a composite shape and an abstract shape.
57pub fn contact_manifolds_composite_shape_shape<ManifoldData, ContactData>(
58    dispatcher: &dyn PersistentQueryDispatcher<ManifoldData, ContactData>,
59    pos12: &Isometry<Real>,
60    composite1: &dyn CompositeShape,
61    shape2: &dyn Shape,
62    prediction: Real,
63    manifolds: &mut Vec<ContactManifold<ManifoldData, ContactData>>,
64    workspace: &mut Option<ContactManifoldsWorkspace>,
65    flipped: bool,
66) where
67    ManifoldData: Default + Clone,
68    ContactData: Default + Copy,
69{
70    ensure_workspace_exists(workspace);
71    let workspace: &mut CompositeShapeShapeContactManifoldsWorkspace =
72        workspace.as_mut().unwrap().0.downcast_mut().unwrap();
73    let new_timestamp = !workspace.timestamp;
74    workspace.timestamp = new_timestamp;
75
76    /*
77     * Compute interferences.
78     */
79
80    let pos12 = *pos12;
81    let pos21 = pos12.inverse();
82
83    // Traverse bvh1 first.
84    let ls_aabb2_1 = shape2.compute_aabb(&pos12).loosened(prediction);
85    let mut old_manifolds = core::mem::take(manifolds);
86
87    let mut leaf1_fn = |leaf1: u32| {
88        composite1.map_part_at(leaf1, &mut |part_pos1, part_shape1, normal_constraints1| {
89            let sub_detector = match workspace.sub_detectors.entry(leaf1) {
90                Entry::Occupied(entry) => {
91                    let sub_detector = entry.into_mut();
92                    let manifold = old_manifolds[sub_detector.manifold_id].take();
93                    sub_detector.manifold_id = manifolds.len();
94                    sub_detector.timestamp = new_timestamp;
95                    manifolds.push(manifold);
96                    sub_detector
97                }
98                Entry::Vacant(entry) => {
99                    let sub_detector = SubDetector {
100                        manifold_id: manifolds.len(),
101                        timestamp: new_timestamp,
102                    };
103
104                    let mut manifold = ContactManifold::new();
105
106                    if flipped {
107                        manifold.subshape1 = 0;
108                        manifold.subshape2 = leaf1;
109                        manifold.subshape_pos2 = part_pos1.copied();
110                    } else {
111                        manifold.subshape1 = leaf1;
112                        manifold.subshape2 = 0;
113                        manifold.subshape_pos1 = part_pos1.copied();
114                    };
115
116                    manifolds.push(manifold);
117                    entry.insert(sub_detector)
118                }
119            };
120
121            let manifold = &mut manifolds[sub_detector.manifold_id];
122
123            if flipped {
124                let _ = dispatcher.contact_manifold_convex_convex(
125                    &part_pos1.prepend_to(&pos21),
126                    shape2,
127                    part_shape1,
128                    None,
129                    normal_constraints1,
130                    prediction,
131                    manifold,
132                );
133            } else {
134                let _ = dispatcher.contact_manifold_convex_convex(
135                    &part_pos1.inv_mul(&pos12),
136                    part_shape1,
137                    shape2,
138                    normal_constraints1,
139                    None,
140                    prediction,
141                    manifold,
142                );
143            }
144        });
145    };
146
147    for leaf_id in composite1.bvh().intersect_aabb(&ls_aabb2_1) {
148        leaf1_fn(leaf_id);
149    }
150
151    workspace
152        .sub_detectors
153        .retain(|_, detector| detector.timestamp == new_timestamp)
154}
155
156impl WorkspaceData for CompositeShapeShapeContactManifoldsWorkspace {
157    fn as_typed_workspace_data(&self) -> TypedWorkspaceData<'_> {
158        TypedWorkspaceData::CompositeShapeShapeContactManifoldsWorkspace(self)
159    }
160
161    fn clone_dyn(&self) -> Box<dyn WorkspaceData> {
162        Box::new(self.clone())
163    }
164}
parry3d/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs

parry3d/query/contact_manifolds/
contact_manifolds_composite_shape_shape.rs
