parry3d/query/nonlinear_shape_cast/
nonlinear_rigid_motion.rs

1use crate::math::{Pose, Real, Vector};
2
3/// Describes the complete motion of a rigid body with both translation and rotation.
4///
5/// This type represents the **6 degrees of freedom** motion of a rigid body:
6/// - **3 translational** (linear velocity in x, y, z)
7/// - **3 rotational** (angular velocity around x, y, z axes)
8///
9/// The motion is assumed to have **constant velocities** over the time interval, meaning:
10/// - Linear velocity doesn't change (no acceleration)
11/// - Angular velocity doesn't change (no angular acceleration)
12///
13/// This is used with [`cast_shapes_nonlinear`](crate::query::cast_shapes_nonlinear) to perform
14/// continuous collision detection for rotating objects.
15///
16/// # Physics Model
17///
18/// At any time `t`, the object's position is computed as:
19/// 1. Rotate around `local_center` by `angvel * t`
20/// 2. Translate by `linvel * t`
21/// 3. Apply to the starting position `start`
22///
23/// This creates a **helical trajectory** (螺旋軌跡) when both velocities are non-zero:
24/// - Pure translation: straight line
25/// - Pure rotation: circular arc
26/// - Both: helix/spiral path
27///
28/// # Fields
29///
30/// * `start` - The initial position and orientation at time `t = 0`
31/// * `local_center` - The point (in the shape's local coordinate system) around which
32///   rotation occurs. For most cases, use the center of mass or `Vector::ZERO`.
33/// * `linvel` - Linear velocity vector (units per second). Direction is the direction
34///   of motion, magnitude is speed.
35/// * `angvel` - Angular velocity:
36///   - **2D**: Scalar rotation rate in radians/second (positive = counter-clockwise)
37///   - **3D**: Axis-angle vector (direction = rotation axis, magnitude = rotation rate in radians/second)
38///
39/// # Example: Simple Translation
40///
41/// ```rust
42/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
43/// use parry3d::query::NonlinearRigidMotion;
44/// use parry3d::math::{Pose, Vector};
45///
46/// // Object moving right at 5 units/second, no rotation
47/// let motion = NonlinearRigidMotion::new(
48///     Pose::translation(0.0, 0.0, 0.0), // start at origin
49///     Vector::ZERO,                      // rotation center (irrelevant here)
50///     Vector::new(5.0, 0.0, 0.0),          // moving right
51///     Vector::ZERO,                      // not rotating
52/// );
53///
54/// // At t=2.0 seconds, object has moved 10 units right
55/// let pos_at_2 = motion.position_at_time(2.0);
56/// assert_eq!(pos_at_2.translation.x, 10.0);
57/// # }
58/// ```
59///
60/// # Example: Pure Rotation (3D)
61///
62/// ```rust
63/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
64/// use parry3d::query::NonlinearRigidMotion;
65/// use parry3d::math::{Pose, Vector};
66/// use std::f32::consts::PI;
67///
68/// // Object spinning around Y axis, no translation
69/// let motion = NonlinearRigidMotion::new(
70///     Pose::translation(0.0, 0.0, 0.0),
71///     Vector::ZERO,                // rotate around origin
72///     Vector::ZERO,                // not translating
73///     Vector::new(0.0, PI, 0.0),      // rotating around Y at π rad/s (180°/s)
74/// );
75///
76/// // At t=1.0 second, object has rotated 180 degrees
77/// let pos_at_1 = motion.position_at_time(1.0);
78/// // Object is now rotated 180° around Y axis
79/// # }
80/// ```
81///
82/// # Example: Combined Translation and Rotation (Helical Path)
83///
84/// ```rust
85/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
86/// use parry3d::query::NonlinearRigidMotion;
87/// use parry3d::math::{Pose, Vector};
88///
89/// // Spinning projectile moving forward
90/// let motion = NonlinearRigidMotion::new(
91///     Pose::translation(0.0, 0.0, 0.0),
92///     Vector::ZERO,
93///     Vector::new(10.0, 0.0, 0.0),     // moving forward at 10 units/s
94///     Vector::new(20.0, 0.0, 0.0),     // spinning around its movement axis
95/// );
96///
97/// // The object traces a helical path (like a bullet with rifling)
98/// let pos_at_half = motion.position_at_time(0.5);
99/// // Moved 5 units forward AND rotated 10 radians
100/// # }
101/// ```
102///
103/// # Example: Rotation Around Off-Center Vector
104///
105/// ```rust
106/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
107/// use parry3d::query::NonlinearRigidMotion;
108/// use parry3d::math::{Pose, Vector};
109///
110/// // Object rotating around a point that's NOT its center
111/// // Useful for: swinging weapons, rotating around pivot point, etc.
112/// let motion = NonlinearRigidMotion::new(
113///     Pose::translation(5.0, 0.0, 0.0), // object is at x=5
114///     Vector::new(-5.0, 0.0, 0.0),           // rotate around x=0 (in local space)
115///     Vector::ZERO,
116///     Vector::new(0.0, 1.0, 0.0),           // rotate around Y axis at 1 rad/s
117/// );
118///
119/// // The object orbits in a circle around the world origin
120/// // Like a hammer being swung around
121/// # }
122/// ```
123///
124/// # Common Use Cases
125///
126/// 1. **Spinning Projectiles**: Bullets, thrown objects with spin
127///
128/// ```rust
129/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
130/// # use parry3d::query::NonlinearRigidMotion;
131/// # use parry3d::math::{Pose, Vector};
132/// let bullet = NonlinearRigidMotion::new(
133///     Pose::translation(0.0, 1.5, 0.0),
134///     Vector::ZERO,
135///     Vector::new(100.0, -2.0, 0.0),  // fast forward, slight drop
136///     Vector::new(50.0, 0.0, 0.0),    // high spin rate
137/// );
138/// # }
139/// ```
140///
141/// 2. **Tumbling Debris**: Objects affected by explosion or impact
142///
143/// ```rust
144/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
145/// # use parry3d::query::NonlinearRigidMotion;
146/// # use parry3d::math::{Pose, Vector};
147/// let debris = NonlinearRigidMotion::new(
148///     Pose::translation(0.0, 2.0, 0.0),
149///     Vector::ZERO,
150///     Vector::new(3.0, 5.0, -2.0),    // chaotic velocity
151///     Vector::new(2.0, -3.0, 1.5),    // chaotic rotation
152/// );
153/// # }
154/// ```
155///
156/// 3. **Rotating Machinery**: Blades, gears, rotating parts
157///
158/// ```rust
159/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
160/// # use parry3d::query::NonlinearRigidMotion;
161/// # use parry3d::math::{Pose, Vector};
162/// let blade = NonlinearRigidMotion::new(
163///     Pose::translation(0.0, 1.0, 0.0),
164///     Vector::ZERO,           // spin around center
165///     Vector::ZERO,           // blade doesn't translate
166///     Vector::new(0.0, 10.0, 0.0), // fast rotation
167/// );
168/// # }
169/// ```
170///
171/// 4. **Stationary Objects**: Use `constant_position()` for non-moving obstacles
172///
173/// ```rust
174/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
175/// # use parry3d::query::NonlinearRigidMotion;
176/// # use parry3d::math::Pose;
177/// let wall = NonlinearRigidMotion::constant_position(
178///     Pose::translation(10.0, 0.0, 0.0)
179/// );
180/// # }
181/// ```
182///
183/// # 2D vs 3D Angular Velocity
184///
185/// The representation of `angvel` differs between 2D and 3D:
186///
187/// **2D (scalar)**:
188/// - Positive value: counter-clockwise rotation
189/// - Negative value: clockwise rotation
190/// - Magnitude: rotation rate in radians/second
191///
192/// ```rust
193/// # #[cfg(all(feature = "dim2", feature = "f32"))] {
194/// use parry2d::query::NonlinearRigidMotion;
195/// use parry2d::math::{Pose, Vector};
196///
197/// let motion = NonlinearRigidMotion::new(
198///     Pose::translation(0.0, 0.0),
199///     Vector::ZERO,
200///     Vector::ZERO,
201///     3.14,  // rotating counter-clockwise at π rad/s
202/// );
203/// # }
204/// ```
205///
206/// **3D (axis-angle vector)**:
207/// - Direction: axis of rotation (right-hand rule)
208/// - Magnitude: rotation rate in radians/second
209/// - Zero vector: no rotation
210///
211/// ```rust
212/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
213/// use parry3d::query::NonlinearRigidMotion;
214/// use parry3d::math::{Pose, Vector};
215///
216/// let motion = NonlinearRigidMotion::new(
217///     Pose::translation(0.0, 0.0, 0.0),
218///     Vector::ZERO,
219///     Vector::ZERO,
220///     Vector::new(0.0, 3.14, 0.0),  // rotate around Y axis at π rad/s
221/// );
222/// # }
223/// ```
224///
225/// # Important Notes
226///
227/// 1. **Local vs World Space**: The `local_center` must be in the **shape's local
228///    coordinate system**, not world space. For a shape centered at origin,
229///    use `Vector::ZERO`.
230///
231/// 2. **Angular Velocity Units**: Always use **radians per second**, not degrees!
232///    - To convert: `degrees * (PI / 180.0) = radians`
233///    - Example: 90°/s = `90.0 * (PI / 180.0)` ≈ 1.571 rad/s
234///
235/// 3. **Constant Velocities**: This assumes velocities don't change over time.
236///    For physics simulations with acceleration/forces, you typically:
237///    - Compute motion for a single small timestep (e.g., 1/60 second)
238///    - Update velocities after each step
239///    - Create new `NonlinearRigidMotion` for next timestep
240///
241/// 4. **Center of Mass**: For realistic physics, `local_center` should be the
242///    shape's center of mass. For simple cases, `Vector::ZERO` often works.
243///
244/// # See Also
245///
246/// - [`cast_shapes_nonlinear`](crate::query::cast_shapes_nonlinear) - Uses this type for collision detection
247/// - [`cast_shapes`](crate::query::cast_shapes) - Linear motion (no rotation)
248/// - [`position_at_time`](Self::position_at_time) - Compute position at any time
249#[derive(Debug, Copy, Clone)]
250pub struct NonlinearRigidMotion {
251    /// The starting isometry at `t = 0`.
252    pub start: Pose,
253    /// The local-space point at which the rotational part of this motion is applied.
254    pub local_center: Vector,
255    /// The translational velocity of this motion.
256    pub linvel: Vector,
257    /// The angular velocity of this motion.
258    #[cfg(feature = "dim2")]
259    pub angvel: Real,
260    /// The angular velocity of this motion.
261    #[cfg(feature = "dim3")]
262    pub angvel: Vector,
263}
264
265impl NonlinearRigidMotion {
266    /// Creates a new rigid motion from a starting position and velocities.
267    ///
268    /// # Arguments
269    ///
270    /// * `start` - Initial position and orientation at time `t = 0`
271    /// * `local_center` - Vector (in local coordinates) around which rotation occurs
272    /// * `linvel` - Linear velocity vector (units per second)
273    /// * `angvel` - Angular velocity (2D: radians/sec scalar, 3D: axis-angle vector)
274    ///
275    /// # Example (2D)
276    ///
277    /// ```
278    /// # #[cfg(all(feature = "dim2", feature = "f32"))] {
279    /// use parry2d::query::NonlinearRigidMotion;
280    /// use parry2d::math::{Pose, Vector};
281    /// use std::f32::consts::PI;
282    ///
283    /// // Object moving right and rotating counter-clockwise
284    /// let motion = NonlinearRigidMotion::new(
285    ///     Pose::translation(0.0, 0.0),
286    ///     Vector::ZERO,
287    ///     Vector::new(5.0, 0.0),  // 5 units/sec to the right
288    ///     PI,                       // π rad/sec counter-clockwise
289    /// );
290    /// # }
291    /// ```
292    ///
293    /// # Example (3D)
294    ///
295    /// ```
296    /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
297    /// use parry3d::query::NonlinearRigidMotion;
298    /// use parry3d::math::{Pose, Vector};
299    ///
300    /// // Object moving forward and spinning around its movement axis
301    /// let motion = NonlinearRigidMotion::new(
302    ///     Pose::translation(0.0, 0.0, 0.0),
303    ///     Vector::ZERO,
304    ///     Vector::new(10.0, 0.0, 0.0),    // moving forward
305    ///     Vector::new(5.0, 0.0, 0.0),     // spinning around X axis
306    /// );
307    /// # }
308    /// ```
309    #[cfg(feature = "dim2")]
310    pub fn new(start: Pose, local_center: Vector, linvel: Vector, angvel: Real) -> Self {
311        NonlinearRigidMotion {
312            start,
313            local_center,
314            linvel,
315            angvel,
316        }
317    }
318
319    /// Creates a new rigid motion from a starting position and velocities.
320    ///
321    /// # Arguments
322    ///
323    /// * `start` - Initial position and orientation at time `t = 0`
324    /// * `local_center` - Vector (in local coordinates) around which rotation occurs
325    /// * `linvel` - Linear velocity vector (units per second)
326    /// * `angvel` - Angular velocity as axis-angle vector (radians per second)
327    ///
328    /// # Example
329    ///
330    /// ```
331    /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
332    /// use parry3d::query::NonlinearRigidMotion;
333    /// use parry3d::math::{Pose, Vector};
334    ///
335    /// // Object moving forward and spinning around its movement axis
336    /// let motion = NonlinearRigidMotion::new(
337    ///     Pose::translation(0.0, 0.0, 0.0),
338    ///     Vector::ZERO,
339    ///     Vector::new(10.0, 0.0, 0.0),    // moving forward
340    ///     Vector::new(5.0, 0.0, 0.0),     // spinning around X axis
341    /// );
342    /// # }
343    /// ```
344    #[cfg(feature = "dim3")]
345    pub fn new(start: Pose, local_center: Vector, linvel: Vector, angvel: Vector) -> Self {
346        NonlinearRigidMotion {
347            start,
348            local_center,
349            linvel,
350            angvel,
351        }
352    }
353
354    /// Creates a stationary motion at the origin (identity transformation).
355    ///
356    /// Equivalent to `constant_position(Pose::identity())`.
357    ///
358    /// # Example
359    ///
360    /// ```
361    /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
362    /// use parry3d::query::NonlinearRigidMotion;
363    ///
364    /// let stationary = NonlinearRigidMotion::identity();
365    /// // Object stays at origin with no rotation, forever
366    /// # }
367    /// ```
368    pub fn identity() -> Self {
369        Self::constant_position(Pose::IDENTITY)
370    }
371
372    /// Creates a motion that stays at a constant position (no translation, no rotation).
373    ///
374    /// Useful for representing static/stationary objects in collision queries.
375    /// Both linear and angular velocities are set to zero.
376    ///
377    /// # Arguments
378    ///
379    /// * `pos` - The fixed position and orientation
380    ///
381    /// # Example
382    ///
383    /// ```
384    /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
385    /// use parry3d::query::NonlinearRigidMotion;
386    /// use parry3d::math::Pose;
387    ///
388    /// // A wall that never moves
389    /// let wall_motion = NonlinearRigidMotion::constant_position(
390    ///     Pose::translation(10.0, 0.0, 0.0)
391    /// );
392    ///
393    /// // At any time t, position is always (10, 0, 0)
394    /// let pos_at_5 = wall_motion.position_at_time(5.0);
395    /// assert_eq!(pos_at_5.translation.x, 10.0);
396    /// # }
397    /// ```
398    pub fn constant_position(pos: Pose) -> Self {
399        Self {
400            start: pos,
401            linvel: Vector::ZERO,
402            #[cfg(feature = "dim2")]
403            angvel: 0.0,
404            #[cfg(feature = "dim3")]
405            angvel: Vector::ZERO,
406            local_center: Vector::ZERO,
407        }
408    }
409
410    fn set_start(&mut self, new_start: Pose) {
411        // NOTE: we need to adjust the local_center so that the angular
412        // velocity is still expressed wrt. the original center.
413        self.local_center = new_start.inverse_transform_point(self.start * self.local_center);
414        self.start = new_start;
415    }
416
417    /// Freezes this motion at a specific time, making it stationary from that point forward.
418    ///
419    /// After calling this method:
420    /// - `self.start` is updated to the position at time `t`
421    /// - Both velocities are set to zero
422    /// - All future calls to `position_at_time()` return the same frozen position
423    ///
424    /// This is useful for "stopping" an object mid-motion, or capturing a specific
425    /// moment in a motion trajectory.
426    ///
427    /// # Arguments
428    ///
429    /// * `t` - The time at which to freeze the motion
430    ///
431    /// # Example
432    ///
433    /// ```
434    /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
435    /// use parry3d::query::NonlinearRigidMotion;
436    /// use parry3d::math::{Pose, Vector};
437    ///
438    /// let mut motion = NonlinearRigidMotion::new(
439    ///     Pose::translation(0.0, 0.0, 0.0),
440    ///     Vector::ZERO,
441    ///     Vector::new(5.0, 0.0, 0.0),  // moving right
442    ///     Vector::ZERO,
443    /// );
444    ///
445    /// // Freeze at t=2.0 (when object is at x=10)
446    /// motion.freeze(2.0);
447    ///
448    /// // Now position is constant at x=10, regardless of time
449    /// let pos_at_100 = motion.position_at_time(100.0);
450    /// assert_eq!(pos_at_100.translation.x, 10.0);
451    /// # }
452    /// ```
453    pub fn freeze(&mut self, t: Real) {
454        self.start = self.position_at_time(t);
455        self.linvel = Vector::ZERO;
456        #[cfg(feature = "dim2")]
457        {
458            self.angvel = 0.0;
459        }
460        #[cfg(feature = "dim3")]
461        {
462            self.angvel = Vector::ZERO;
463        }
464    }
465
466    /// Appends a constant translation to this rigid-motion.
467    #[must_use]
468    pub fn append_translation(&self, tra: Vector) -> Self {
469        let mut result = *self;
470        result.set_start(Pose::from_translation(tra) * result.start);
471        result
472    }
473
474    /// Prepends a constant translation to this rigid-motion.
475    #[must_use]
476    pub fn prepend_translation(&self, tra: Vector) -> Self {
477        let mut result = *self;
478        result.set_start(result.start * Pose::from_translation(tra));
479        result
480    }
481
482    /// Appends a constant isometry to this rigid-motion.
483    #[must_use]
484    pub fn append(&self, iso: Pose) -> Self {
485        let mut result = *self;
486        result.set_start(iso * result.start);
487        result
488    }
489
490    /// Prepends a constant translation to this rigid-motion.
491    #[must_use]
492    pub fn prepend(&self, iso: Pose) -> Self {
493        let mut result = *self;
494        result.set_start(result.start * iso);
495        result
496    }
497
498    /// Computes the position and orientation at a given time.
499    ///
500    /// Returns the full isometry (position + orientation) of the rigid body at time `t`,
501    /// accounting for both translation and rotation from the starting position.
502    ///
503    /// The computation follows this sequence:
504    /// 1. Rotate around `local_center` by angle `angvel * t`
505    /// 2. Translate by displacement `linvel * t`
506    /// 3. Apply to the starting position `start`
507    ///
508    /// # Arguments
509    ///
510    /// * `t` - Time value (typically in seconds, matching your velocity units)
511    ///
512    /// # Returns
513    ///
514    /// The complete transformation (position and orientation) at time `t`.
515    ///
516    /// # Example: Tracking a Moving Object
517    ///
518    /// ```
519    /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
520    /// use parry3d::query::NonlinearRigidMotion;
521    /// use parry3d::math::{Pose, Vector};
522    ///
523    /// let motion = NonlinearRigidMotion::new(
524    ///     Pose::translation(0.0, 0.0, 0.0),
525    ///     Vector::ZERO,
526    ///     Vector::new(3.0, 0.0, 0.0),     // 3 units/sec to the right
527    ///     Vector::new(0.0, 1.0, 0.0),     // 1 radian/sec around Y axis
528    /// );
529    ///
530    /// // Position at t=0
531    /// let pos_0 = motion.position_at_time(0.0);
532    /// assert_eq!(pos_0.translation.x, 0.0);
533    ///
534    /// // Position at t=2.0 seconds
535    /// let pos_2 = motion.position_at_time(2.0);
536    /// // Object has moved 6 units to the right
537    /// assert!((pos_2.translation.x - 6.0).abs() < 0.01);
538    /// // And rotated 2 radians around Y
539    /// # }
540    /// ```
541    ///
542    /// # Example: Animation Frame
543    ///
544    /// ```
545    /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
546    /// use parry3d::query::NonlinearRigidMotion;
547    /// use parry3d::math::{Pose, Vector};
548    ///
549    /// let motion = NonlinearRigidMotion::new(
550    ///     Pose::translation(0.0, 5.0, 0.0),
551    ///     Vector::ZERO,
552    ///     Vector::new(0.0, -9.8, 0.0),    // falling (gravity)
553    ///     Vector::new(1.0, 2.0, 0.5),     // tumbling
554    /// );
555    ///
556    /// // Render at 60 FPS
557    /// let dt = 1.0 / 60.0;
558    /// for frame in 0..60 {
559    ///     let t = frame as f32 * dt;
560    ///     let pos = motion.position_at_time(t);
561    ///     // Use `pos` to render object at this frame
562    /// }
563    /// # }
564    /// ```
565    pub fn position_at_time(&self, t: Real) -> Pose {
566        let center = self.start * self.local_center;
567        let shift = Pose::from_translation(center);
568        (shift * Pose::new(self.linvel * t, self.angvel * t)) * (shift.inverse() * self.start)
569    }
570}
parry3d/query/nonlinear_shape_cast/nonlinear_rigid_motion.rs

parry3d/query/nonlinear_shape_cast/
nonlinear_rigid_motion.rs
