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    let deformable = composite2.is_deformable();
85
86    heightfield1.map_elements_in_local_aabb(&ls_aabb2_1, &mut |leaf1, part1| {
87        #[cfg(feature = "dim2")]
88        let sub_shape1 = Capsule::new(part1.a, part1.b, 0.0); // TODO: use a segment instead.
89        #[cfg(feature = "dim3")]
90        let sub_shape1 = *part1;
91
92        let ls_aabb1_2 = part1.compute_aabb(pos21).loosened(prediction);
93        let mut leaf_fn2 = |leaf2: u32| {
94            composite2.map_part_at(leaf2, &mut |part_pos2, part_shape2, normal_constraints2| {
95                let sub_detector = match workspace.sub_detectors.entry((leaf1, leaf2)) {
96                    Entry::Occupied(entry) => {
97                        let sub_detector = entry.into_mut();
98                        let manifold = old_manifolds[sub_detector.manifold_id].take();
99                        sub_detector.manifold_id = manifolds.len();
100                        sub_detector.timestamp = new_timestamp;
101                        manifolds.push(manifold);
102                        sub_detector
103                    }
104                    Entry::Vacant(entry) => {
105                        let sub_detector = SubDetector {
106                            manifold_id: manifolds.len(),
107                            timestamp: new_timestamp,
108                        };
109
110                        let mut manifold = ContactManifold::new();
111
112                        if flipped {
113                            manifold.subshape1 = leaf2;
114                            manifold.subshape2 = leaf1;
115                            manifold.set_subshape_pos1(part_pos2.copied());
116                        } else {
117                            manifold.subshape1 = leaf1;
118                            manifold.subshape2 = leaf2;
119                            manifold.set_subshape_pos2(part_pos2.copied());
120                        };
121
122                        manifolds.push(manifold);
123                        entry.insert(sub_detector)
124                    }
125                };
126
127                let manifold = &mut manifolds[sub_detector.manifold_id];
128                if deformable {
129                    manifold.mark_shapes_deformed();
130                }
131
132                #[cfg(feature = "dim2")]
133                let triangle_normals = None::<()>;
134                #[cfg(feature = "dim3")]
135                let triangle_normals = heightfield1.triangle_normal_constraints(leaf1);
136                let normal_constraints1 = triangle_normals
137                    .as_ref()
138                    .map(|proj| proj as &dyn NormalConstraints);
139
140                if flipped {
141                    let _ = dispatcher.contact_manifold_convex_convex(
142                        &part_pos2.inv_mul(pos21),
143                        part_shape2,
144                        &sub_shape1,
145                        normal_constraints2,
146                        normal_constraints1,
147                        prediction,
148                        manifold,
149                    );
150                } else {
151                    let _ = dispatcher.contact_manifold_convex_convex(
152                        &part_pos2.prepend_to(pos12),
153                        &sub_shape1,
154                        part_shape2,
155                        normal_constraints1,
156                        normal_constraints2,
157                        prediction,
158                        manifold,
159                    );
160                }
161            });
162        };
163
164        for leaf_id in bvh2.intersect_aabb(&ls_aabb1_2) {
165            leaf_fn2(leaf_id);
166        }
167    });
168
169    workspace
170        .sub_detectors
171        .retain(|_, detector| detector.timestamp == new_timestamp);
172}
173
174impl WorkspaceData for HeightFieldCompositeShapeContactManifoldsWorkspace {
175    fn as_typed_workspace_data(&self) -> TypedWorkspaceData<'_> {
176        TypedWorkspaceData::HeightfieldCompositeShapeContactManifoldsWorkspace(self)
177    }
178
179    fn clone_dyn(&self) -> Box<dyn WorkspaceData> {
180        Box::new(self.clone())
181    }
182}
parry3d/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs

parry3d/query/contact_manifolds/
contact_manifolds_heightfield_composite_shape.rs
