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

parry3d/query/contact_manifolds/
contact_manifolds_trimesh_shape.rs
