parry2d/query/closest_points/
closest_points_ball_ball.rs

1use crate::math::{Pose, Real};
2use crate::query::ClosestPoints;
3use crate::shape::Ball;
4
5/// Closest points between balls.
6///
7/// Each returned point is expressed on the local-space of the corresponding shape.
8#[inline]
9pub fn closest_points_ball_ball(pos12: &Pose, b1: &Ball, b2: &Ball, margin: Real) -> ClosestPoints {
10    assert!(
11        margin >= 0.0,
12        "The proximity margin must be positive or null."
13    );
14
15    let r1 = b1.radius;
16    let r2 = b2.radius;
17    let delta_pos = pos12.translation;
18    let distance = delta_pos.length();
19    let sum_radius = r1 + r2;
20
21    if distance - margin <= sum_radius {
22        if distance <= sum_radius {
23            ClosestPoints::Intersecting
24        } else {
25            let normal = delta_pos.normalize();
26            let p1 = normal * r1;
27            let p2 = pos12.rotation.inverse() * normal * -r2;
28            ClosestPoints::WithinMargin(p1, p2)
29        }
30    } else {
31        ClosestPoints::Disjoint
32    }
33}