1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37

use crate::math::Point;
use crate::query::ClosestPoints;
use crate::shape::Ball;
use na::{self, RealField};

/// Proximity between balls.
#[inline]
pub fn closest_points_ball_ball<N: RealField>(
    center1: &Point<N>,
    b1: &Ball<N>,
    center2: &Point<N>,
    b2: &Ball<N>,
    margin: N,
) -> ClosestPoints<N> {
    assert!(
        margin >= na::zero(),
        "The proximity margin must be positive or null."
    );

    let r1 = b1.radius();
    let r2 = b2.radius();
    let delta_pos = *center2 - *center1;
    let distance_squared = delta_pos.norm_squared();
    let sum_radius = r1 + r2;
    let sum_radius_with_error = sum_radius + margin;

    if distance_squared <= sum_radius_with_error * sum_radius_with_error {
        if distance_squared <= sum_radius * sum_radius {
            ClosestPoints::Intersecting
        } else {
            let normal = delta_pos.normalize();
            ClosestPoints::WithinMargin(*center1 + normal * r1, *center2 + normal * (-r2))
        }
    } else {
        ClosestPoints::Disjoint
    }
}