parry3d/query/contact_manifolds/
contact_manifolds_convex_ball.rs

1use crate::math::{Pose, Real, Vector};
2use crate::query::contact_manifolds::{NormalConstraints, NormalConstraintsPair};
3use crate::query::{ContactManifold, Ray, TrackedContact};
4use crate::shape::{Ball, PackedFeatureId, Shape};
5
6/// Computes the contact manifold between a convex shape and a ball, both represented as a `Shape` trait-object.
7pub fn contact_manifold_convex_ball_shapes<ManifoldData, ContactData>(
8    pos12: &Pose,
9    shape1: &dyn Shape,
10    shape2: &dyn Shape,
11    normal_constraints1: Option<&dyn NormalConstraints>,
12    normal_constraints2: Option<&dyn NormalConstraints>,
13    prediction: Real,
14    manifold: &mut ContactManifold<ManifoldData, ContactData>,
15) where
16    ContactData: Default + Copy,
17{
18    if let Some(ball1) = shape1.as_ball() {
19        contact_manifold_convex_ball(
20            &pos12.inverse(),
21            shape2,
22            ball1,
23            normal_constraints2,
24            normal_constraints1,
25            prediction,
26            manifold,
27            true,
28        );
29    } else if let Some(ball2) = shape2.as_ball() {
30        contact_manifold_convex_ball(
31            pos12,
32            shape1,
33            ball2,
34            normal_constraints1,
35            normal_constraints2,
36            prediction,
37            manifold,
38            false,
39        );
40    }
41}
42
43/// Computes the contact manifold between a convex shape and a ball.
44pub fn contact_manifold_convex_ball<'a, ManifoldData, ContactData, S1>(
45    pos12: &Pose,
46    shape1: &'a S1,
47    ball2: &'a Ball,
48    normal_constraints1: Option<&dyn NormalConstraints>,
49    normal_constraints2: Option<&dyn NormalConstraints>,
50    prediction: Real,
51    manifold: &mut ContactManifold<ManifoldData, ContactData>,
52    flipped: bool,
53) where
54    S1: ?Sized + Shape,
55    ContactData: Default + Copy,
56{
57    let local_p2_1 = pos12.translation;
58    let (proj, mut fid1) = shape1.project_local_point_and_get_feature(local_p2_1);
59    let mut local_p1 = proj.point;
60    let dpos = local_p2_1 - local_p1;
61
62    // local_n1 points from the surface towards our origin if defined, otherwise from the other
63    // shape's origin towards our origin if defined, otherwise towards +x
64    // NOTE: we used `dpos.normalize_and_length()` here before. But since `normalize_and_length()`
65    //       multiplying by the reciprocal (1.0 / length) instead of directly dividing by `length`,
66    //       it can introduce tiny numerical errors that result in drift breaking perfectly verticality
67    //       of contact normals for, e.g., the 3D spring joint demo in rapier. So we divide ourselves
68    //       instead.
69    let mut dist = dpos.length();
70    let mut local_n1 = if dist == 0.0 {
71        pos12.translation.normalize_or(Vector::Y)
72    } else {
73        dpos / dist
74    };
75
76    if proj.is_inside {
77        local_n1 = -local_n1;
78        dist = -dist;
79    }
80
81    if dist <= ball2.radius + prediction {
82        let mut local_n2 = pos12.rotation.inverse() * -local_n1;
83        let uncorrected_local_n2 = local_n2;
84
85        if !(normal_constraints1, normal_constraints2).project_local_normals(
86            pos12,
87            &mut local_n1,
88            &mut local_n2,
89        ) {
90            // The contact got completely discarded by the normal correction.
91            manifold.clear();
92            return;
93        }
94
95        let local_p2 = local_n2 * ball2.radius;
96
97        // If a correction happened, adjust the contact point on the first body.
98        if uncorrected_local_n2 != local_n2 {
99            let ray1 = Ray::new(
100                pos12.translation,
101                if proj.is_inside { local_n1 } else { -local_n1 },
102            );
103
104            if let Some(hit) = shape1.cast_local_ray_and_get_normal(&ray1, Real::MAX, false) {
105                local_p1 = ray1.point_at(hit.time_of_impact);
106                dist = if proj.is_inside {
107                    -hit.time_of_impact
108                } else {
109                    hit.time_of_impact
110                };
111                fid1 = hit.feature;
112            } else {
113                manifold.clear();
114                return;
115            }
116        }
117
118        let contact_point = TrackedContact::flipped(
119            local_p1,
120            local_p2,
121            fid1.into(),
122            PackedFeatureId::face(0),
123            dist - ball2.radius,
124            flipped,
125        );
126
127        if manifold.points.len() != 1 {
128            manifold.clear();
129            manifold.points.push(contact_point);
130        } else {
131            // Copy only the geometry so we keep the warmstart impulses.
132            manifold.points[0].copy_geometry_from(contact_point);
133        }
134
135        if flipped {
136            manifold.local_n1 = local_n2;
137            manifold.local_n2 = local_n1;
138        } else {
139            manifold.local_n1 = local_n1;
140            manifold.local_n2 = local_n2;
141        }
142    } else {
143        manifold.clear();
144    }
145}
parry3d/query/contact_manifolds/contact_manifolds_convex_ball.rs

parry3d/query/contact_manifolds/
contact_manifolds_convex_ball.rs
