parry3d/query/nonlinear_shape_cast/
nonlinear_rigid_motion.rs

1use crate::math::{Isometry, Point, Real, Translation, Vector};
2
3/// A nonlinear motion from a starting isometry traveling at constant translational and rotational velocity.
4#[derive(Debug, Copy, Clone)]
5pub struct NonlinearRigidMotion {
6    /// The starting isometry at `t = 0`.
7    pub start: Isometry<Real>,
8    /// The local-space point at which the rotational part of this motion is applied.
9    pub local_center: Point<Real>,
10    /// The translational velocity of this motion.
11    pub linvel: Vector<Real>,
12    /// The angular velocity of this motion.
13    #[cfg(feature = "dim2")]
14    pub angvel: Real,
15    /// The angular velocity of this motion.
16    #[cfg(feature = "dim3")]
17    pub angvel: Vector<Real>,
18}
19
20impl NonlinearRigidMotion {
21    /// Initialize a motion from a starting isometry and linear and angular velocities.
22    #[cfg(feature = "dim2")]
23    pub fn new(
24        start: Isometry<Real>,
25        local_center: Point<Real>,
26        linvel: Vector<Real>,
27        angvel: Real,
28    ) -> Self {
29        NonlinearRigidMotion {
30            start,
31            local_center,
32            linvel,
33            angvel,
34        }
35    }
36
37    /// Initialize a motion from a starting isometry and linear and angular velocities.
38    #[cfg(feature = "dim3")]
39    pub fn new(
40        start: Isometry<Real>,
41        local_center: Point<Real>,
42        linvel: Vector<Real>,
43        angvel: Vector<Real>,
44    ) -> Self {
45        NonlinearRigidMotion {
46            start,
47            local_center,
48            linvel,
49            angvel,
50        }
51    }
52
53    /// Create a `NonlinearRigidMotion` that always returns the identity matrix.
54    pub fn identity() -> Self {
55        Self::constant_position(Isometry::identity())
56    }
57
58    /// Create a `NonlinearRigidMotion` that always return `pos`.
59    pub fn constant_position(pos: Isometry<Real>) -> Self {
60        Self {
61            start: pos,
62            linvel: na::zero(),
63            angvel: na::zero(),
64            local_center: Point::origin(),
65        }
66    }
67
68    fn set_start(&mut self, new_start: Isometry<Real>) {
69        // NOTE: we need to adjust the local_center so that the angular
70        // velocity is still expressed wrt. the original center.
71        self.local_center = new_start.inverse_transform_point(&(self.start * self.local_center));
72        self.start = new_start;
73    }
74
75    /// Freezes this motion at the time `t`.
76    ///
77    /// After calling this, any further calls to `self.position_at_time`
78    /// will always return `self.position_at_time(t)` (where `t` is the value given
79    /// to this method). This sets the linear velocity and angular velocity
80    /// of `self` to zero.
81    pub fn freeze(&mut self, t: Real) {
82        self.start = self.position_at_time(t);
83        self.linvel = na::zero();
84        self.angvel = na::zero();
85    }
86
87    /// Appends a constant translation to this rigid-motion.
88    #[must_use]
89    pub fn append_translation(&self, tra: Vector<Real>) -> Self {
90        let mut result = *self;
91        result.set_start(Translation::from(tra) * result.start);
92        result
93    }
94
95    /// Prepends a constant translation to this rigid-motion.
96    #[must_use]
97    pub fn prepend_translation(&self, tra: Vector<Real>) -> Self {
98        let mut result = *self;
99        result.set_start(result.start * Translation::from(tra));
100        result
101    }
102
103    /// Appends a constant isometry to this rigid-motion.
104    #[must_use]
105    pub fn append(&self, iso: Isometry<Real>) -> Self {
106        let mut result = *self;
107        result.set_start(iso * result.start);
108        result
109    }
110
111    /// Prepends a constant translation to this rigid-motion.
112    #[must_use]
113    pub fn prepend(&self, iso: Isometry<Real>) -> Self {
114        let mut result = *self;
115        result.set_start(result.start * iso);
116        result
117    }
118
119    /// Computes the position at time `t` of a rigid-body following the motion described by `self`.
120    pub fn position_at_time(&self, t: Real) -> Isometry<Real> {
121        let center = self.start * self.local_center;
122        let shift = Translation::from(center.coords);
123        (shift * Isometry::new(self.linvel * t, self.angvel * t)) * (shift.inverse() * self.start)
124    }
125}
parry3d/query/nonlinear_shape_cast/nonlinear_rigid_motion.rs

parry3d/query/nonlinear_shape_cast/
nonlinear_rigid_motion.rs
