avian3d/math/
mod.rs

1//! Math types and traits used by the crate.
2//!
3//! Most of the math types are feature-dependent, so they will be different for `2d`/`3d` and `f32`/`f64`.
4
5#![allow(unused_imports)]
6
7#[cfg(feature = "f32")]
8mod single;
9use approx::abs_diff_ne;
10use glam_matrix_extras::{SymmetricDMat2, SymmetricDMat3, SymmetricMat2, SymmetricMat3};
11#[cfg(feature = "f32")]
12pub use single::*;
13
14#[cfg(feature = "f64")]
15mod double;
16#[cfg(feature = "f64")]
17pub use double::*;
18
19use bevy_math::{prelude::*, *};
20
21/// The active dimension.
22#[cfg(feature = "2d")]
23pub const DIM: usize = 2;
24/// The active dimension.
25#[cfg(feature = "3d")]
26pub const DIM: usize = 3;
27
28/// The `f32` vector type chosen based on the dimension.
29#[cfg(feature = "2d")]
30pub(crate) use bevy_math::Vec2 as VectorF32;
31
32/// The `f32` vector type chosen based on the dimension.
33#[cfg(feature = "3d")]
34pub(crate) use bevy_math::Vec3 as VectorF32;
35
36/// The `i32` vector type chosen based on the dimension.
37#[cfg(feature = "2d")]
38pub(crate) use bevy_math::IVec2 as IVector;
39
40/// The `i32` vector type chosen based on the dimension.
41#[cfg(feature = "3d")]
42pub(crate) use bevy_math::IVec3 as IVector;
43
44/// The ray type chosen based on the dimension.
45#[cfg(feature = "2d")]
46pub(crate) type Ray = Ray2d;
47
48/// The ray type chosen based on the dimension.
49#[cfg(feature = "3d")]
50pub(crate) type Ray = Ray3d;
51
52// Note: This is called `Dir` instead of `Direction` because Bevy has a conflicting `Direction` type.
53/// The direction type chosen based on the dimension.
54#[cfg(feature = "2d")]
55pub type Dir = Dir2;
56
57/// The direction type chosen based on the dimension.
58#[cfg(feature = "3d")]
59pub type Dir = Dir3;
60
61/// The vector type for angular values chosen based on the dimension.
62#[cfg(feature = "2d")]
63pub(crate) type AngularVector = Scalar;
64
65/// The vector type for angular values chosen based on the dimension.
66#[cfg(feature = "3d")]
67pub(crate) type AngularVector = Vector;
68
69/// The symmetric tensor type chosen based on the dimension.
70/// Often used for angular inertia.
71///
72/// In 2D, this is a scalar, while in 3D, it is a 3x3 matrix.
73#[cfg(feature = "2d")]
74pub(crate) type SymmetricTensor = Scalar;
75
76/// The symmetric tensor type chosen based on the dimension.
77/// Often used for angular inertia.
78///
79/// In 2D, this is a scalar, while in 3D, it is a 3x3 matrix.
80#[cfg(feature = "3d")]
81pub(crate) type SymmetricTensor = SymmetricMatrix;
82
83/// The rotation type chosen based on the dimension.
84#[cfg(feature = "2d")]
85pub(crate) type Rot = crate::physics_transform::Rotation;
86
87/// The rotation type chosen based on the dimension.
88#[cfg(feature = "3d")]
89pub(crate) type Rot = Quaternion;
90
91/// The isometry type chosen based on the dimension.
92#[cfg(feature = "2d")]
93pub(crate) type Isometry = Isometry2d;
94
95/// The isometry type chosen based on the dimension.
96#[cfg(feature = "3d")]
97pub(crate) type Isometry = Isometry3d;
98
99/// Adjust the precision of the math construct to the precision chosen for compilation.
100pub trait AdjustPrecision {
101    /// A math construct type with the desired precision.
102    type Adjusted;
103    /// Adjusts the precision of [`self`] to [`Self::Adjusted`](#associatedtype.Adjusted).
104    fn adjust_precision(&self) -> Self::Adjusted;
105}
106
107/// Adjust the precision down to `f32` regardless of compilation.
108pub trait AsF32 {
109    /// The `f32` version of a math construct.
110    type F32;
111    /// Returns the `f32` version of this type.
112    fn f32(&self) -> Self::F32;
113}
114
115impl AsF32 for DVec3 {
116    type F32 = Vec3;
117    fn f32(&self) -> Self::F32 {
118        self.as_vec3()
119    }
120}
121
122impl AsF32 for Vec3 {
123    type F32 = Self;
124    fn f32(&self) -> Self::F32 {
125        *self
126    }
127}
128
129impl AsF32 for DVec2 {
130    type F32 = Vec2;
131    fn f32(&self) -> Self::F32 {
132        self.as_vec2()
133    }
134}
135
136impl AsF32 for Vec2 {
137    type F32 = Self;
138    fn f32(&self) -> Self::F32 {
139        *self
140    }
141}
142
143impl AsF32 for Vec4 {
144    type F32 = Self;
145    fn f32(&self) -> Self::F32 {
146        *self
147    }
148}
149
150impl AsF32 for DQuat {
151    type F32 = Quat;
152    fn f32(&self) -> Self::F32 {
153        self.as_quat()
154    }
155}
156
157impl AsF32 for Quat {
158    type F32 = Self;
159    fn f32(&self) -> Self::F32 {
160        *self
161    }
162}
163
164impl AsF32 for DMat2 {
165    type F32 = Mat2;
166    fn f32(&self) -> Self::F32 {
167        self.as_mat2()
168    }
169}
170
171impl AsF32 for Mat2 {
172    type F32 = Self;
173    fn f32(&self) -> Self::F32 {
174        *self
175    }
176}
177
178impl AsF32 for SymmetricDMat2 {
179    type F32 = SymmetricMat2;
180    fn f32(&self) -> Self::F32 {
181        SymmetricMat2 {
182            m00: self.m00 as f32,
183            m01: self.m01 as f32,
184            m11: self.m11 as f32,
185        }
186    }
187}
188
189impl AsF32 for SymmetricMat2 {
190    type F32 = Self;
191    fn f32(&self) -> Self::F32 {
192        *self
193    }
194}
195
196impl AsF32 for DMat3 {
197    type F32 = Mat3;
198    fn f32(&self) -> Self::F32 {
199        self.as_mat3()
200    }
201}
202
203impl AsF32 for Mat3 {
204    type F32 = Self;
205    fn f32(&self) -> Self::F32 {
206        *self
207    }
208}
209
210impl AsF32 for SymmetricDMat3 {
211    type F32 = SymmetricMat3;
212    fn f32(&self) -> Self::F32 {
213        SymmetricMat3 {
214            m00: self.m00 as f32,
215            m01: self.m01 as f32,
216            m02: self.m02 as f32,
217            m11: self.m11 as f32,
218            m12: self.m12 as f32,
219            m22: self.m22 as f32,
220        }
221    }
222}
223
224impl AsF32 for SymmetricMat3 {
225    type F32 = Self;
226    fn f32(&self) -> Self::F32 {
227        *self
228    }
229}
230
231#[cfg(feature = "2d")]
232pub(crate) fn cross(a: Vector, b: Vector) -> Scalar {
233    a.perp_dot(b)
234}
235
236#[cfg(feature = "3d")]
237pub(crate) fn cross(a: Vector, b: Vector) -> Vector {
238    a.cross(b)
239}
240
241/// An extension trait for computing reciprocals without division by zero.
242pub trait RecipOrZero {
243    /// Computes the reciprocal of `self` if `self` is not zero,
244    /// and returns zero otherwise to avoid division by zero.
245    fn recip_or_zero(self) -> Self;
246}
247
248impl RecipOrZero for f32 {
249    #[inline]
250    fn recip_or_zero(self) -> Self {
251        if self != 0.0 && self.is_finite() {
252            self.recip()
253        } else {
254            0.0
255        }
256    }
257}
258
259impl RecipOrZero for f64 {
260    #[inline]
261    fn recip_or_zero(self) -> Self {
262        if self != 0.0 && self.is_finite() {
263            self.recip()
264        } else {
265            0.0
266        }
267    }
268}
269
270impl RecipOrZero for Vec2 {
271    #[inline]
272    fn recip_or_zero(self) -> Self {
273        Self::new(self.x.recip_or_zero(), self.y.recip_or_zero())
274    }
275}
276
277impl RecipOrZero for Vec3 {
278    #[inline]
279    fn recip_or_zero(self) -> Self {
280        Self::new(
281            self.x.recip_or_zero(),
282            self.y.recip_or_zero(),
283            self.z.recip_or_zero(),
284        )
285    }
286}
287
288impl RecipOrZero for DVec2 {
289    #[inline]
290    fn recip_or_zero(self) -> Self {
291        Self::new(self.x.recip_or_zero(), self.y.recip_or_zero())
292    }
293}
294
295impl RecipOrZero for DVec3 {
296    #[inline]
297    fn recip_or_zero(self) -> Self {
298        Self::new(
299            self.x.recip_or_zero(),
300            self.y.recip_or_zero(),
301            self.z.recip_or_zero(),
302        )
303    }
304}
305
306/// An extension trait for matrix types.
307pub trait MatExt {
308    /// The scalar type of the matrix.
309    type Scalar;
310
311    /// Computes the inverse of `self` if `self` is not zero,
312    /// and returns zero otherwise to avoid division by zero.
313    fn inverse_or_zero(self) -> Self;
314
315    /// Checks if the matrix is isotropic, meaning that it is invariant
316    /// under all rotations of the coordinate system.
317    ///
318    /// For second-order tensors, this means that the diagonal elements
319    /// are equal and the off-diagonal elements are zero.
320    fn is_isotropic(&self, epsilon: Self::Scalar) -> bool;
321}
322
323impl MatExt for Mat2 {
324    type Scalar = f32;
325
326    #[inline]
327    fn inverse_or_zero(self) -> Self {
328        if self.determinant() == 0.0 {
329            Self::ZERO
330        } else {
331            self.inverse()
332        }
333    }
334
335    #[inline]
336    fn is_isotropic(&self, epsilon: f32) -> bool {
337        // Extract diagonal elements.
338        let diag = Vec2::new(self.x_axis.x, self.y_axis.y);
339
340        // All diagonal elements must be approximately equal.
341        if abs_diff_ne!(diag.x, diag.y, epsilon = epsilon) {
342            return false;
343        }
344
345        // Extract off-diagonal elements.
346        let off_diag = [self.x_axis.y, self.y_axis.x];
347
348        // All off-diagonal elements must be approximately zero.
349        off_diag.iter().all(|&x| x.abs() < epsilon)
350    }
351}
352
353impl MatExt for DMat2 {
354    type Scalar = f64;
355
356    #[inline]
357    fn inverse_or_zero(self) -> Self {
358        if self.determinant() == 0.0 {
359            Self::ZERO
360        } else {
361            self.inverse()
362        }
363    }
364
365    #[inline]
366    fn is_isotropic(&self, epsilon: f64) -> bool {
367        // Extract diagonal elements.
368        let diag = DVec2::new(self.x_axis.x, self.y_axis.y);
369
370        // All diagonal elements must be approximately equal.
371        if abs_diff_ne!(diag.x, diag.y, epsilon = epsilon) {
372            return false;
373        }
374
375        // Extract off-diagonal elements.
376        let off_diag = [self.x_axis.y, self.y_axis.x];
377
378        // All off-diagonal elements must be approximately zero.
379        off_diag.iter().all(|&x| x.abs() < epsilon)
380    }
381}
382
383impl MatExt for SymmetricMat2 {
384    type Scalar = f32;
385
386    #[inline]
387    fn inverse_or_zero(self) -> Self {
388        if self.determinant() == 0.0 {
389            Self::ZERO
390        } else {
391            self.inverse()
392        }
393    }
394
395    #[inline]
396    fn is_isotropic(&self, epsilon: f32) -> bool {
397        // Extract diagonal elements.
398        let diag = Vec2::new(self.m00, self.m11);
399
400        // All diagonal elements must be approximately equal.
401        if abs_diff_ne!(diag.x, diag.y, epsilon = epsilon) {
402            return false;
403        }
404
405        // All off-diagonal elements must be approximately zero.
406        self.m01.abs() < epsilon
407    }
408}
409
410impl MatExt for SymmetricDMat2 {
411    type Scalar = f64;
412
413    #[inline]
414    fn inverse_or_zero(self) -> Self {
415        if self.determinant() == 0.0 {
416            Self::ZERO
417        } else {
418            self.inverse()
419        }
420    }
421
422    #[inline]
423    fn is_isotropic(&self, epsilon: f64) -> bool {
424        // Extract diagonal elements.
425        let diag = DVec2::new(self.m00, self.m11);
426
427        // All diagonal elements must be approximately equal.
428        if abs_diff_ne!(diag.x, diag.y, epsilon = epsilon) {
429            return false;
430        }
431
432        // All off-diagonal elements must be approximately zero.
433        self.m01.abs() < epsilon
434    }
435}
436
437impl MatExt for Mat3 {
438    type Scalar = f32;
439
440    #[inline]
441    fn inverse_or_zero(self) -> Self {
442        if self.determinant() == 0.0 {
443            Self::ZERO
444        } else {
445            self.inverse()
446        }
447    }
448
449    #[inline]
450    fn is_isotropic(&self, epsilon: f32) -> bool {
451        // Extract diagonal elements.
452        let diag = Vec3::new(self.x_axis.x, self.y_axis.y, self.z_axis.z);
453
454        // All diagonal elements must be approximately equal.
455        if abs_diff_ne!(diag.x, diag.y, epsilon = epsilon)
456            || abs_diff_ne!(diag.y, diag.z, epsilon = epsilon)
457        {
458            return false;
459        }
460
461        // Extract off-diagonal elements.
462        let off_diag = [
463            self.x_axis.y,
464            self.x_axis.z,
465            self.y_axis.x,
466            self.y_axis.z,
467            self.z_axis.x,
468            self.z_axis.y,
469        ];
470
471        // All off-diagonal elements must be approximately zero.
472        off_diag.iter().all(|&x| x.abs() < epsilon)
473    }
474}
475
476impl MatExt for DMat3 {
477    type Scalar = f64;
478
479    #[inline]
480    fn inverse_or_zero(self) -> Self {
481        if self.determinant() == 0.0 {
482            Self::ZERO
483        } else {
484            self.inverse()
485        }
486    }
487
488    #[inline]
489    fn is_isotropic(&self, epsilon: f64) -> bool {
490        // Extract diagonal elements.
491        let diag = DVec3::new(self.x_axis.x, self.y_axis.y, self.z_axis.z);
492
493        // All diagonal elements must be approximately equal.
494        if abs_diff_ne!(diag.x, diag.y, epsilon = epsilon)
495            || abs_diff_ne!(diag.y, diag.z, epsilon = epsilon)
496        {
497            return false;
498        }
499
500        // Extract off-diagonal elements.
501        let off_diag = [
502            self.x_axis.y,
503            self.x_axis.z,
504            self.y_axis.x,
505            self.y_axis.z,
506            self.z_axis.x,
507            self.z_axis.y,
508        ];
509
510        // All off-diagonal elements must be approximately zero.
511        off_diag.iter().all(|&x| x.abs() < epsilon)
512    }
513}
514
515impl MatExt for SymmetricMat3 {
516    type Scalar = f32;
517
518    #[inline]
519    fn inverse_or_zero(self) -> Self {
520        if self.determinant() == 0.0 {
521            Self::ZERO
522        } else {
523            self.inverse()
524        }
525    }
526
527    #[inline]
528    fn is_isotropic(&self, epsilon: f32) -> bool {
529        // Extract diagonal elements.
530        let diag = Vec3::new(self.m00, self.m11, self.m22);
531
532        // All diagonal elements must be approximately equal.
533        if abs_diff_ne!(diag.x, diag.y, epsilon = epsilon)
534            || abs_diff_ne!(diag.y, diag.z, epsilon = epsilon)
535        {
536            return false;
537        }
538
539        // Extract off-diagonal elements.
540        let off_diag = [self.m01, self.m02, self.m12];
541
542        // All off-diagonal elements must be approximately zero.
543        off_diag.iter().all(|&x| x.abs() < epsilon)
544    }
545}
546
547impl MatExt for SymmetricDMat3 {
548    type Scalar = f64;
549
550    #[inline]
551    fn inverse_or_zero(self) -> Self {
552        if self.determinant() == 0.0 {
553            Self::ZERO
554        } else {
555            self.inverse()
556        }
557    }
558
559    #[inline]
560    fn is_isotropic(&self, epsilon: f64) -> bool {
561        // Extract diagonal elements.
562        let diag = DVec3::new(self.m00, self.m11, self.m22);
563
564        // All diagonal elements must be approximately equal.
565        if abs_diff_ne!(diag.x, diag.y, epsilon = epsilon)
566            || abs_diff_ne!(diag.y, diag.z, epsilon = epsilon)
567        {
568            return false;
569        }
570
571        // Extract off-diagonal elements.
572        let off_diag = [self.m01, self.m02, self.m12];
573
574        // All off-diagonal elements must be approximately zero.
575        off_diag.iter().all(|&x| x.abs() < epsilon)
576    }
577}
578
579#[allow(clippy::unnecessary_cast)]
580#[cfg(all(feature = "2d", any(feature = "parry-f32", feature = "parry-f64")))]
581pub(crate) fn pose_to_isometry(pose: &parry::math::Pose) -> Isometry2d {
582    let rotation = Rot2::from_sin_cos(pose.rotation.im as f32, pose.rotation.re as f32);
583    Isometry2d::new(pose.translation.f32(), rotation)
584}
585
586#[cfg(all(
587    feature = "default-collider",
588    any(feature = "parry-f32", feature = "parry-f64")
589))]
590use crate::prelude::*;
591
592#[cfg(all(
593    feature = "2d",
594    feature = "default-collider",
595    any(feature = "parry-f32", feature = "parry-f64")
596))]
597pub(crate) fn make_pose(
598    position: impl Into<Position>,
599    rotation: impl Into<Rotation>,
600) -> parry::math::Pose2 {
601    let position: Position = position.into();
602    let rotation: Rotation = rotation.into();
603    parry::math::Pose2::new(position.0, rotation.as_radians())
604}
605
606#[cfg(all(
607    feature = "3d",
608    feature = "default-collider",
609    any(feature = "parry-f32", feature = "parry-f64")
610))]
611pub(crate) fn make_pose(
612    position: impl Into<Position>,
613    rotation: impl Into<Rotation>,
614) -> parry::math::Pose3 {
615    let position: Position = position.into();
616    let rotation: Rotation = rotation.into();
617    parry::math::Pose3::from_parts(position.0, rotation.0)
618}
619
620/// Computes the skew-symmetric matrix corresponding to the given vector.
621///
622/// ```text
623///                          [   0  -v.z  v.y ]
624/// skew_symmetric_mat3(v) = [  v.z   0  -v.x ]
625///                          [ -v.y  v.x   0  ]
626/// ```
627#[inline]
628#[must_use]
629#[cfg(feature = "3d")]
630pub fn skew_symmetric_mat3(v: Vector3) -> Matrix3 {
631    Matrix3::from_cols_array(&[0.0, v.z, -v.y, -v.z, 0.0, v.x, v.y, -v.x, 0.0])
632}
633
634/// Computes the rotation matrix of the orthonormal basis computed from the given axis.
635///
636/// The `axis` must be a unit vector.
637#[inline]
638#[must_use]
639pub fn orthonormal_basis_from_vec(axis: Vector) -> Rot {
640    #[cfg(feature = "2d")]
641    {
642        let normal = axis.perp();
643        orthonormal_basis([axis, normal])
644    }
645    #[cfg(feature = "3d")]
646    {
647        let (normal1, normal2) = axis.any_orthonormal_pair();
648        orthonormal_basis([axis, normal1, normal2])
649    }
650}
651
652/// Computes the rotation matrix of the orthonormal basis computed from the given axes.
653///
654/// Each axis must be a unit vector.
655#[inline]
656#[must_use]
657pub fn orthonormal_basis(axes: [Vector; DIM]) -> Rot {
658    #[cfg(feature = "2d")]
659    {
660        let mat = Matrix2::from_cols(axes[0], axes[1]);
661        crate::physics_transform::Rotation::from(mat)
662    }
663    #[cfg(feature = "3d")]
664    {
665        let mat = Matrix3::from_cols(axes[0], axes[1], axes[2]);
666        Quaternion::from_mat3(&mat)
667    }
668}
avian3d/math/mod.rs

avian3d/math/
mod.rs
