parry2d/query/nonlinear_shape_cast/nonlinear_rigid_motion.rs
1use crate::math::{Isometry, Point, Real, Translation, 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 `Point::origin()`.
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 nalgebra::{Isometry3, Point3, Vector3};
45///
46/// // Object moving right at 5 units/second, no rotation
47/// let motion = NonlinearRigidMotion::new(
48/// Isometry3::translation(0.0, 0.0, 0.0), // start at origin
49/// Point3::origin(), // rotation center (irrelevant here)
50/// Vector3::new(5.0, 0.0, 0.0), // moving right
51/// Vector3::zeros(), // 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.vector.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 nalgebra::{Isometry3, Point3, Vector3};
66/// use std::f32::consts::PI;
67///
68/// // Object spinning around Y axis, no translation
69/// let motion = NonlinearRigidMotion::new(
70/// Isometry3::translation(0.0, 0.0, 0.0),
71/// Point3::origin(), // rotate around origin
72/// Vector3::zeros(), // not translating
73/// Vector3::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 nalgebra::{Isometry3, Point3, Vector3};
88///
89/// // Spinning projectile moving forward
90/// let motion = NonlinearRigidMotion::new(
91/// Isometry3::translation(0.0, 0.0, 0.0),
92/// Point3::origin(),
93/// Vector3::new(10.0, 0.0, 0.0), // moving forward at 10 units/s
94/// Vector3::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 Point
104///
105/// ```rust
106/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
107/// use parry3d::query::NonlinearRigidMotion;
108/// use nalgebra::{Isometry3, Point3, Vector3};
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/// Isometry3::translation(5.0, 0.0, 0.0), // object is at x=5
114/// Point3::new(-5.0, 0.0, 0.0), // rotate around x=0 (in local space)
115/// Vector3::zeros(),
116/// Vector3::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 nalgebra::{Isometry3, Point3, Vector3};
132/// let bullet = NonlinearRigidMotion::new(
133/// Isometry3::translation(0.0, 1.5, 0.0),
134/// Point3::origin(),
135/// Vector3::new(100.0, -2.0, 0.0), // fast forward, slight drop
136/// Vector3::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 nalgebra::{Isometry3, Point3, Vector3};
147/// let debris = NonlinearRigidMotion::new(
148/// Isometry3::translation(0.0, 2.0, 0.0),
149/// Point3::origin(),
150/// Vector3::new(3.0, 5.0, -2.0), // chaotic velocity
151/// Vector3::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 nalgebra::{Isometry3, Point3, Vector3};
162/// let blade = NonlinearRigidMotion::new(
163/// Isometry3::translation(0.0, 1.0, 0.0),
164/// Point3::origin(), // spin around center
165/// Vector3::zeros(), // blade doesn't translate
166/// Vector3::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 nalgebra::Isometry3;
177/// let wall = NonlinearRigidMotion::constant_position(
178/// Isometry3::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 nalgebra::{Isometry2, Point2, Vector2};
196///
197/// let motion = NonlinearRigidMotion::new(
198/// Isometry2::translation(0.0, 0.0),
199/// Point2::origin(),
200/// Vector2::zeros(),
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 nalgebra::{Isometry3, Point3, Vector3};
215///
216/// let motion = NonlinearRigidMotion::new(
217/// Isometry3::translation(0.0, 0.0, 0.0),
218/// Point3::origin(),
219/// Vector3::zeros(),
220/// Vector3::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 `Point::origin()`.
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, `Point::origin()` 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: Isometry<Real>,
253 /// The local-space point at which the rotational part of this motion is applied.
254 pub local_center: Point<Real>,
255 /// The translational velocity of this motion.
256 pub linvel: Vector<Real>,
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<Real>,
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` - Point (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 nalgebra::{Isometry2, Point2, Vector2};
281 /// use std::f32::consts::PI;
282 ///
283 /// // Object moving right and rotating counter-clockwise
284 /// let motion = NonlinearRigidMotion::new(
285 /// Isometry2::translation(0.0, 0.0),
286 /// Point2::origin(),
287 /// Vector2::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 nalgebra::{Isometry3, Point3, Vector3};
299 ///
300 /// // Object moving forward and spinning around its movement axis
301 /// let motion = NonlinearRigidMotion::new(
302 /// Isometry3::translation(0.0, 0.0, 0.0),
303 /// Point3::origin(),
304 /// Vector3::new(10.0, 0.0, 0.0), // moving forward
305 /// Vector3::new(5.0, 0.0, 0.0), // spinning around X axis
306 /// );
307 /// # }
308 /// ```
309 #[cfg(feature = "dim2")]
310 pub fn new(
311 start: Isometry<Real>,
312 local_center: Point<Real>,
313 linvel: Vector<Real>,
314 angvel: Real,
315 ) -> Self {
316 NonlinearRigidMotion {
317 start,
318 local_center,
319 linvel,
320 angvel,
321 }
322 }
323
324 /// Creates a new rigid motion from a starting position and velocities.
325 ///
326 /// # Arguments
327 ///
328 /// * `start` - Initial position and orientation at time `t = 0`
329 /// * `local_center` - Point (in local coordinates) around which rotation occurs
330 /// * `linvel` - Linear velocity vector (units per second)
331 /// * `angvel` - Angular velocity as axis-angle vector (radians per second)
332 ///
333 /// # Example
334 ///
335 /// ```
336 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
337 /// use parry3d::query::NonlinearRigidMotion;
338 /// use nalgebra::{Isometry3, Point3, Vector3};
339 ///
340 /// // Object moving forward and spinning around its movement axis
341 /// let motion = NonlinearRigidMotion::new(
342 /// Isometry3::translation(0.0, 0.0, 0.0),
343 /// Point3::origin(),
344 /// Vector3::new(10.0, 0.0, 0.0), // moving forward
345 /// Vector3::new(5.0, 0.0, 0.0), // spinning around X axis
346 /// );
347 /// # }
348 /// ```
349 #[cfg(feature = "dim3")]
350 pub fn new(
351 start: Isometry<Real>,
352 local_center: Point<Real>,
353 linvel: Vector<Real>,
354 angvel: Vector<Real>,
355 ) -> Self {
356 NonlinearRigidMotion {
357 start,
358 local_center,
359 linvel,
360 angvel,
361 }
362 }
363
364 /// Creates a stationary motion at the origin (identity transformation).
365 ///
366 /// Equivalent to `constant_position(Isometry::identity())`.
367 ///
368 /// # Example
369 ///
370 /// ```
371 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
372 /// use parry3d::query::NonlinearRigidMotion;
373 ///
374 /// let stationary = NonlinearRigidMotion::identity();
375 /// // Object stays at origin with no rotation, forever
376 /// # }
377 /// ```
378 pub fn identity() -> Self {
379 Self::constant_position(Isometry::identity())
380 }
381
382 /// Creates a motion that stays at a constant position (no translation, no rotation).
383 ///
384 /// Useful for representing static/stationary objects in collision queries.
385 /// Both linear and angular velocities are set to zero.
386 ///
387 /// # Arguments
388 ///
389 /// * `pos` - The fixed position and orientation
390 ///
391 /// # Example
392 ///
393 /// ```
394 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
395 /// use parry3d::query::NonlinearRigidMotion;
396 /// use nalgebra::Isometry3;
397 ///
398 /// // A wall that never moves
399 /// let wall_motion = NonlinearRigidMotion::constant_position(
400 /// Isometry3::translation(10.0, 0.0, 0.0)
401 /// );
402 ///
403 /// // At any time t, position is always (10, 0, 0)
404 /// let pos_at_5 = wall_motion.position_at_time(5.0);
405 /// assert_eq!(pos_at_5.translation.vector.x, 10.0);
406 /// # }
407 /// ```
408 pub fn constant_position(pos: Isometry<Real>) -> Self {
409 Self {
410 start: pos,
411 linvel: na::zero(),
412 angvel: na::zero(),
413 local_center: Point::origin(),
414 }
415 }
416
417 fn set_start(&mut self, new_start: Isometry<Real>) {
418 // NOTE: we need to adjust the local_center so that the angular
419 // velocity is still expressed wrt. the original center.
420 self.local_center = new_start.inverse_transform_point(&(self.start * self.local_center));
421 self.start = new_start;
422 }
423
424 /// Freezes this motion at a specific time, making it stationary from that point forward.
425 ///
426 /// After calling this method:
427 /// - `self.start` is updated to the position at time `t`
428 /// - Both velocities are set to zero
429 /// - All future calls to `position_at_time()` return the same frozen position
430 ///
431 /// This is useful for "stopping" an object mid-motion, or capturing a specific
432 /// moment in a motion trajectory.
433 ///
434 /// # Arguments
435 ///
436 /// * `t` - The time at which to freeze the motion
437 ///
438 /// # Example
439 ///
440 /// ```
441 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
442 /// use parry3d::query::NonlinearRigidMotion;
443 /// use nalgebra::{Isometry3, Point3, Vector3};
444 ///
445 /// let mut motion = NonlinearRigidMotion::new(
446 /// Isometry3::translation(0.0, 0.0, 0.0),
447 /// Point3::origin(),
448 /// Vector3::new(5.0, 0.0, 0.0), // moving right
449 /// Vector3::zeros(),
450 /// );
451 ///
452 /// // Freeze at t=2.0 (when object is at x=10)
453 /// motion.freeze(2.0);
454 ///
455 /// // Now position is constant at x=10, regardless of time
456 /// let pos_at_100 = motion.position_at_time(100.0);
457 /// assert_eq!(pos_at_100.translation.vector.x, 10.0);
458 /// # }
459 /// ```
460 pub fn freeze(&mut self, t: Real) {
461 self.start = self.position_at_time(t);
462 self.linvel = na::zero();
463 self.angvel = na::zero();
464 }
465
466 /// Appends a constant translation to this rigid-motion.
467 #[must_use]
468 pub fn append_translation(&self, tra: Vector<Real>) -> Self {
469 let mut result = *self;
470 result.set_start(Translation::from(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<Real>) -> Self {
477 let mut result = *self;
478 result.set_start(result.start * Translation::from(tra));
479 result
480 }
481
482 /// Appends a constant isometry to this rigid-motion.
483 #[must_use]
484 pub fn append(&self, iso: Isometry<Real>) -> 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: Isometry<Real>) -> 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 nalgebra::{Isometry3, Point3, Vector3};
522 ///
523 /// let motion = NonlinearRigidMotion::new(
524 /// Isometry3::translation(0.0, 0.0, 0.0),
525 /// Point3::origin(),
526 /// Vector3::new(3.0, 0.0, 0.0), // 3 units/sec to the right
527 /// Vector3::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.vector.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.vector.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 nalgebra::{Isometry3, Point3, Vector3};
548 ///
549 /// let motion = NonlinearRigidMotion::new(
550 /// Isometry3::translation(0.0, 5.0, 0.0),
551 /// Point3::origin(),
552 /// Vector3::new(0.0, -9.8, 0.0), // falling (gravity)
553 /// Vector3::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) -> Isometry<Real> {
566 let center = self.start * self.local_center;
567 let shift = Translation::from(center.coords);
568 (shift * Isometry::new(self.linvel * t, self.angvel * t)) * (shift.inverse() * self.start)
569 }
570}