parry3d/query/contact_manifolds/
contact_manifolds_pfm_pfm.rs

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

parry3d/query/contact_manifolds/
contact_manifolds_pfm_pfm.rs
