parry3d/query/contact_manifolds/
contact_manifolds_heightfield_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;
11#[cfg(feature = "dim2")]
12use crate::shape::Capsule;
13use crate::shape::{HeightField, Shape};
14use crate::utils::hashmap::{Entry, HashMap};
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    archive(check_bytes)
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 HeightFieldShapeContactManifoldsWorkspace {
31    timestamp: bool,
32    sub_detectors: HashMap<u32, SubDetector>,
33}
34
35impl HeightFieldShapeContactManifoldsWorkspace {
36    pub fn new() -> Self {
37        Self::default()
38    }
39}
40
41/// Computes the contact manifold between an heightfield and a shape, both represented as `Shape` trait-objects.
42pub fn contact_manifolds_heightfield_shape_shapes<ManifoldData, ContactData>(
43    dispatcher: &dyn PersistentQueryDispatcher<ManifoldData, ContactData>,
44    pos12: &Isometry<Real>,
45    shape1: &dyn Shape,
46    shape2: &dyn Shape,
47    prediction: Real,
48    manifolds: &mut Vec<ContactManifold<ManifoldData, ContactData>>,
49    workspace: &mut Option<ContactManifoldsWorkspace>,
50) where
51    ManifoldData: Default + Clone,
52    ContactData: Default + Copy,
53{
54    if let Some(heightfield1) = shape1.as_heightfield() {
55        contact_manifolds_heightfield_shape(
56            dispatcher,
57            pos12,
58            heightfield1,
59            shape2,
60            prediction,
61            manifolds,
62            workspace,
63            false,
64        )
65    } else if let Some(heightfield2) = shape2.as_heightfield() {
66        contact_manifolds_heightfield_shape(
67            dispatcher,
68            &pos12.inverse(),
69            heightfield2,
70            shape1,
71            prediction,
72            manifolds,
73            workspace,
74            true,
75        )
76    }
77}
78
79fn ensure_workspace_exists(workspace: &mut Option<ContactManifoldsWorkspace>) {
80    if workspace
81        .as_ref()
82        .and_then(|w| {
83            w.0.downcast_ref::<HeightFieldShapeContactManifoldsWorkspace>()
84        })
85        .is_some()
86    {
87        return;
88    }
89
90    *workspace = Some(ContactManifoldsWorkspace(Box::new(
91        HeightFieldShapeContactManifoldsWorkspace::new(),
92    )));
93}
94
95/// Computes the contact manifold between an heightfield and an abstract shape.
96pub fn contact_manifolds_heightfield_shape<ManifoldData, ContactData>(
97    dispatcher: &dyn PersistentQueryDispatcher<ManifoldData, ContactData>,
98    pos12: &Isometry<Real>,
99    heightfield1: &HeightField,
100    shape2: &dyn Shape,
101    prediction: Real,
102    manifolds: &mut Vec<ContactManifold<ManifoldData, ContactData>>,
103    workspace: &mut Option<ContactManifoldsWorkspace>,
104    flipped: bool,
105) where
106    ManifoldData: Default + Clone,
107    ContactData: Default + Copy,
108{
109    ensure_workspace_exists(workspace);
110    let workspace: &mut HeightFieldShapeContactManifoldsWorkspace =
111        workspace.as_mut().unwrap().0.downcast_mut().unwrap();
112    let new_timestamp = !workspace.timestamp;
113    workspace.timestamp = new_timestamp;
114
115    /*
116     * Compute interferences.
117     */
118    // TODO: somehow precompute the Aabb and reuse it?
119    let ls_aabb2 = shape2.compute_aabb(pos12).loosened(prediction);
120    let mut old_manifolds = core::mem::take(manifolds);
121
122    heightfield1.map_elements_in_local_aabb(&ls_aabb2, &mut |i, part1| {
123        #[cfg(feature = "dim2")]
124        let sub_shape1 = Capsule::new(part1.a, part1.b, 0.0); // TODO: use a segment instead.
125        #[cfg(feature = "dim3")]
126        let sub_shape1 = *part1;
127
128        let sub_detector = match workspace.sub_detectors.entry(i) {
129            Entry::Occupied(entry) => {
130                let sub_detector = entry.into_mut();
131                let manifold = old_manifolds[sub_detector.manifold_id].take();
132                sub_detector.manifold_id = manifolds.len();
133                sub_detector.timestamp = new_timestamp;
134                manifolds.push(manifold);
135                sub_detector
136            }
137            Entry::Vacant(entry) => {
138                let sub_detector = SubDetector {
139                    manifold_id: manifolds.len(),
140                    timestamp: new_timestamp,
141                };
142
143                let (id1, id2) = if flipped { (0, i) } else { (i, 0) };
144                manifolds.push(ContactManifold::with_data(
145                    id1,
146                    id2,
147                    ManifoldData::default(),
148                ));
149
150                entry.insert(sub_detector)
151            }
152        };
153
154        let manifold = &mut manifolds[sub_detector.manifold_id];
155
156        #[cfg(feature = "dim2")]
157        let pseudo_normals = None::<()>;
158        #[cfg(feature = "dim3")]
159        let pseudo_normals = heightfield1.triangle_normal_constraints(i);
160
161        let normal_constraints1 = pseudo_normals.as_ref().map(|pn| pn as &_);
162
163        if flipped {
164            let _ = dispatcher.contact_manifold_convex_convex(
165                &pos12.inverse(),
166                shape2,
167                &sub_shape1,
168                None,
169                normal_constraints1,
170                prediction,
171                manifold,
172            );
173        } else {
174            let _ = dispatcher.contact_manifold_convex_convex(
175                pos12,
176                &sub_shape1,
177                shape2,
178                normal_constraints1,
179                None,
180                prediction,
181                manifold,
182            );
183        }
184    });
185
186    workspace
187        .sub_detectors
188        .retain(|_, detector| detector.timestamp == new_timestamp);
189}
190
191impl WorkspaceData for HeightFieldShapeContactManifoldsWorkspace {
192    fn as_typed_workspace_data(&self) -> TypedWorkspaceData<'_> {
193        TypedWorkspaceData::HeightfieldShapeContactManifoldsWorkspace(self)
194    }
195
196    fn clone_dyn(&self) -> Box<dyn WorkspaceData> {
197        Box::new(self.clone())
198    }
199}
parry3d/query/contact_manifolds/contact_manifolds_heightfield_shape.rs

parry3d/query/contact_manifolds/
contact_manifolds_heightfield_shape.rs
