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

parry3d/query/contact_manifolds/
contact_manifolds_trimesh_shape.rs
