parry3d/query/sweep_toi/
sweep.rs

1use crate::math::{Pose, Real, Rotation, Vector};
2
3/// Rotates a vector by a rotation.
4#[inline]
5pub(crate) fn rotate_vec(q: &Rotation, v: Vector) -> Vector {
6    #[cfg(feature = "dim2")]
7    {
8        q.transform_vector(v)
9    }
10    #[cfg(feature = "dim3")]
11    {
12        *q * v
13    }
14}
15
16/// Rotates a vector by the inverse of a rotation.
17#[inline]
18pub(crate) fn inv_rotate_vec(q: &Rotation, v: Vector) -> Vector {
19    #[cfg(feature = "dim2")]
20    {
21        q.inverse_transform_vector(v)
22    }
23    #[cfg(feature = "dim3")]
24    {
25        q.inverse() * v
26    }
27}
28
29/// Normalized linear interpolation between two rotations (3D: shortest arc).
30#[inline]
31pub(crate) fn nlerp(q1: &Rotation, q2: &Rotation, t: Real) -> Rotation {
32    #[cfg(feature = "dim2")]
33    {
34        q1.lerp(*q2, t).normalize()
35    }
36    #[cfg(feature = "dim3")]
37    {
38        let q1 = if q1.dot(*q2) < 0.0 { -*q1 } else { *q1 };
39        (q1 * (1.0 - t) + *q2 * t).normalize()
40    }
41}
42
43/// Describes the motion of a rigid body over a timestep as linear interpolation between two
44/// endpoint poses: the center of mass moves on a straight line while the rotation is
45/// interpolated with a normalized lerp (nlerp).
46///
47/// This is a common motion model for continuous collision detection. It is exact
48/// at both endpoints and a good approximation in between as long as the rotation delta stays
49/// below ~45°.
50#[derive(Copy, Clone, Debug, PartialEq)]
51#[cfg_attr(
52    feature = "serde-serialize",
53    derive(serde::Serialize, serde::Deserialize)
54)]
55pub struct Sweep {
56    /// The center of mass expressed in the shape’s local frame.
57    pub local_center: Vector,
58    /// The world-space center of mass at the start of the sweep.
59    pub c1: Vector,
60    /// The world-space center of mass at the end of the sweep.
61    pub c2: Vector,
62    /// The rotation at the start of the sweep.
63    pub q1: Rotation,
64    /// The rotation at the end of the sweep.
65    pub q2: Rotation,
66}
67
68impl Sweep {
69    /// Builds a sweep from the start and end poses of a shape’s local frame.
70    pub fn from_poses(start: &Pose, end: &Pose, local_center: Vector) -> Self {
71        Self {
72            local_center,
73            c1: start.transform_point(local_center),
74            c2: end.transform_point(local_center),
75            q1: start.rotation,
76            q2: end.rotation,
77        }
78    }
79
80    /// A degenerate sweep holding the shape stationary at the given pose.
81    pub fn constant(pose: &Pose, local_center: Vector) -> Self {
82        Self::from_poses(pose, pose, local_center)
83    }
84
85    /// The pose of the shape’s local frame at time `t ∈ [0, 1]`.
86    ///
87    /// The center of mass is lerped, the rotation is nlerped, and the local-frame origin is
88    /// recovered by un-shifting the local center.
89    pub fn transform_at(&self, t: Real) -> Pose {
90        let q = nlerp(&self.q1, &self.q2, t);
91        let p = self.c1.lerp(self.c2, t) - rotate_vec(&q, self.local_center);
92        Pose::from_parts(p, q)
93    }
94
95    /// The pose of the shape’s local frame at the end of the sweep (`t = 1`), computed exactly.
96    pub fn final_transform(&self) -> Pose {
97        let p = self.c2 - rotate_vec(&self.q2, self.local_center);
98        Pose::from_parts(p, self.q2)
99    }
100
101    /// Translates the entire sweep by `-origin`.
102    ///
103    /// Used to re-center the time-of-impact computation for better floating-point accuracy.
104    pub fn shifted(&self, origin: Vector) -> Self {
105        Self {
106            local_center: self.local_center,
107            c1: self.c1 - origin,
108            c2: self.c2 - origin,
109            q1: self.q1,
110            q2: self.q2,
111        }
112    }
113}
parry3d/query/sweep_toi/sweep.rs

parry3d/query/sweep_toi/
sweep.rs
