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

parry3d/query/contact_manifolds/
contact_manifolds_heightfield_shape.rs
