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::{Isometry, 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::IsometryOpt;
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    archive(check_bytes)
22)]
23#[derive(Clone)]
24struct SubDetector {
25    manifold_id: usize,
26    timestamp: bool,
27}
28
29#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
30#[derive(Clone, Default)]
31pub struct HeightFieldCompositeShapeContactManifoldsWorkspace {
32    timestamp: bool,
33    sub_detectors: HashMap<(u32, u32), SubDetector>,
34}
35
36impl HeightFieldCompositeShapeContactManifoldsWorkspace {
37    pub fn new() -> Self {
38        Self::default()
39    }
40}
41
42fn ensure_workspace_exists(workspace: &mut Option<ContactManifoldsWorkspace>) {
43    if workspace
44        .as_ref()
45        .and_then(|w| {
46            w.0.downcast_ref::<HeightFieldCompositeShapeContactManifoldsWorkspace>()
47        })
48        .is_some()
49    {
50        return;
51    }
52
53    *workspace = Some(ContactManifoldsWorkspace(Box::new(
54        HeightFieldCompositeShapeContactManifoldsWorkspace::new(),
55    )));
56}
57
58/// Computes the contact manifold between an heightfield and a composite shape.
59pub fn contact_manifolds_heightfield_composite_shape<ManifoldData, ContactData>(
60    dispatcher: &dyn PersistentQueryDispatcher<ManifoldData, ContactData>,
61    pos12: &Isometry<Real>,
62    pos21: &Isometry<Real>,
63    heightfield1: &HeightField,
64    composite2: &dyn CompositeShape,
65    prediction: Real,
66    manifolds: &mut Vec<ContactManifold<ManifoldData, ContactData>>,
67    workspace: &mut Option<ContactManifoldsWorkspace>,
68    flipped: bool,
69) where
70    ManifoldData: Default + Clone,
71    ContactData: Default + Copy,
72{
73    ensure_workspace_exists(workspace);
74    let workspace: &mut HeightFieldCompositeShapeContactManifoldsWorkspace =
75        workspace.as_mut().unwrap().0.downcast_mut().unwrap();
76    let new_timestamp = !workspace.timestamp;
77    workspace.timestamp = new_timestamp;
78
79    /*
80     * Compute interferences.
81     */
82    let bvh2 = composite2.bvh();
83    let ls_aabb2_1 = bvh2.root_aabb().transform_by(pos12).loosened(prediction);
84    let mut old_manifolds = core::mem::take(manifolds);
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.subshape_pos1 = part_pos2.copied();
116                        } else {
117                            manifold.subshape1 = leaf1;
118                            manifold.subshape2 = leaf2;
119                            manifold.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
129                #[cfg(feature = "dim2")]
130                let triangle_normals = None::<()>;
131                #[cfg(feature = "dim3")]
132                let triangle_normals = heightfield1.triangle_normal_constraints(leaf1);
133                let normal_constraints1 = triangle_normals
134                    .as_ref()
135                    .map(|proj| proj as &dyn NormalConstraints);
136
137                if flipped {
138                    let _ = dispatcher.contact_manifold_convex_convex(
139                        &part_pos2.inv_mul(pos21),
140                        part_shape2,
141                        &sub_shape1,
142                        normal_constraints2,
143                        normal_constraints1,
144                        prediction,
145                        manifold,
146                    );
147                } else {
148                    let _ = dispatcher.contact_manifold_convex_convex(
149                        &part_pos2.prepend_to(pos12),
150                        &sub_shape1,
151                        part_shape2,
152                        normal_constraints1,
153                        normal_constraints2,
154                        prediction,
155                        manifold,
156                    );
157                }
158            });
159        };
160
161        for leaf_id in bvh2.intersect_aabb(&ls_aabb1_2) {
162            leaf_fn2(leaf_id);
163        }
164    });
165
166    workspace
167        .sub_detectors
168        .retain(|_, detector| detector.timestamp == new_timestamp);
169}
170
171impl WorkspaceData for HeightFieldCompositeShapeContactManifoldsWorkspace {
172    fn as_typed_workspace_data(&self) -> TypedWorkspaceData<'_> {
173        TypedWorkspaceData::HeightfieldCompositeShapeContactManifoldsWorkspace(self)
174    }
175
176    fn clone_dyn(&self) -> Box<dyn WorkspaceData> {
177        Box::new(self.clone())
178    }
179}
parry3d/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs

parry3d/query/contact_manifolds/
contact_manifolds_heightfield_composite_shape.rs
