parry3d/query/shape_cast/
shape_cast_ball_ball.rs

1use na::Unit;
2
3use crate::math::{Isometry, Point, Real, Vector};
4use crate::query::details::ShapeCastOptions;
5use crate::query::{self, Ray, ShapeCastHit, ShapeCastStatus};
6use crate::shape::Ball;
7use num::Zero;
8
9/// Time Of Impact of two balls under translational movement.
10#[inline]
11pub fn cast_shapes_ball_ball(
12    pos12: &Isometry<Real>,
13    vel12: &Vector<Real>,
14    b1: &Ball,
15    b2: &Ball,
16    options: ShapeCastOptions,
17) -> Option<ShapeCastHit> {
18    let rsum = b1.radius + b2.radius + options.target_distance;
19    let radius = rsum;
20    let center = Point::from(-pos12.translation.vector);
21    let ray = Ray::new(Point::origin(), *vel12);
22
23    if let (inside, Some(time_of_impact)) =
24        query::details::ray_toi_with_ball(&center, radius, &ray, true)
25    {
26        if time_of_impact > options.max_time_of_impact {
27            return None;
28        }
29
30        let dpt = ray.point_at(time_of_impact) - center;
31        let normal1;
32        let normal2;
33        let witness1;
34        let witness2;
35
36        if radius.is_zero() {
37            normal1 = Vector::x_axis();
38            normal2 = pos12.inverse_transform_unit_vector(&(-Vector::x_axis()));
39            witness1 = Point::origin();
40            witness2 = Point::origin();
41        } else {
42            normal1 = Unit::new_unchecked(dpt / radius);
43            normal2 = pos12.inverse_transform_unit_vector(&(-normal1));
44            witness1 = Point::from(*normal1 * b1.radius);
45            witness2 = Point::from(*normal2 * b2.radius);
46        }
47
48        if !options.stop_at_penetration && time_of_impact < 1.0e-5 && normal1.dot(vel12) >= 0.0 {
49            return None;
50        }
51
52        let status = if inside && center.coords.norm_squared() < rsum * rsum {
53            ShapeCastStatus::PenetratingOrWithinTargetDist
54        } else {
55            ShapeCastStatus::Converged
56        };
57
58        Some(ShapeCastHit {
59            time_of_impact,
60            normal1,
61            normal2,
62            witness1,
63            witness2,
64            status,
65        })
66    } else {
67        None
68    }
69}
parry3d/query/shape_cast/shape_cast_ball_ball.rs

parry3d/query/shape_cast/
shape_cast_ball_ball.rs
