parry3d/query/contact_manifolds/
contact_manifolds_halfspace_pfm.rs

1use crate::math::{Isometry, Real};
2use crate::query::{ContactManifold, TrackedContact};
3use crate::shape::{HalfSpace, PackedFeatureId, PolygonalFeature, PolygonalFeatureMap, Shape};
4
5/// Computes the contact manifold between a convex shape and a ball, both represented as a `Shape` trait-object.
6pub fn contact_manifold_halfspace_pfm_shapes<ManifoldData, ContactData>(
7    pos12: &Isometry<Real>,
8    shape1: &dyn Shape,
9    shape2: &dyn Shape,
10    prediction: Real,
11    manifold: &mut ContactManifold<ManifoldData, ContactData>,
12) where
13    ContactData: Default + Copy,
14{
15    if let (Some(halfspace1), Some((pfm2, border_radius2))) =
16        (shape1.as_halfspace(), shape2.as_polygonal_feature_map())
17    {
18        contact_manifold_halfspace_pfm(
19            pos12,
20            halfspace1,
21            pfm2,
22            border_radius2,
23            prediction,
24            manifold,
25            false,
26        );
27    } else if let (Some((pfm1, border_radius1)), Some(halfspace2)) =
28        (shape1.as_polygonal_feature_map(), shape2.as_halfspace())
29    {
30        contact_manifold_halfspace_pfm(
31            &pos12.inverse(),
32            halfspace2,
33            pfm1,
34            border_radius1,
35            prediction,
36            manifold,
37            true,
38        );
39    }
40}
41
42/// Computes the contact manifold between a convex shape and a ball.
43pub fn contact_manifold_halfspace_pfm<'a, ManifoldData, ContactData, S2>(
44    pos12: &Isometry<Real>,
45    halfspace1: &'a HalfSpace,
46    pfm2: &'a S2,
47    border_radius2: Real,
48    prediction: Real,
49    manifold: &mut ContactManifold<ManifoldData, ContactData>,
50    flipped: bool,
51) where
52    S2: ?Sized + PolygonalFeatureMap,
53    ContactData: Default + Copy,
54{
55    let normal1_2 = pos12.inverse_transform_unit_vector(&halfspace1.normal);
56    let mut feature2 = PolygonalFeature::default();
57    pfm2.local_support_feature(&-normal1_2, &mut feature2);
58
59    // We do this clone to perform contact tracking and transfer impulses.
60    // TODO: find a more efficient way of doing this.
61    let old_manifold_points = core::mem::take(&mut manifold.points);
62
63    for i in 0..feature2.num_vertices {
64        let vtx2 = feature2.vertices[i];
65        let vtx2_1 = pos12 * vtx2;
66        let dist_to_plane = vtx2_1.coords.dot(&halfspace1.normal);
67
68        if dist_to_plane - border_radius2 <= prediction {
69            // Keep this contact point.
70            manifold.points.push(TrackedContact::flipped(
71                vtx2_1 - *halfspace1.normal * dist_to_plane,
72                vtx2 - *normal1_2 * border_radius2,
73                PackedFeatureId::face(0),
74                feature2.vids[i],
75                dist_to_plane - border_radius2,
76                flipped,
77            ));
78        }
79    }
80
81    if flipped {
82        manifold.local_n1 = -*normal1_2;
83        manifold.local_n2 = *halfspace1.normal;
84    } else {
85        manifold.local_n1 = *halfspace1.normal;
86        manifold.local_n2 = -*normal1_2;
87    }
88
89    // println!("Found contacts: {}", manifold.points.len());
90
91    // Transfer impulses.
92    manifold.match_contacts(&old_manifold_points);
93}
parry3d/query/contact_manifolds/contact_manifolds_halfspace_pfm.rs

parry3d/query/contact_manifolds/
contact_manifolds_halfspace_pfm.rs
