parry2d/query/contact/
contact_ball_ball.rs

1use crate::math::{ComplexField, Pose, Real, Vector};
2use crate::query::Contact;
3use crate::shape::Ball;
4use num::Zero;
5
6/// Contact between balls.
7#[inline]
8pub fn contact_ball_ball(pos12: &Pose, b1: &Ball, b2: &Ball, prediction: Real) -> Option<Contact> {
9    let r1 = b1.radius;
10    let r2 = b2.radius;
11    let center2_1 = pos12.translation;
12    let distance_squared = center2_1.length_squared();
13    let sum_radius = r1 + r2;
14    let sum_radius_with_error = sum_radius + prediction;
15
16    if distance_squared < sum_radius_with_error * sum_radius_with_error {
17        let normal1 = if !distance_squared.is_zero() {
18            (center2_1).normalize()
19        } else {
20            Vector::X
21        };
22        let normal2 = -(pos12.rotation.inverse() * normal1);
23        let point1 = normal1 * r1;
24        let point2 = normal2 * r2;
25
26        Some(Contact::new(
27            point1,
28            point2,
29            normal1,
30            normal2,
31            <Real as ComplexField>::sqrt(distance_squared) - sum_radius,
32        ))
33    } else {
34        None
35    }
36}