parry3d/query/closest_points/
closest_points_ball_ball.rs

1use crate::math::{Isometry, Point, 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(
10    pos12: &Isometry<Real>,
11    b1: &Ball,
12    b2: &Ball,
13    margin: Real,
14) -> ClosestPoints {
15    assert!(
16        margin >= 0.0,
17        "The proximity margin must be positive or null."
18    );
19
20    let r1 = b1.radius;
21    let r2 = b2.radius;
22    let delta_pos = pos12.translation.vector;
23    let distance = delta_pos.norm();
24    let sum_radius = r1 + r2;
25
26    if distance - margin <= sum_radius {
27        if distance <= sum_radius {
28            ClosestPoints::Intersecting
29        } else {
30            let normal = delta_pos.normalize();
31            let p1 = Point::from(normal * r1);
32            let p2 = Point::from(pos12.inverse_transform_vector(&normal) * -r2);
33            ClosestPoints::WithinMargin(p1, p2)
34        }
35    } else {
36        ClosestPoints::Disjoint
37    }
38}