ncollide3d/interpolation/
rigid_motion.rs

1use na::RealField;
2
3use crate::math::{Isometry, Point, Translation, Vector};
4
5/// A continuous rigid motion.
6///
7/// This is a function, assumed to be continuous, that, given a parameter `t` returns a direct isometry.
8/// Mathematically speaking this is a one-parameter curve on the space of direct isometries. This curve
9/// should have a continuity of at least `C0`.
10pub trait RigidMotion<N: RealField + Copy> {
11    /// Get a position at the time `t`.
12    fn position_at_time(&self, t: N) -> Isometry<N>;
13}
14
15impl<N: RealField + Copy> RigidMotion<N> for Isometry<N> {
16    fn position_at_time(&self, _: N) -> Isometry<N> {
17        *self
18    }
19}
20
21/// Interpolation between two isometries using LERP for the translation part and SLERP for the rotation.
22pub struct InterpolatedRigidMotion<N: RealField + Copy> {
23    /// The transformation at `t = 0.0`.
24    pub start: Isometry<N>,
25    /// The transformation at `t = 1.0`.
26    pub end: Isometry<N>,
27}
28
29impl<N: RealField + Copy> InterpolatedRigidMotion<N> {
30    /// Initialize a lerp-slerp interpolation with the given start and end transformations.
31    ///
32    /// The `start` is the transformation at the time `t = 0.0` and `end` is the transformation at
33    /// the time `t = 1.0`.
34    pub fn new(start: Isometry<N>, end: Isometry<N>) -> Self {
35        InterpolatedRigidMotion { start, end }
36    }
37}
38
39impl<N: RealField + Copy> RigidMotion<N> for InterpolatedRigidMotion<N> {
40    fn position_at_time(&self, t: N) -> Isometry<N> {
41        self.start.lerp_slerp(&self.end, t)
42    }
43}
44
45/// A linear motion from a starting isometry traveling at constant translational velocity.
46pub struct ConstantLinearVelocityRigidMotion<N: RealField + Copy> {
47    /// The time at which this parametrization begins. Can be negative.
48    pub t0: N,
49    /// The starting isometry at `t = self.t0`.
50    pub start: Isometry<N>,
51    /// The translational velocity of this motion.
52    pub velocity: Vector<N>,
53}
54
55impl<N: RealField + Copy> ConstantLinearVelocityRigidMotion<N> {
56    /// Initialize a linear motion from a starting isometry and a translational velocity.
57    pub fn new(t0: N, start: Isometry<N>, velocity: Vector<N>) -> Self {
58        ConstantLinearVelocityRigidMotion {
59            t0,
60            start,
61            velocity,
62        }
63    }
64}
65
66impl<N: RealField + Copy> RigidMotion<N> for ConstantLinearVelocityRigidMotion<N> {
67    fn position_at_time(&self, t: N) -> Isometry<N> {
68        Isometry::from_parts(
69            (self.start.translation.vector + self.velocity * (t - self.t0)).into(),
70            self.start.rotation,
71        )
72    }
73}
74
75/// A linear motion from a starting isometry traveling at constant translational velocity.
76#[derive(Debug)]
77pub struct ConstantVelocityRigidMotion<N: RealField + Copy> {
78    /// The time at which this parametrization begins. Can be negative.
79    pub t0: N,
80    /// The starting isometry at `t = self.t0`.
81    pub start: Isometry<N>,
82    /// The local-space point at which the rotational part of this motion is applied.
83    pub local_center: Point<N>,
84    /// The translational velocity of this motion.
85    pub linvel: Vector<N>,
86    /// The angular velocity of this motion.
87    #[cfg(feature = "dim2")]
88    pub angvel: N,
89    /// The angular velocity of this motion.
90    #[cfg(feature = "dim3")]
91    pub angvel: Vector<N>,
92}
93
94impl<N: RealField + Copy> ConstantVelocityRigidMotion<N> {
95    /// Initialize a motion from a starting isometry and linear and angular velocities.
96    #[cfg(feature = "dim2")]
97    pub fn new(
98        t0: N,
99        start: Isometry<N>,
100        local_center: Point<N>,
101        linvel: Vector<N>,
102        angvel: N,
103    ) -> Self {
104        ConstantVelocityRigidMotion {
105            t0,
106            start,
107            local_center,
108            linvel,
109            angvel,
110        }
111    }
112
113    /// Initialize a motion from a starting isometry and linear and angular velocities.
114    #[cfg(feature = "dim3")]
115    pub fn new(
116        t0: N,
117        start: Isometry<N>,
118        local_center: Point<N>,
119        linvel: Vector<N>,
120        angvel: Vector<N>,
121    ) -> Self {
122        ConstantVelocityRigidMotion {
123            t0,
124            start,
125            local_center,
126            linvel,
127            angvel,
128        }
129    }
130}
131
132impl<N: RealField + Copy> RigidMotion<N> for ConstantVelocityRigidMotion<N> {
133    fn position_at_time(&self, t: N) -> Isometry<N> {
134        let scaled_linvel = self.linvel * (t - self.t0);
135        let scaled_angvel = self.angvel * (t - self.t0);
136
137        let center = self.start.rotation * self.local_center.coords;
138        let lhs = self.start.translation * Translation::from(center);
139        let rhs = Translation::from(-center) * self.start.rotation;
140
141        lhs * Isometry::new(scaled_linvel, scaled_angvel) * rhs
142    }
143}
144
145/*
146 * For composition.
147 */
148
149/// Trait for composing some rigid motions.
150pub trait RigidMotionComposition<N: RealField + Copy>: RigidMotion<N> {
151    /// Prepend a translation to the rigid motion `self`.
152    fn prepend_translation(&self, translation: Vector<N>) -> PrependTranslation<N, Self> {
153        PrependTranslation {
154            motion: self,
155            translation,
156        }
157    }
158
159    /// Prepend a transformation to the rigid motion `self`.
160    fn prepend_transformation(
161        &self,
162        transformation: Isometry<N>,
163    ) -> PrependTransformation<N, Self> {
164        PrependTransformation {
165            motion: self,
166            transformation,
167        }
168    }
169}
170
171impl<N: RealField + Copy, M: ?Sized + RigidMotion<N>> RigidMotionComposition<N> for M {}
172
173/// The result of prepending a translation to a rigid motion.
174pub struct PrependTranslation<'a, N: RealField + Copy, M: ?Sized> {
175    motion: &'a M,
176    translation: Vector<N>,
177}
178
179impl<'a, N: RealField + Copy, M: ?Sized + RigidMotion<N>> RigidMotion<N>
180    for PrependTranslation<'a, N, M>
181{
182    fn position_at_time(&self, t: N) -> Isometry<N> {
183        let m = self.motion.position_at_time(t);
184        m * Translation::from(self.translation)
185    }
186}
187
188/// The result of prepending an isometric transformation to a rigid motion.
189pub struct PrependTransformation<'a, N: RealField + Copy, M: ?Sized> {
190    motion: &'a M,
191    transformation: Isometry<N>,
192}
193
194impl<'a, N: RealField + Copy, M: ?Sized + RigidMotion<N>> RigidMotion<N>
195    for PrependTransformation<'a, N, M>
196{
197    fn position_at_time(&self, t: N) -> Isometry<N> {
198        let m = self.motion.position_at_time(t);
199        m * self.transformation
200    }
201}
ncollide3d/interpolation/rigid_motion.rs

ncollide3d/interpolation/
rigid_motion.rs
