parry3d/query/contact_manifolds/
contact_manifolds_capsule_capsule.rs

1use crate::math::{Isometry, Real, Vector};
2use crate::query::{ContactManifold, TrackedContact};
3#[cfg(feature = "dim2")]
4use crate::shape::SegmentPointLocation;
5use crate::shape::{Capsule, PackedFeatureId, Shape};
6use approx::AbsDiffEq;
7use na::Unit;
8
9/// Computes the contact manifold between two capsules given as `Shape` trait-objects.
10pub fn contact_manifold_capsule_capsule_shapes<ManifoldData, ContactData>(
11    pos12: &Isometry<Real>,
12    shape1: &dyn Shape,
13    shape2: &dyn Shape,
14    prediction: Real,
15    manifold: &mut ContactManifold<ManifoldData, ContactData>,
16) where
17    ContactData: Default + Copy,
18{
19    if let (Some(capsule1), Some(capsule2)) = (shape1.as_capsule(), shape2.as_capsule()) {
20        contact_manifold_capsule_capsule(pos12, capsule1, capsule2, prediction, manifold);
21    }
22}
23
24/// Computes the contact manifold between two capsules.
25#[cfg(feature = "dim2")]
26pub fn contact_manifold_capsule_capsule<'a, ManifoldData, ContactData>(
27    pos12: &Isometry<Real>,
28    capsule1: &'a Capsule,
29    capsule2: &'a Capsule,
30    prediction: Real,
31    manifold: &mut ContactManifold<ManifoldData, ContactData>,
32) where
33    ContactData: Default + Copy,
34{
35    let seg1 = capsule1.segment;
36    let seg2_1 = capsule2.segment.transformed(pos12);
37    let (loc1, loc2) = crate::query::details::closest_points_segment_segment_with_locations_nD(
38        (&seg1.a, &seg1.b),
39        (&seg2_1.a, &seg2_1.b),
40    );
41
42    // We do this clone to perform contact tracking and transfer impulses.
43    // TODO: find a more efficient way of doing this.
44    let old_manifold_points = manifold.points.clone();
45    manifold.clear();
46
47    let fid1 = if let SegmentPointLocation::OnVertex(v1) = loc1 {
48        v1 * 2
49    } else {
50        1
51    };
52    let fid2 = if let SegmentPointLocation::OnVertex(v2) = loc2 {
53        v2 * 2
54    } else {
55        1
56    };
57
58    let bcoords1 = loc1.barycentric_coordinates();
59    let bcoords2 = loc2.barycentric_coordinates();
60    let local_p1 = seg1.a * bcoords1[0] + seg1.b.coords * bcoords1[1];
61    let local_p2_1 = seg2_1.a * bcoords2[0] + seg2_1.b.coords * bcoords2[1];
62
63    let local_n1 =
64        Unit::try_new(local_p2_1 - local_p1, Real::default_epsilon()).unwrap_or(Vector::y_axis());
65    let dist = (local_p2_1 - local_p1).dot(&local_n1);
66
67    if dist <= prediction + capsule1.radius + capsule2.radius {
68        let local_n2 = pos12.inverse_transform_unit_vector(&-local_n1);
69        let local_p2 = pos12.inverse_transform_point(&local_p2_1);
70        let contact = TrackedContact::new(
71            local_p1,
72            local_p2,
73            PackedFeatureId::face(fid1),
74            PackedFeatureId::face(fid2),
75            dist,
76        );
77        manifold.points.push(contact);
78
79        manifold.local_n1 = *local_n1;
80        manifold.local_n2 = *local_n2;
81    } else {
82        // No contact within tolerance.
83        return;
84    }
85
86    if let (Some(dir1), Some(dir2)) = (seg1.direction(), seg2_1.direction()) {
87        if dir1.dot(&dir2).abs() >= crate::utils::COS_FRAC_PI_8
88            && dir1.dot(&local_n1).abs() < crate::utils::SIN_FRAC_PI_8
89        {
90            // Capsules axes are almost parallel and are almost perpendicular to the normal.
91            // Find a second contact point.
92            if let Some((clip_a, clip_b)) = crate::query::details::clip_segment_segment_with_normal(
93                (seg1.a, seg1.b),
94                (seg2_1.a, seg2_1.b),
95                *local_n1,
96            ) {
97                let contact =
98                    if (clip_a.0 - local_p1).norm_squared() > Real::default_epsilon() * 100.0 {
99                        // Use clip_a as the second contact.
100                        TrackedContact::new(
101                            clip_a.0,
102                            pos12.inverse_transform_point(&clip_a.1),
103                            PackedFeatureId::face(clip_a.2 as u32),
104                            PackedFeatureId::face(clip_a.3 as u32),
105                            (clip_a.1 - clip_a.0).dot(&local_n1),
106                        )
107                    } else {
108                        // Use clip_b as the second contact.
109                        TrackedContact::new(
110                            clip_b.0,
111                            pos12.inverse_transform_point(&clip_b.1),
112                            PackedFeatureId::face(clip_b.2 as u32),
113                            PackedFeatureId::face(clip_b.3 as u32),
114                            (clip_b.1 - clip_b.0).dot(&local_n1),
115                        )
116                    };
117
118                manifold.points.push(contact);
119            }
120        }
121    }
122
123    for point in &mut manifold.points {
124        point.local_p1 += manifold.local_n1 * capsule1.radius;
125        point.local_p2 += manifold.local_n2 * capsule2.radius;
126        point.dist -= capsule1.radius + capsule2.radius;
127    }
128
129    manifold.match_contacts(&old_manifold_points);
130}
131
132/// Computes the contact manifold between two capsules.
133#[cfg(feature = "dim3")]
134pub fn contact_manifold_capsule_capsule<'a, ManifoldData, ContactData>(
135    pos12: &Isometry<Real>,
136    capsule1: &'a Capsule,
137    capsule2: &'a Capsule,
138    prediction: Real,
139    manifold: &mut ContactManifold<ManifoldData, ContactData>,
140) where
141    ContactData: Default + Copy,
142{
143    let seg1 = capsule1.segment;
144    let seg2_1 = capsule2.segment.transformed(pos12);
145    let (loc1, loc2) =
146        crate::query::closest_points::closest_points_segment_segment_with_locations_nD(
147            (&seg1.a, &seg1.b),
148            (&seg2_1.a, &seg2_1.b),
149        );
150
151    let bcoords1 = loc1.barycentric_coordinates();
152    let bcoords2 = loc2.barycentric_coordinates();
153    let local_p1 = seg1.a * bcoords1[0] + seg1.b.coords * bcoords1[1];
154    let local_p2_1 = seg2_1.a * bcoords2[0] + seg2_1.b.coords * bcoords2[1];
155
156    let local_n1 =
157        Unit::try_new(local_p2_1 - local_p1, Real::default_epsilon()).unwrap_or(Vector::y_axis());
158    let dist = (local_p2_1 - local_p1).dot(&local_n1) - capsule1.radius - capsule2.radius;
159
160    if dist <= prediction {
161        let local_n2 = pos12.inverse_transform_unit_vector(&-local_n1);
162        let fid = PackedFeatureId::face(0);
163        let contact = TrackedContact::new(
164            local_p1 + *local_n1 * capsule1.radius,
165            pos12.inverse_transform_point(&local_p2_1) + *local_n2 * capsule2.radius,
166            fid,
167            fid,
168            dist,
169        );
170
171        if !manifold.points.is_empty() {
172            manifold.points[0].copy_geometry_from(contact);
173        } else {
174            manifold.points.push(contact);
175        }
176
177        manifold.local_n1 = *local_n1;
178        manifold.local_n2 = *local_n2;
179    } else {
180        manifold.clear();
181    }
182}
parry3d/query/contact_manifolds/contact_manifolds_capsule_capsule.rs

parry3d/query/contact_manifolds/
contact_manifolds_capsule_capsule.rs
