parry3d_f64/query/shape_cast/
shape_cast.rs

1use na::Unit;
2
3use crate::math::{Isometry, Point, Real, Vector};
4use crate::query::{DefaultQueryDispatcher, QueryDispatcher, Unsupported};
5use crate::shape::Shape;
6
7/// The status of the time-of-impact computation algorithm.
8#[derive(Copy, Clone, Debug, PartialEq, Eq)]
9pub enum ShapeCastStatus {
10    /// The shape-casting algorithm ran out of iterations before achieving convergence.
11    ///
12    /// The content of the `ShapeCastHit` will still be a conservative approximation of the actual result so
13    /// it is often fine to interpret this case as a success.
14    OutOfIterations,
15    /// The shape-casting algorithm converged successfully.
16    Converged,
17    /// Something went wrong during the shape-casting, likely due to numerical instabilities.
18    ///
19    /// The content of the `ShapeCastHit` will still be a conservative approximation of the actual result so
20    /// it is often fine to interpret this case as a success.
21    Failed,
22    /// The two shape already overlap, or are separated by a distance smaller than
23    /// [`ShapeCastOptions::target_distance`] at the time 0.
24    ///
25    /// The witness points and normals provided by the `ShapeCastHit` will have unreliable values unless
26    /// [`ShapeCastOptions::compute_impact_geometry_on_penetration`] was set to `true` when calling
27    /// the time-of-impact function.
28    PenetratingOrWithinTargetDist,
29}
30
31/// The result of a shape casting..
32#[derive(Copy, Clone, Debug)]
33pub struct ShapeCastHit {
34    /// The time at which the objects touch.
35    pub time_of_impact: Real,
36    /// The local-space closest point on the first shape at the time of impact.
37    ///
38    /// This value is unreliable if `status` is [`ShapeCastStatus::PenetratingOrWithinTargetDist`]
39    /// and [`ShapeCastOptions::compute_impact_geometry_on_penetration`] was set to `false`.
40    pub witness1: Point<Real>,
41    /// The local-space closest point on the second shape at the time of impact.
42    ///
43    /// This value is unreliable if `status` is [`ShapeCastStatus::PenetratingOrWithinTargetDist`]
44    /// and both [`ShapeCastOptions::compute_impact_geometry_on_penetration`] was set to `false`
45    /// when calling the time-of-impact function.
46    pub witness2: Point<Real>,
47    /// The local-space outward normal on the first shape at the time of impact.
48    ///
49    /// This value is unreliable if `status` is [`ShapeCastStatus::PenetratingOrWithinTargetDist`]
50    /// and both [`ShapeCastOptions::compute_impact_geometry_on_penetration`] was set to `false`
51    /// when calling the time-of-impact function.
52    pub normal1: Unit<Vector<Real>>,
53    /// The local-space outward normal on the second shape at the time of impact.
54    ///
55    /// This value is unreliable if `status` is [`ShapeCastStatus::PenetratingOrWithinTargetDist`]
56    /// and both [`ShapeCastOptions::compute_impact_geometry_on_penetration`] was set to `false`
57    /// when calling the time-of-impact function.
58    pub normal2: Unit<Vector<Real>>,
59    /// The way the shape-casting algorithm terminated.
60    pub status: ShapeCastStatus,
61}
62
63impl ShapeCastHit {
64    /// Swaps every data of this shape-casting result such that the role of both shapes are swapped.
65    ///
66    /// In practice, this makes it so that `self.witness1` and `self.normal1` are swapped with
67    /// `self.witness2` and `self.normal2`.
68    pub fn swapped(self) -> Self {
69        Self {
70            time_of_impact: self.time_of_impact,
71            witness1: self.witness2,
72            witness2: self.witness1,
73            normal1: self.normal2,
74            normal2: self.normal1,
75            status: self.status,
76        }
77    }
78
79    /// Transform `self.witness1` and `self.normal1` by `pos`.
80    pub fn transform1_by(&self, pos: &Isometry<Real>) -> Self {
81        Self {
82            time_of_impact: self.time_of_impact,
83            witness1: pos * self.witness1,
84            witness2: self.witness2,
85            normal1: pos * self.normal1,
86            normal2: self.normal2,
87            status: self.status,
88        }
89    }
90}
91
92/// Configuration for controlling the behavior of time-of-impact (i.e. shape-casting) calculations.
93#[derive(Copy, Clone, Debug, PartialEq)]
94pub struct ShapeCastOptions {
95    /// The maximum time-of-impacts that can be computed.
96    ///
97    /// Any impact occurring after this time will be ignored.
98    pub max_time_of_impact: Real,
99    /// The shapes will be considered as impacting as soon as their distance is smaller or
100    /// equal to this target distance. Must be positive or zero.
101    ///
102    /// If the shapes are separated by a distance smaller than `target_distance` at time 0, the
103    /// calculated witness points and normals are only reliable if
104    /// [`Self::compute_impact_geometry_on_penetration`] is set to `true`.
105    pub target_distance: Real,
106    /// If `false`, the time-of-impact algorithm will automatically discard any impact at time
107    /// 0 where the velocity is separating (i.e., the relative velocity is such that the distance
108    /// between the objects projected on the impact normal is increasing through time).
109    pub stop_at_penetration: bool,
110    /// If `true`, witness points and normals will be calculated even when the time-of-impact is 0.
111    pub compute_impact_geometry_on_penetration: bool,
112}
113
114impl ShapeCastOptions {
115    // Constructor for the most common use-case.
116    /// Crates a [`ShapeCastOptions`] with the default values except for the maximum time of impact.
117    pub fn with_max_time_of_impact(max_time_of_impact: Real) -> Self {
118        Self {
119            max_time_of_impact,
120            ..Default::default()
121        }
122    }
123}
124
125impl Default for ShapeCastOptions {
126    fn default() -> Self {
127        Self {
128            max_time_of_impact: Real::MAX,
129            target_distance: 0.0,
130            stop_at_penetration: true,
131            compute_impact_geometry_on_penetration: true,
132        }
133    }
134}
135
136/// Computes the smallest time when two shapes under translational movement are separated by a
137/// distance smaller or equal to `distance`.
138///
139/// Returns `0.0` if the objects are touching or closer than `options.target_distance`,
140/// or penetrating.
141pub fn cast_shapes(
142    pos1: &Isometry<Real>,
143    vel1: &Vector<Real>,
144    g1: &dyn Shape,
145    pos2: &Isometry<Real>,
146    vel2: &Vector<Real>,
147    g2: &dyn Shape,
148    options: ShapeCastOptions,
149) -> Result<Option<ShapeCastHit>, Unsupported> {
150    let pos12 = pos1.inv_mul(pos2);
151    let vel12 = pos1.inverse_transform_vector(&(vel2 - vel1));
152    DefaultQueryDispatcher.cast_shapes(&pos12, &vel12, g1, g2, options)
153}
parry3d_f64/query/shape_cast/shape_cast.rs

parry3d_f64/query/shape_cast/
shape_cast.rs
