ncollide3d/query/closest_points/
closest_points_ball_ball.rs

1use crate::math::Point;
2use crate::query::ClosestPoints;
3use crate::shape::Ball;
4use na::{self, RealField};
5
6/// Proximity between balls.
7#[inline]
8pub fn closest_points_ball_ball<N: RealField + Copy>(
9    center1: &Point<N>,
10    b1: &Ball<N>,
11    center2: &Point<N>,
12    b2: &Ball<N>,
13    margin: N,
14) -> ClosestPoints<N> {
15    assert!(
16        margin >= na::zero(),
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 = *center2 - *center1;
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            ClosestPoints::WithinMargin(*center1 + normal * r1, *center2 + normal * (-r2))
32        }
33    } else {
34        ClosestPoints::Disjoint
35    }
36}