parry3d/query/contact_manifolds/
contact_manifolds_heightfield_composite_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, NormalConstraints};
9use crate::query::query_dispatcher::PersistentQueryDispatcher;
10use crate::query::ContactManifold;
11#[cfg(feature = "dim2")]
12use crate::shape::Capsule;
13use crate::shape::{CompositeShape, HeightField, Shape};
14use crate::utils::hashmap::{Entry, HashMap};
15use crate::utils::PoseOpt;
16
17#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
18#[cfg_attr(
19    feature = "rkyv",
20    derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize)
21)]
22#[derive(Clone)]
23struct SubDetector {
24    manifold_id: usize,
25    timestamp: bool,
26}
27
28#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
29#[derive(Clone, Default)]
30pub struct HeightFieldCompositeShapeContactManifoldsWorkspace {
31    timestamp: bool,
32    sub_detectors: HashMap<(u32, u32), SubDetector>,
33}
34
35impl HeightFieldCompositeShapeContactManifoldsWorkspace {
36    pub fn new() -> Self {
37        Self::default()
38    }
39}
40
41fn ensure_workspace_exists(workspace: &mut Option<ContactManifoldsWorkspace>) {
42    if workspace
43        .as_ref()
44        .and_then(|w| {
45            w.0.downcast_ref::<HeightFieldCompositeShapeContactManifoldsWorkspace>()
46        })
47        .is_some()
48    {
49        return;
50    }
51
52    *workspace = Some(ContactManifoldsWorkspace(Box::new(
53        HeightFieldCompositeShapeContactManifoldsWorkspace::new(),
54    )));
55}
56
57/// Computes the contact manifold between an heightfield and a composite shape.
58pub fn contact_manifolds_heightfield_composite_shape<ManifoldData, ContactData>(
59    dispatcher: &dyn PersistentQueryDispatcher<ManifoldData, ContactData>,
60    pos12: &Pose,
61    pos21: &Pose,
62    heightfield1: &HeightField,
63    composite2: &dyn CompositeShape,
64    prediction: Real,
65    manifolds: &mut Vec<ContactManifold<ManifoldData, ContactData>>,
66    workspace: &mut Option<ContactManifoldsWorkspace>,
67    flipped: bool,
68) where
69    ManifoldData: Default + Clone,
70    ContactData: Default + Copy,
71{
72    ensure_workspace_exists(workspace);
73    let workspace: &mut HeightFieldCompositeShapeContactManifoldsWorkspace =
74        workspace.as_mut().unwrap().0.downcast_mut().unwrap();
75    let new_timestamp = !workspace.timestamp;
76    workspace.timestamp = new_timestamp;
77
78    /*
79     * Compute interferences.
80     */
81    let bvh2 = composite2.bvh();
82    let ls_aabb2_1 = bvh2.root_aabb().transform_by(pos12).loosened(prediction);
83    let mut old_manifolds = core::mem::take(manifolds);
84
85    heightfield1.map_elements_in_local_aabb(&ls_aabb2_1, &mut |leaf1, part1| {
86        #[cfg(feature = "dim2")]
87        let sub_shape1 = Capsule::new(part1.a, part1.b, 0.0); // TODO: use a segment instead.
88        #[cfg(feature = "dim3")]
89        let sub_shape1 = *part1;
90
91        let ls_aabb1_2 = part1.compute_aabb(pos21).loosened(prediction);
92        let mut leaf_fn2 = |leaf2: u32| {
93            composite2.map_part_at(leaf2, &mut |part_pos2, part_shape2, normal_constraints2| {
94                let sub_detector = match workspace.sub_detectors.entry((leaf1, leaf2)) {
95                    Entry::Occupied(entry) => {
96                        let sub_detector = entry.into_mut();
97                        let manifold = old_manifolds[sub_detector.manifold_id].take();
98                        sub_detector.manifold_id = manifolds.len();
99                        sub_detector.timestamp = new_timestamp;
100                        manifolds.push(manifold);
101                        sub_detector
102                    }
103                    Entry::Vacant(entry) => {
104                        let sub_detector = SubDetector {
105                            manifold_id: manifolds.len(),
106                            timestamp: new_timestamp,
107                        };
108
109                        let mut manifold = ContactManifold::new();
110
111                        if flipped {
112                            manifold.subshape1 = leaf2;
113                            manifold.subshape2 = leaf1;
114                            manifold.subshape_pos1 = part_pos2.copied();
115                        } else {
116                            manifold.subshape1 = leaf1;
117                            manifold.subshape2 = leaf2;
118                            manifold.subshape_pos2 = part_pos2.copied();
119                        };
120
121                        manifolds.push(manifold);
122                        entry.insert(sub_detector)
123                    }
124                };
125
126                let manifold = &mut manifolds[sub_detector.manifold_id];
127
128                #[cfg(feature = "dim2")]
129                let triangle_normals = None::<()>;
130                #[cfg(feature = "dim3")]
131                let triangle_normals = heightfield1.triangle_normal_constraints(leaf1);
132                let normal_constraints1 = triangle_normals
133                    .as_ref()
134                    .map(|proj| proj as &dyn NormalConstraints);
135
136                if flipped {
137                    let _ = dispatcher.contact_manifold_convex_convex(
138                        &part_pos2.inv_mul(pos21),
139                        part_shape2,
140                        &sub_shape1,
141                        normal_constraints2,
142                        normal_constraints1,
143                        prediction,
144                        manifold,
145                    );
146                } else {
147                    let _ = dispatcher.contact_manifold_convex_convex(
148                        &part_pos2.prepend_to(pos12),
149                        &sub_shape1,
150                        part_shape2,
151                        normal_constraints1,
152                        normal_constraints2,
153                        prediction,
154                        manifold,
155                    );
156                }
157            });
158        };
159
160        for leaf_id in bvh2.intersect_aabb(&ls_aabb1_2) {
161            leaf_fn2(leaf_id);
162        }
163    });
164
165    workspace
166        .sub_detectors
167        .retain(|_, detector| detector.timestamp == new_timestamp);
168}
169
170impl WorkspaceData for HeightFieldCompositeShapeContactManifoldsWorkspace {
171    fn as_typed_workspace_data(&self) -> TypedWorkspaceData<'_> {
172        TypedWorkspaceData::HeightfieldCompositeShapeContactManifoldsWorkspace(self)
173    }
174
175    fn clone_dyn(&self) -> Box<dyn WorkspaceData> {
176        Box::new(self.clone())
177    }
178}
parry3d/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs

parry3d/query/contact_manifolds/
contact_manifolds_heightfield_composite_shape.rs
