parry3d/query/contact_manifolds/
contact_manifolds_trimesh_shape.rs

1use alloc::{boxed::Box, vec::Vec};
2
3use crate::bounding_volume::{Aabb, 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::details::NormalConstraints;
10use crate::query::query_dispatcher::PersistentQueryDispatcher;
11use crate::query::ContactManifold;
12use crate::shape::{Shape, TriMesh};
13
14#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
15#[cfg_attr(
16    feature = "rkyv",
17    derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize),
18    archive(check_bytes)
19)]
20#[derive(Clone)]
21pub struct TriMeshShapeContactManifoldsWorkspace {
22    interferences: Vec<u32>,
23    local_aabb2: Aabb,
24    old_interferences: Vec<u32>,
25}
26
27impl Default for TriMeshShapeContactManifoldsWorkspace {
28    fn default() -> Self {
29        Self::new()
30    }
31}
32
33impl TriMeshShapeContactManifoldsWorkspace {
34    pub fn new() -> Self {
35        Self {
36            interferences: Vec::new(),
37            local_aabb2: Aabb::new_invalid(),
38            old_interferences: Vec::new(),
39        }
40    }
41}
42
43/// Computes the contact manifold between a triangle-mesh an a shape, both represented as `Shape` trait-objects.
44pub fn contact_manifolds_trimesh_shape_shapes<ManifoldData, ContactData>(
45    dispatcher: &dyn PersistentQueryDispatcher<ManifoldData, ContactData>,
46    pos12: &Isometry<Real>,
47    shape1: &dyn Shape,
48    shape2: &dyn Shape,
49    prediction: Real,
50    manifolds: &mut Vec<ContactManifold<ManifoldData, ContactData>>,
51    workspace: &mut Option<ContactManifoldsWorkspace>,
52) where
53    ManifoldData: Default,
54    ContactData: Default + Copy,
55{
56    if let Some(trimesh1) = shape1.as_trimesh() {
57        contact_manifolds_trimesh_shape(
58            dispatcher, pos12, trimesh1, shape2, prediction, manifolds, workspace, false,
59        )
60    } else if let Some(trimesh2) = shape2.as_trimesh() {
61        contact_manifolds_trimesh_shape(
62            dispatcher,
63            &pos12.inverse(),
64            trimesh2,
65            shape1,
66            prediction,
67            manifolds,
68            workspace,
69            true,
70        )
71    }
72}
73
74fn ensure_workspace_exists(workspace: &mut Option<ContactManifoldsWorkspace>) {
75    if workspace
76        .as_mut()
77        .and_then(|w| w.0.downcast_mut::<TriMeshShapeContactManifoldsWorkspace>())
78        .is_some()
79    {
80        return;
81    }
82
83    *workspace = Some(ContactManifoldsWorkspace(Box::new(
84        TriMeshShapeContactManifoldsWorkspace::new(),
85    )));
86}
87
88/// Computes the contact manifold between a triangle-mesh and a shape.
89pub fn contact_manifolds_trimesh_shape<ManifoldData, ContactData>(
90    dispatcher: &dyn PersistentQueryDispatcher<ManifoldData, ContactData>,
91    pos12: &Isometry<Real>,
92    trimesh1: &TriMesh,
93    shape2: &dyn Shape,
94    prediction: Real,
95    manifolds: &mut Vec<ContactManifold<ManifoldData, ContactData>>,
96    workspace: &mut Option<ContactManifoldsWorkspace>,
97    flipped: bool,
98) where
99    ManifoldData: Default,
100    ContactData: Default + Copy,
101{
102    ensure_workspace_exists(workspace);
103    let workspace: &mut TriMeshShapeContactManifoldsWorkspace =
104        workspace.as_mut().unwrap().0.downcast_mut().unwrap();
105
106    /*
107     * Compute interferences.
108     */
109    // TODO: somehow precompute the Aabb and reuse it?
110    let mut new_local_aabb2 = shape2.compute_aabb(pos12).loosened(prediction);
111    let same_local_aabb2 = workspace.local_aabb2.contains(&new_local_aabb2);
112    let mut old_manifolds = Vec::new();
113
114    if !same_local_aabb2 {
115        let extra_margin =
116            (new_local_aabb2.maxs - new_local_aabb2.mins).map(|e| (e / 10.0).min(0.1));
117        new_local_aabb2.mins -= extra_margin;
118        new_local_aabb2.maxs += extra_margin;
119
120        let local_aabb2 = new_local_aabb2; // .loosened(prediction * 2.0); // TODO: what would be the best value?
121        core::mem::swap(
122            &mut workspace.old_interferences,
123            &mut workspace.interferences,
124        );
125
126        core::mem::swap(manifolds, &mut old_manifolds);
127
128        // NOTE: This assertion may fire due to the invalid triangle_ids that the
129        // near-phase may return (due to SIMD sentinels).
130        //
131        // assert_eq!(
132        //     workspace
133        //         .old_interferences
134        //         .len()
135        //         .min(trimesh1.num_triangles()),
136        //     workspace.old_manifolds.len()
137        // );
138
139        workspace.interferences.clear();
140        workspace
141            .interferences
142            .extend(trimesh1.bvh().intersect_aabb(&local_aabb2));
143        workspace.local_aabb2 = local_aabb2;
144    }
145
146    /*
147     * Dispatch to the specific solver by keeping the previous manifold if we already had one.
148     */
149    let new_interferences = &workspace.interferences;
150    let mut old_inter_it = workspace.old_interferences.drain(..).peekable();
151    let mut old_manifolds_it = old_manifolds.drain(..);
152
153    // TODO: don't redispatch at each frame (we should probably do the same as
154    // the heightfield).
155    for (i, triangle_id) in new_interferences.iter().enumerate() {
156        if *triangle_id >= trimesh1.num_triangles() as u32 {
157            // Because of SIMD padding, the broad-phase may return triangle indices greater
158            // than the max.
159            continue;
160        }
161
162        if !same_local_aabb2 {
163            loop {
164                match old_inter_it.peek() {
165                    Some(old_triangle_id) if *old_triangle_id < *triangle_id => {
166                        let _ = old_inter_it.next();
167                        let _ = old_manifolds_it.next();
168                    }
169                    _ => break,
170                }
171            }
172
173            let manifold = if old_inter_it.peek() != Some(triangle_id) {
174                let (id1, id2) = if flipped {
175                    (0, *triangle_id)
176                } else {
177                    (*triangle_id, 0)
178                };
179                ContactManifold::with_data(id1, id2, ManifoldData::default())
180            } else {
181                // We already have a manifold for this triangle.
182                let _ = old_inter_it.next();
183                old_manifolds_it.next().unwrap()
184            };
185
186            manifolds.push(manifold);
187        }
188
189        let manifold = &mut manifolds[i];
190        let triangle1 = trimesh1.triangle(*triangle_id);
191        let triangle_normals1 = trimesh1.triangle_normal_constraints(*triangle_id);
192        let normal_constraints1 = triangle_normals1
193            .as_ref()
194            .map(|proj| proj as &dyn NormalConstraints);
195
196        if flipped {
197            let _ = dispatcher.contact_manifold_convex_convex(
198                &pos12.inverse(),
199                shape2,
200                &triangle1,
201                None,
202                normal_constraints1,
203                prediction,
204                manifold,
205            );
206        } else {
207            let _ = dispatcher.contact_manifold_convex_convex(
208                pos12,
209                &triangle1,
210                shape2,
211                normal_constraints1,
212                None,
213                prediction,
214                manifold,
215            );
216        }
217    }
218}
219
220impl WorkspaceData for TriMeshShapeContactManifoldsWorkspace {
221    fn as_typed_workspace_data(&self) -> TypedWorkspaceData<'_> {
222        TypedWorkspaceData::TriMeshShapeContactManifoldsWorkspace(self)
223    }
224
225    fn clone_dyn(&self) -> Box<dyn WorkspaceData> {
226        Box::new(self.clone())
227    }
228}
parry3d/query/contact_manifolds/contact_manifolds_trimesh_shape.rs

parry3d/query/contact_manifolds/
contact_manifolds_trimesh_shape.rs
