parry3d/query/contact/
contact_ball_ball.rs

1use crate::math::{Isometry, Point, Real, Vector};
2use crate::query::Contact;
3use crate::shape::Ball;
4use na::{self, ComplexField, Unit};
5use num::Zero;
6
7/// Contact between balls.
8#[inline]
9pub fn contact_ball_ball(
10    pos12: &Isometry<Real>,
11    b1: &Ball,
12    b2: &Ball,
13    prediction: Real,
14) -> Option<Contact> {
15    let r1 = b1.radius;
16    let r2 = b2.radius;
17    let center2_1 = pos12.translation.vector;
18    let distance_squared = center2_1.norm_squared();
19    let sum_radius = r1 + r2;
20    let sum_radius_with_error = sum_radius + prediction;
21
22    if distance_squared < sum_radius_with_error * sum_radius_with_error {
23        let normal1 = if !distance_squared.is_zero() {
24            Unit::new_normalize(center2_1)
25        } else {
26            Vector::x_axis()
27        };
28        let normal2 = -pos12.inverse_transform_unit_vector(&normal1);
29        let point1 = Point::from(*normal1 * r1);
30        let point2 = Point::from(*normal2 * r2);
31
32        Some(Contact::new(
33            point1,
34            point2,
35            normal1,
36            normal2,
37            ComplexField::sqrt(distance_squared) - sum_radius,
38        ))
39    } else {
40        None
41    }
42}
parry3d/query/contact/contact_ball_ball.rs

parry3d/query/contact/
contact_ball_ball.rs
