parry3d/math/
mod.rs

1//! Math types and utilities for parry, wrapping glam types.
2
3mod vector_ext;
4
5/// The scalar type used throughout this crate.
6#[cfg(feature = "f64")]
7pub use f64 as Real;
8#[cfg(feature = "f64")]
9pub use i64 as Int;
10
11/// The scalar type used throughout this crate.
12#[cfg(feature = "f32")]
13pub use f32 as Real;
14#[cfg(feature = "f32")]
15pub use i32 as Int;
16
17// Re-export ComplexField and RealField from simba for math operations
18pub use simba::scalar::{ComplexField, RealField};
19
20pub use vector_ext::*;
21
22// Re-export SIMD types from parent module
23pub use crate::simd::*;
24
25// Re-export types from glamx based on precision
26#[cfg(feature = "f64")]
27pub use glamx::{
28    DPose2 as Pose2, DPose3 as Pose3, DRot2 as Rot2, DRot3 as Rot3,
29    DSymmetricEigen2 as SymmetricEigen2, DSymmetricEigen3 as SymmetricEigen3, MatExt,
30};
31#[cfg(feature = "f32")]
32pub use glamx::{MatExt, Pose2, Pose3, Rot2, Rot3, SymmetricEigen2, SymmetricEigen3};
33
34// Re-export glam types used directly
35#[cfg(feature = "f64")]
36pub use glamx::{
37    DMat2 as Mat2, DMat3 as Mat3, DVec2 as Vec2, DVec3 as Vec3, DVec3 as Vec3A, DVec4 as Vec4,
38};
39#[cfg(feature = "f32")]
40pub use glamx::{Mat2, Mat3, Vec2, Vec3, Vec3A, Vec4};
41
42/// The default tolerance used for geometric operations.
43pub const DEFAULT_EPSILON: Real = Real::EPSILON;
44
45/// The dimension of the space.
46#[cfg(feature = "dim2")]
47pub const DIM: usize = 2;
48/// The dimension of the space.
49#[cfg(feature = "dim3")]
50pub const DIM: usize = 3;
51
52/// The dimension of the space multiplied by two.
53pub const TWO_DIM: usize = DIM * 2;
54
55// ==================== DIMENSION-DEPENDENT TYPE ALIASES ====================
56
57// --- 3D Type Aliases ---
58
59#[cfg(feature = "dim3")]
60mod dim3_types {
61    use super::*;
62    #[cfg(feature = "f64")]
63    use glamx::{DMat3, DVec3};
64
65    /// The vector type.
66    #[cfg(feature = "f32")]
67    pub type Vector = Vec3;
68    /// The vector type.
69    #[cfg(feature = "f64")]
70    pub type Vector = DVec3;
71
72    /// The integer vector type.
73    #[cfg(feature = "f32")]
74    pub type IVector = glamx::IVec3;
75    /// The integer vector type.
76    #[cfg(feature = "f64")]
77    pub type IVector = glamx::I64Vec3;
78
79    /// The angular vector type.
80    #[cfg(feature = "f32")]
81    pub type AngVector = Vec3;
82    /// The angular vector type.
83    #[cfg(feature = "f64")]
84    pub type AngVector = DVec3;
85
86    /// The matrix type.
87    #[cfg(feature = "f32")]
88    pub type Matrix = Mat3;
89    /// The matrix type.
90    #[cfg(feature = "f64")]
91    pub type Matrix = DMat3;
92
93    /// The transformation matrix type (pose = rotation + translation).
94    pub type Pose = Pose3;
95
96    /// The rotation type.
97    pub type Rotation = Rot3;
98
99    /// The orientation type.
100    #[cfg(feature = "f32")]
101    pub type Orientation = Vec3;
102    /// The orientation type.
103    #[cfg(feature = "f64")]
104    pub type Orientation = DVec3;
105
106    /// A vector with a dimension equal to the maximum number of degrees of freedom of a rigid body.
107    #[cfg(feature = "f32")]
108    pub type SpatialVector = [f32; 6];
109    /// A vector with a dimension equal to the maximum number of degrees of freedom of a rigid body.
110    #[cfg(feature = "f64")]
111    pub type SpatialVector = [f64; 6];
112
113    /// The angular inertia of a rigid body.
114    pub type AngularInertia = crate::utils::SdpMatrix3<Real>;
115
116    /// The principal angular inertia of a rigid body.
117    #[cfg(feature = "f32")]
118    pub type PrincipalAngularInertia = Vec3;
119    /// The principal angular inertia of a rigid body.
120    #[cfg(feature = "f64")]
121    pub type PrincipalAngularInertia = DVec3;
122
123    /// A matrix that represent the cross product with a given vector.
124    #[cfg(feature = "f32")]
125    pub type CrossMatrix = Mat3;
126    /// A matrix that represent the cross product with a given vector.
127    #[cfg(feature = "f64")]
128    pub type CrossMatrix = DMat3;
129
130    /// A 3D symmetric-definite-positive matrix.
131    pub type SdpMatrix = crate::utils::SdpMatrix3<Real>;
132
133    /// The result of eigendecomposition of a symmetric matrix.
134    #[cfg(feature = "f32")]
135    pub type SymmetricEigen = SymmetricEigen3;
136    /// The result of eigendecomposition of a symmetric matrix.
137    #[cfg(feature = "f64")]
138    pub type SymmetricEigen = glamx::DSymmetricEigen3;
139}
140
141#[cfg(feature = "dim3")]
142pub use dim3_types::*;
143
144// --- 2D Type Aliases ---
145
146#[cfg(feature = "dim2")]
147mod dim2_types {
148    use super::*;
149    #[cfg(feature = "f64")]
150    use glamx::{DMat2, DVec2, DVec3};
151
152    /// The vector type.
153    #[cfg(feature = "f32")]
154    pub type Vector = Vec2;
155    /// The vector type.
156    #[cfg(feature = "f64")]
157    pub type Vector = DVec2;
158
159    /// The integer vector type.
160    #[cfg(feature = "f32")]
161    pub type IVector = glamx::IVec2;
162    /// The integer vector type.
163    #[cfg(feature = "f64")]
164    pub type IVector = glamx::I64Vec2;
165
166    /// The angular vector type (scalar for 2D).
167    pub type AngVector = Real;
168
169    /// The matrix type.
170    #[cfg(feature = "f32")]
171    pub type Matrix = Mat2;
172    /// The matrix type.
173    #[cfg(feature = "f64")]
174    pub type Matrix = DMat2;
175
176    /// The transformation matrix type (pose = rotation + translation).
177    pub type Pose = Pose2;
178
179    /// The rotation type.
180    pub type Rotation = Rot2;
181
182    /// The orientation type (scalar angle for 2D).
183    pub type Orientation = Real;
184
185    /// A vector with a dimension equal to the maximum number of degrees of freedom of a rigid body.
186    #[cfg(feature = "f32")]
187    pub type SpatialVector = Vec3;
188    /// A vector with a dimension equal to the maximum number of degrees of freedom of a rigid body.
189    #[cfg(feature = "f64")]
190    pub type SpatialVector = DVec3;
191
192    /// The angular inertia of a rigid body (scalar for 2D).
193    pub type AngularInertia = Real;
194
195    /// The principal angular inertia of a rigid body (scalar for 2D).
196    pub type PrincipalAngularInertia = Real;
197
198    /// A matrix that represent the cross product with a given vector.
199    #[cfg(feature = "f32")]
200    pub type CrossMatrix = Vec2;
201    /// A matrix that represent the cross product with a given vector.
202    #[cfg(feature = "f64")]
203    pub type CrossMatrix = DVec2;
204
205    /// A 2D symmetric-definite-positive matrix.
206    pub type SdpMatrix = crate::utils::SdpMatrix2<Real>;
207
208    /// The result of eigendecomposition of a symmetric matrix.
209    #[cfg(feature = "f32")]
210    pub type SymmetricEigen = SymmetricEigen2;
211    /// The result of eigendecomposition of a symmetric matrix.
212    #[cfg(feature = "f64")]
213    pub type SymmetricEigen = glamx::DSymmetricEigen2;
214}
215
216#[cfg(feature = "dim2")]
217pub use dim2_types::*;
218
219// ==================== ORTHONORMAL BASIS COMPUTATION ====================
220
221/// Computes an orthonormal basis for the subspace orthogonal to the given vectors.
222/// Calls the callback `f` with each basis vector.
223///
224/// For 3D: given 1 vector, produces 2 orthonormal vectors perpendicular to it.
225#[cfg(feature = "dim3")]
226pub fn orthonormal_subspace_basis<F>(vs: &[Vector], mut f: F)
227where
228    F: FnMut(Vector) -> bool,
229{
230    if vs.is_empty() {
231        return;
232    }
233
234    // Normalize the input vector
235    let v = vs[0].normalize_or_zero();
236
237    if v == Vector::ZERO {
238        return;
239    }
240
241    // Find a vector that's not parallel to v
242    let orth = if v.x.abs() > v.z.abs() {
243        Vector::new(-v.y, v.x, 0.0)
244    } else {
245        Vector::new(0.0, -v.z, v.y)
246    };
247
248    // First orthonormal vector
249    let orth1 = orth.normalize();
250    if !f(orth1) {
251        return;
252    }
253
254    // Second orthonormal vector (cross product)
255    let orth2 = v.cross(orth1);
256    let _ = f(orth2);
257}
258
259// ==================== 2D TYPES FOR 3D CONTEXTS ====================
260// These are needed for algorithms like spiral intersection that use 2D math in 3D contexts.
261
262/// A 2D vector type for use in any dimension context.
263pub type Vector2 = Vec2;
264
265/// A 2x2 matrix type for use in any dimension context.
266pub type Matrix2 = Mat2;
267
268/// A 3D vector type for use in any dimension context.
269pub type Vector3 = Vec3;
270
271/// A 3x3 matrix type for use in any dimension context.
272pub type Matrix3 = Mat3;
273
274/// Converts an integer vector to a floating-point vector.
275#[cfg(all(feature = "dim2", feature = "f32"))]
276pub fn ivect_to_vect(p: IVector) -> Vector {
277    p.as_vec2()
278}
279/// Converts an integer vector to a floating-point vector.
280#[cfg(all(feature = "dim2", feature = "f64"))]
281pub fn ivect_to_vect(p: IVector) -> Vector {
282    p.as_dvec2()
283}
284/// Converts an integer vector to a floating-point vector.
285#[cfg(all(feature = "dim3", feature = "f32"))]
286pub fn ivect_to_vect(p: IVector) -> Vector {
287    p.as_vec3()
288}
289/// Converts an integer vector to a floating-point vector.
290#[cfg(all(feature = "dim3", feature = "f64"))]
291pub fn ivect_to_vect(p: IVector) -> Vector {
292    p.as_dvec3()
293}
294
295/// Converts a floating-point vector to an integer vector.
296#[cfg(all(feature = "dim2", feature = "f32"))]
297pub fn vect_to_ivect(p: Vector) -> IVector {
298    p.as_ivec2()
299}
300/// Converts a floating-point vector to an integer vector.
301#[cfg(all(feature = "dim2", feature = "f64"))]
302pub fn vect_to_ivect(p: Vector) -> IVector {
303    p.as_i64vec2()
304}
305/// Converts a floating-point vector to an integer vector.
306#[cfg(all(feature = "dim3", feature = "f32"))]
307pub fn vect_to_ivect(p: Vector) -> IVector {
308    p.as_ivec3()
309}
310/// Converts a floating-point vector to an integer vector.
311#[cfg(all(feature = "dim3", feature = "f64"))]
312pub fn vect_to_ivect(p: Vector) -> IVector {
313    p.as_i64vec3()
314}
parry3d/math/mod.rs

parry3d/math/
mod.rs
