parry3d/query/contact_manifolds/
contact_manifolds_pfm_pfm.rs

1use crate::math::{Pose, Real, Vector};
2use crate::query::contact_manifolds::{NormalConstraints, NormalConstraintsPair};
3use crate::query::{
4    self,
5    gjk::{GJKResult, VoronoiSimplex},
6    ContactManifold, TrackedContact,
7};
8use crate::shape::{PackedFeatureId, PolygonalFeature, PolygonalFeatureMap, Shape};
9
10/// Computes the contact manifold between two convex shapes implementing the `PolygonalSupportMap`
11/// trait, both represented as `Shape` trait-objects.
12pub fn contact_manifold_pfm_pfm_shapes<ManifoldData, ContactData>(
13    pos12: &Pose,
14    shape1: &dyn Shape,
15    shape2: &dyn Shape,
16    normal_constraints1: Option<&dyn NormalConstraints>,
17    normal_constraints2: Option<&dyn NormalConstraints>,
18    prediction: Real,
19    manifold: &mut ContactManifold<ManifoldData, ContactData>,
20) where
21    ManifoldData: Default,
22    ContactData: Default + Copy,
23{
24    if let (Some((pfm1, border_radius1)), Some((pfm2, border_radius2))) = (
25        shape1.as_polygonal_feature_map(),
26        shape2.as_polygonal_feature_map(),
27    ) {
28        contact_manifold_pfm_pfm(
29            pos12,
30            pfm1,
31            border_radius1,
32            normal_constraints1,
33            pfm2,
34            border_radius2,
35            normal_constraints2,
36            prediction,
37            manifold,
38        );
39    }
40}
41
42/// Computes the contact manifold between two convex shapes implementing the `PolygonalSupportMap` trait.
43pub fn contact_manifold_pfm_pfm<'a, ManifoldData, ContactData, S1, S2>(
44    pos12: &Pose,
45    pfm1: &'a S1,
46    border_radius1: Real,
47    normal_constraints1: Option<&dyn NormalConstraints>,
48    pfm2: &'a S2,
49    border_radius2: Real,
50    normal_constraints2: Option<&dyn NormalConstraints>,
51    prediction: Real,
52    manifold: &mut ContactManifold<ManifoldData, ContactData>,
53) where
54    S1: ?Sized + PolygonalFeatureMap,
55    S2: ?Sized + PolygonalFeatureMap,
56    ManifoldData: Default,
57    ContactData: Default + Copy,
58{
59    // We use very small thresholds for the manifold update because something to high would
60    // cause numerical drifts with the effect of introducing bumps in
61    // what should have been smooth rolling motions.
62    if manifold.try_update_contacts_eps(pos12, crate::utils::COS_1_DEGREES, 1.0e-6) {
63        return;
64    }
65
66    let init_dir = (manifold.local_n1).try_normalize();
67    let total_prediction = prediction + border_radius1 + border_radius2;
68    let contact = query::details::contact_support_map_support_map_with_params(
69        pos12,
70        pfm1,
71        pfm2,
72        total_prediction,
73        &mut VoronoiSimplex::new(),
74        init_dir,
75    );
76
77    let old_manifold_points = manifold.points.clone();
78    manifold.clear();
79
80    match contact {
81        GJKResult::ClosestPoints(p1, p2_1, dir) => {
82            let mut local_n1 = dir;
83            let mut local_n2 = pos12.rotation.inverse() * -dir;
84            let dist = (p2_1 - p1).dot(local_n1);
85
86            if !(normal_constraints1, normal_constraints2).project_local_normals(
87                pos12,
88                &mut local_n1,
89                &mut local_n2,
90            ) {
91                // The contact got completely discarded by the normal correction.
92                return;
93            }
94
95            let mut feature1 = PolygonalFeature::default();
96            let mut feature2 = PolygonalFeature::default();
97            pfm1.local_support_feature(local_n1, &mut feature1);
98            pfm2.local_support_feature(local_n2, &mut feature2);
99
100            PolygonalFeature::contacts(
101                pos12,
102                &pos12.inverse(),
103                local_n1,
104                local_n2,
105                &feature1,
106                &feature2,
107                manifold,
108                false,
109            );
110
111            if (cfg!(feature = "dim3") || cfg!(feature = "dim2") && manifold.points.is_empty())
112                // If normal constraints changed the GJK direction, it is no longer valid so we cant use it for this additional contact.
113                && local_n1 == dir
114            {
115                let contact = TrackedContact::new(
116                    p1,
117                    pos12.inverse_transform_point(p2_1),
118                    PackedFeatureId::UNKNOWN, // TODO: We don't know what features are involved.
119                    PackedFeatureId::UNKNOWN,
120                    (p2_1 - p1).dot(local_n1),
121                );
122                manifold.points.push(contact);
123            }
124
125            if normal_constraints1.is_some() || normal_constraints2.is_some() {
126                // HACK: some normal correction can lead to very incorrect penetration
127                //       depth, e.g., if the other object extends very far toward that direction.
128                //       This is caused by the locality of the convex/convex check.
129                //       I haven’t found a good mathematically robust approach to account for
130                //       that locally, so for now, we eliminate points that are large divergence
131                //       relative to the unconstrained penetration distance.
132                manifold
133                    .points
134                    .retain(|pt| dist >= 0.0 || pt.dist >= 0.0 || pt.dist >= dist * 5.0);
135            }
136
137            // Adjust points to take the radius into account.
138            if border_radius1 != 0.0 || border_radius2 != 0.0 {
139                for contact in &mut manifold.points {
140                    contact.local_p1 += local_n1 * border_radius1;
141                    contact.local_p2 += local_n2 * border_radius2;
142                    contact.dist -= border_radius1 + border_radius2;
143                }
144            }
145
146            manifold.local_n1 = local_n1;
147            manifold.local_n2 = local_n2;
148        }
149        GJKResult::NoIntersection(dir) => {
150            // Use the manifold normal as a cache.
151            manifold.local_n1 = dir;
152        }
153        _ => {
154            // Reset the cached direction.
155            manifold.local_n1 = Vector::ZERO;
156        }
157    }
158
159    // Transfer impulses.
160    manifold.match_contacts(&old_manifold_points);
161}
parry3d/query/contact_manifolds/contact_manifolds_pfm_pfm.rs

parry3d/query/contact_manifolds/
contact_manifolds_pfm_pfm.rs
