diff --git a/Cargo.toml b/Cargo.toml index 0cde558..e38800b 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,19 +1,24 @@ [workspace] -members = [ "build/rapier2d", "build/rapier_testbed2d", "examples2d", "benchmarks2d", - "build/rapier3d", "build/rapier_testbed3d", "examples3d", "benchmarks3d" ] +members = [ "build/rapier2d", "build/rapier2d-f64", "build/rapier_testbed2d", "examples2d", "benchmarks2d", + "build/rapier3d", "build/rapier3d-f64", "build/rapier_testbed3d", "examples3d", "benchmarks3d" ] [patch.crates-io] #wrapped2d = { git = "https://github.com/Bastacyclop/rust_box2d.git" } #simba = { path = "../simba" } -#ncollide2d = { path = "../ncollide/build/ncollide2d" } -#ncollide3d = { path = "../ncollide/build/ncollide3d" } -#nphysics2d = { path = "../nphysics/build/nphysics2d" } -#nphysics3d = { path = "../nphysics/build/nphysics3d" } -#kiss3d = { path = "../kiss3d" } -#cdl2d = { path = "../cdl/build/cdl2d" } -#cdl3d = { path = "../cdl/build/cdl3d" } -cdl2d = { git = "https://github.com/sebcrozet/cdl.git" } -cdl3d = { git = "https://github.com/sebcrozet/cdl.git" } +ncollide2d = { path = "../ncollide/build/ncollide2d" } +ncollide3d = { path = "../ncollide/build/ncollide3d" } +nphysics2d = { path = "../nphysics/build/nphysics2d" } +nphysics3d = { path = "../nphysics/build/nphysics3d" } +kiss3d = { path = "../kiss3d" } +cdl2d = { path = "../cdl/build/cdl2d" } +cdl3d = { path = "../cdl/build/cdl3d" } +cdl2d-f64 = { path = "../cdl/build/cdl2d-f64" } +cdl3d-f64 = { path = "../cdl/build/cdl3d-f64" } +#cdl2d = { git = "https://github.com/sebcrozet/cdl.git" } +#cdl3d = { git = "https://github.com/sebcrozet/cdl.git" } +#cdl2d-f64 = { git = "https://github.com/sebcrozet/cdl.git" } +#cdl3d-f64 = { git = "https://github.com/sebcrozet/cdl.git" } +#nalgebra = { path = "../nalgebra" } [profile.release] #debug = true diff --git a/benchmarks2d/Cargo.toml b/benchmarks2d/Cargo.toml index 90a6326..21169bb 100644 --- a/benchmarks2d/Cargo.toml +++ b/benchmarks2d/Cargo.toml @@ -14,7 +14,7 @@ enhanced-determinism = [ "rapier2d/enhanced-determinism" ] [dependencies] rand = "0.7" Inflector = "0.11" -nalgebra = "0.23" +nalgebra = "0.24" [dependencies.rapier_testbed2d] path = "../build/rapier_testbed2d" diff --git a/benchmarks3d/Cargo.toml b/benchmarks3d/Cargo.toml index 03194df..16399c9 100644 --- a/benchmarks3d/Cargo.toml +++ b/benchmarks3d/Cargo.toml @@ -14,7 +14,7 @@ enhanced-determinism = [ "rapier3d/enhanced-determinism" ] [dependencies] rand = "0.7" Inflector = "0.11" -nalgebra = "0.23" +nalgebra = "0.24" [dependencies.rapier_testbed3d] path = "../build/rapier_testbed3d" diff --git a/build/rapier2d-f64/Cargo.toml b/build/rapier2d-f64/Cargo.toml new file mode 100644 index 0000000..a1a4b54 --- /dev/null +++ b/build/rapier2d-f64/Cargo.toml @@ -0,0 +1,57 @@ +[package] +name = "rapier2d-f64" +version = "0.4.2" +authors = [ "Sébastien Crozet " ] +description = "2-dimensional physics engine in Rust." +documentation = "http://docs.rs/rapier2d" +homepage = "http://rapier.rs" +repository = "https://github.com/dimforge/rapier" +readme = "README.md" +keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ] +license = "Apache-2.0" +edition = "2018" + +[features] +default = [ "dim2", "f64" ] +dim2 = [ ] +f64 = [ ] +parallel = [ "rayon" ] +simd-stable = [ "simba/wide", "simd-is-enabled" ] +simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] +# Do not enable this feature directly. It is automatically +# enabled with the "simd-stable" or "simd-nightly" feature. +simd-is-enabled = [ ] +wasm-bindgen = [ "instant/wasm-bindgen" ] +serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "cdl2d-f64/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ] +enhanced-determinism = [ "simba/libm_force", "cdl2d-f64/enhanced-determinism", "indexmap" ] + +[lib] +name = "rapier2d_f64" +path = "../../src/lib.rs" +required-features = [ "dim2", "f64" ] + + +[dependencies] +vec_map = "0.8" +instant = { version = "0.1", features = [ "now" ]} +num-traits = "0.2" +nalgebra = "0.24" +cdl2d-f64 = "0.1" +simba = "0.3" +approx = "0.4" +rayon = { version = "1", optional = true } +crossbeam = "0.8" +generational-arena = "0.2" +arrayvec = "0.5" +bit-vec = "0.6" +rustc-hash = "1" +serde = { version = "1", features = [ "derive" ], optional = true } +erased-serde = { version = "0.3", optional = true } +indexmap = { version = "1", features = [ "serde-1" ], optional = true } +downcast-rs = "1.2" +num-derive = "0.3" +bitflags = "1" + +[dev-dependencies] +bincode = "1" +serde = { version = "1", features = [ "derive" ] } diff --git a/build/rapier2d/Cargo.toml b/build/rapier2d/Cargo.toml index 3bbb8f0..1729e6a 100644 --- a/build/rapier2d/Cargo.toml +++ b/build/rapier2d/Cargo.toml @@ -12,8 +12,9 @@ license = "Apache-2.0" edition = "2018" [features] -default = [ "dim2" ] +default = [ "dim2", "f32" ] dim2 = [ ] +f32 = [ ] parallel = [ "rayon" ] simd-stable = [ "simba/wide", "simd-is-enabled" ] simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] @@ -27,14 +28,14 @@ enhanced-determinism = [ "simba/libm_force", "cdl2d/enhanced-determinism", "inde [lib] name = "rapier2d" path = "../../src/lib.rs" -required-features = [ "dim2" ] +required-features = [ "dim2", "f32" ] [dependencies] vec_map = "0.8" instant = { version = "0.1", features = [ "now" ]} num-traits = "0.2" -nalgebra = "0.23" +nalgebra = "0.24" cdl2d = "0.1" simba = "0.3" approx = "0.4" diff --git a/build/rapier3d-f64/Cargo.toml b/build/rapier3d-f64/Cargo.toml new file mode 100644 index 0000000..6c46557 --- /dev/null +++ b/build/rapier3d-f64/Cargo.toml @@ -0,0 +1,56 @@ +[package] +name = "rapier3d-f64" +version = "0.4.2" +authors = [ "Sébastien Crozet " ] +description = "3-dimensional physics engine in Rust." +documentation = "http://docs.rs/rapier3d" +homepage = "http://rapier.rs" +repository = "https://github.com/dimforge/rapier" +readme = "README.md" +keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ] +license = "Apache-2.0" +edition = "2018" + +[features] +default = [ "dim3", "f64" ] +dim3 = [ ] +f64 = [ ] +parallel = [ "rayon" ] +simd-stable = [ "cdl3d-f64/simd-stable", "simba/wide", "simd-is-enabled" ] +simd-nightly = [ "cdl3d-f64/simd-nightly", "simba/packed_simd", "simd-is-enabled" ] +# Do not enable this feature directly. It is automatically +# enabled with the "simd-stable" or "simd-nightly" feature. +simd-is-enabled = [ ] +wasm-bindgen = [ "instant/wasm-bindgen" ] +serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "cdl3d-f64/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ] +enhanced-determinism = [ "simba/libm_force", "cdl3d-f64/enhanced-determinism" ] + +[lib] +name = "rapier3d_f64" +path = "../../src/lib.rs" +required-features = [ "dim3", "f64" ] + + +[dependencies] +vec_map = "0.8" +instant = { version = "0.1", features = [ "now" ]} +num-traits = "0.2" +nalgebra = "0.24" +cdl3d-f64 = "0.1" +simba = "0.3" +approx = "0.4" +rayon = { version = "1", optional = true } +crossbeam = "0.8" +generational-arena = "0.2" +arrayvec = "0.5" +bit-vec = "0.6" +rustc-hash = "1" +serde = { version = "1", features = [ "derive" ], optional = true } +erased-serde = { version = "0.3", optional = true } +downcast-rs = "1.2" +num-derive = "0.3" +bitflags = "1" + +[dev-dependencies] +bincode = "1" +serde = { version = "1", features = [ "derive" ] } diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml index d2a1fae..6c8e4d1 100644 --- a/build/rapier3d/Cargo.toml +++ b/build/rapier3d/Cargo.toml @@ -12,8 +12,9 @@ license = "Apache-2.0" edition = "2018" [features] -default = [ "dim3" ] +default = [ "dim3", "f32" ] dim3 = [ ] +f32 = [ ] parallel = [ "rayon" ] simd-stable = [ "cdl3d/simd-stable", "simba/wide", "simd-is-enabled" ] simd-nightly = [ "cdl3d/simd-nightly", "simba/packed_simd", "simd-is-enabled" ] @@ -27,14 +28,14 @@ enhanced-determinism = [ "simba/libm_force", "cdl3d/enhanced-determinism" ] [lib] name = "rapier3d" path = "../../src/lib.rs" -required-features = [ "dim3" ] +required-features = [ "dim3", "f32" ] [dependencies] vec_map = "0.8" instant = { version = "0.1", features = [ "now" ]} num-traits = "0.2" -nalgebra = "0.23" +nalgebra = "0.24" cdl3d = "0.1" simba = "0.3" approx = "0.4" diff --git a/build/rapier_testbed2d/Cargo.toml b/build/rapier_testbed2d/Cargo.toml index 63d1136..c1ac651 100644 --- a/build/rapier_testbed2d/Cargo.toml +++ b/build/rapier_testbed2d/Cargo.toml @@ -23,7 +23,7 @@ other-backends = [ "wrapped2d", "nphysics2d" ] [dependencies] -nalgebra = "0.23" +nalgebra = "0.24" kiss3d = { version = "0.28", features = [ "conrod" ] } rand = "0.7" rand_pcg = "0.2" diff --git a/build/rapier_testbed3d/Cargo.toml b/build/rapier_testbed3d/Cargo.toml index f307211..af2311c 100644 --- a/build/rapier_testbed3d/Cargo.toml +++ b/build/rapier_testbed3d/Cargo.toml @@ -22,7 +22,7 @@ parallel = [ "rapier3d/parallel", "num_cpus" ] other-backends = [ "physx", "physx-sys", "glam", "nphysics3d" ] [dependencies] -nalgebra = "0.23" +nalgebra = "0.24" kiss3d = { version = "0.28", features = [ "conrod" ] } rand = "0.7" rand_pcg = "0.2" diff --git a/examples2d/Cargo.toml b/examples2d/Cargo.toml index f1ed728..6580923 100644 --- a/examples2d/Cargo.toml +++ b/examples2d/Cargo.toml @@ -14,7 +14,7 @@ enhanced-determinism = [ "rapier2d/enhanced-determinism" ] [dependencies] rand = "0.7" Inflector = "0.11" -nalgebra = "0.23" +nalgebra = "0.24" [dependencies.rapier_testbed2d] path = "../build/rapier_testbed2d" diff --git a/examples3d/Cargo.toml b/examples3d/Cargo.toml index 46fbca6..1f9cba3 100644 --- a/examples3d/Cargo.toml +++ b/examples3d/Cargo.toml @@ -14,7 +14,7 @@ enhanced-determinism = [ "rapier3d/enhanced-determinism" ] [dependencies] rand = "0.7" Inflector = "0.11" -nalgebra = "0.23" +nalgebra = "0.24" [dependencies.rapier_testbed3d] path = "../build/rapier_testbed3d" diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index b31c3f6..706fb3f 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -1,11 +1,13 @@ +use crate::math::Real; + /// Parameters for a time-step of the physics engine. #[derive(Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct IntegrationParameters { /// The timestep length (default: `1.0 / 60.0`) - dt: f32, + dt: Real, /// The inverse of `dt`. - inv_dt: f32, + inv_dt: Real, // /// If `true` and if rapier is compiled with the `parallel` feature, this will enable rayon-based multithreading (default: `true`). // /// // /// This parameter is ignored if rapier is not compiled with is `parallel` feature. @@ -19,31 +21,31 @@ pub struct IntegrationParameters { pub return_after_ccd_substep: bool, /// The Error Reduction Parameter in `[0, 1]` is the proportion of /// the positional error to be corrected at each time step (default: `0.2`). - pub erp: f32, + pub erp: Real, /// The Error Reduction Parameter for joints in `[0, 1]` is the proportion of /// the positional error to be corrected at each time step (default: `0.2`). - pub joint_erp: f32, + pub joint_erp: Real, /// Each cached impulse are multiplied by this coefficient in `[0, 1]` /// when they are re-used to initialize the solver (default `1.0`). - pub warmstart_coeff: f32, + pub warmstart_coeff: Real, /// Contacts at points where the involved bodies have a relative /// velocity smaller than this threshold wont be affected by the restitution force (default: `1.0`). - pub restitution_velocity_threshold: f32, + pub restitution_velocity_threshold: Real, /// Amount of penetration the engine wont attempt to correct (default: `0.001m`). - pub allowed_linear_error: f32, + pub allowed_linear_error: Real, /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`). - pub prediction_distance: f32, + pub prediction_distance: Real, /// Amount of angular drift of joint limits the engine wont /// attempt to correct (default: `0.001rad`). - pub allowed_angular_error: f32, + pub allowed_angular_error: Real, /// Maximum linear correction during one step of the non-linear position solver (default: `0.2`). - pub max_linear_correction: f32, + pub max_linear_correction: Real, /// Maximum angular correction during one step of the non-linear position solver (default: `0.2`). - pub max_angular_correction: f32, + pub max_angular_correction: Real, /// Maximum nonlinear SOR-prox scaling parameter when the constraint /// correction direction is close to the kernel of the involved multibody's /// jacobian (default: `0.2`). - pub max_stabilization_multiplier: f32, + pub max_stabilization_multiplier: Real, /// Maximum number of iterations performed by the velocity constraints solver (default: `4`). pub max_velocity_iterations: usize, /// Maximum number of iterations performed by the position-based constraints solver (default: `1`). @@ -88,18 +90,18 @@ pub struct IntegrationParameters { impl IntegrationParameters { /// Creates a set of integration parameters with the given values. pub fn new( - dt: f32, + dt: Real, // multithreading_enabled: bool, - erp: f32, - joint_erp: f32, - warmstart_coeff: f32, - restitution_velocity_threshold: f32, - allowed_linear_error: f32, - allowed_angular_error: f32, - max_linear_correction: f32, - max_angular_correction: f32, - prediction_distance: f32, - max_stabilization_multiplier: f32, + erp: Real, + joint_erp: Real, + warmstart_coeff: Real, + restitution_velocity_threshold: Real, + allowed_linear_error: Real, + allowed_angular_error: Real, + max_linear_correction: Real, + max_angular_correction: Real, + prediction_distance: Real, + max_stabilization_multiplier: Real, max_velocity_iterations: usize, max_position_iterations: usize, max_ccd_position_iterations: usize, @@ -140,7 +142,7 @@ impl IntegrationParameters { /// The current time-stepping length. #[inline(always)] - pub fn dt(&self) -> f32 { + pub fn dt(&self) -> Real { self.dt } @@ -148,7 +150,7 @@ impl IntegrationParameters { /// /// This is zero if `self.dt` is zero. #[inline(always)] - pub fn inv_dt(&self) -> f32 { + pub fn inv_dt(&self) -> Real { self.inv_dt } @@ -156,7 +158,7 @@ impl IntegrationParameters { /// /// This automatically recompute `self.inv_dt`. #[inline] - pub fn set_dt(&mut self, dt: f32) { + pub fn set_dt(&mut self, dt: Real) { assert!(dt >= 0.0, "The time-stepping length cannot be negative."); self.dt = dt; if dt == 0.0 { @@ -170,7 +172,7 @@ impl IntegrationParameters { /// /// This automatically recompute `self.dt`. #[inline] - pub fn set_inv_dt(&mut self, inv_dt: f32) { + pub fn set_inv_dt(&mut self, inv_dt: Real) { self.inv_dt = inv_dt; if inv_dt == 0.0 { self.dt = 0.0 diff --git a/src/dynamics/joint/ball_joint.rs b/src/dynamics/joint/ball_joint.rs index ec255d4..82e2a10 100644 --- a/src/dynamics/joint/ball_joint.rs +++ b/src/dynamics/joint/ball_joint.rs @@ -1,29 +1,29 @@ -use crate::math::{Point, Vector}; +use crate::math::{Point, Real, Vector}; #[derive(Copy, Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// A joint that removes all relative linear motion between a pair of points on two bodies. pub struct BallJoint { /// Where the ball joint is attached on the first body, expressed in the first body local frame. - pub local_anchor1: Point, + pub local_anchor1: Point, /// Where the ball joint is attached on the first body, expressed in the first body local frame. - pub local_anchor2: Point, + pub local_anchor2: Point, /// The impulse applied by this joint on the first body. /// /// The impulse applied to the second body is given by `-impulse`. - pub impulse: Vector, + pub impulse: Vector, } impl BallJoint { /// Creates a new Ball joint from two anchors given on the local spaces of the respective bodies. - pub fn new(local_anchor1: Point, local_anchor2: Point) -> Self { + pub fn new(local_anchor1: Point, local_anchor2: Point) -> Self { Self::with_impulse(local_anchor1, local_anchor2, Vector::zeros()) } pub(crate) fn with_impulse( - local_anchor1: Point, - local_anchor2: Point, - impulse: Vector, + local_anchor1: Point, + local_anchor2: Point, + impulse: Vector, ) -> Self { Self { local_anchor1, diff --git a/src/dynamics/joint/fixed_joint.rs b/src/dynamics/joint/fixed_joint.rs index 0731cfb..359e14a 100644 --- a/src/dynamics/joint/fixed_joint.rs +++ b/src/dynamics/joint/fixed_joint.rs @@ -1,4 +1,4 @@ -use crate::math::{Isometry, SpacialVector}; +use crate::math::{Isometry, Real, SpacialVector}; #[derive(Copy, Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -8,22 +8,22 @@ use crate::math::{Isometry, SpacialVector}; pub struct FixedJoint { /// The frame of reference for the first body affected by this joint, expressed in the local frame /// of the first body. - pub local_anchor1: Isometry, + pub local_anchor1: Isometry, /// The frame of reference for the second body affected by this joint, expressed in the local frame /// of the first body. - pub local_anchor2: Isometry, + pub local_anchor2: Isometry, /// The impulse applied to the first body affected by this joint. /// /// The impulse applied to the second body affected by this joint is given by `-impulse`. /// This combines both linear and angular impulses: /// - In 2D, `impulse.xy()` gives the linear impulse, and `impulse.z` the angular impulse. /// - In 3D, `impulse.xyz()` gives the linear impulse, and `(impulse[3], impulse[4], impulse[5])` the angular impulse. - pub impulse: SpacialVector, + pub impulse: SpacialVector, } impl FixedJoint { /// Creates a new fixed joint from the frames of reference of both bodies. - pub fn new(local_anchor1: Isometry, local_anchor2: Isometry) -> Self { + pub fn new(local_anchor1: Isometry, local_anchor2: Isometry) -> Self { Self { local_anchor1, local_anchor2, diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs index a6fd558..174ce79 100644 --- a/src/dynamics/joint/prismatic_joint.rs +++ b/src/dynamics/joint/prismatic_joint.rs @@ -1,4 +1,4 @@ -use crate::math::{Isometry, Point, Vector, DIM}; +use crate::math::{Isometry, Point, Real, Vector, DIM}; use crate::utils::WBasis; use na::Unit; #[cfg(feature = "dim2")] @@ -11,35 +11,35 @@ use na::Vector5; /// A joint that removes all relative motion between two bodies, except for the translations along one axis. pub struct PrismaticJoint { /// Where the prismatic joint is attached on the first body, expressed in the local space of the first attached body. - pub local_anchor1: Point, + pub local_anchor1: Point, /// Where the prismatic joint is attached on the second body, expressed in the local space of the second attached body. - pub local_anchor2: Point, - pub(crate) local_axis1: Unit>, - pub(crate) local_axis2: Unit>, - pub(crate) basis1: [Vector; DIM - 1], - pub(crate) basis2: [Vector; DIM - 1], + pub local_anchor2: Point, + pub(crate) local_axis1: Unit>, + pub(crate) local_axis2: Unit>, + pub(crate) basis1: [Vector; DIM - 1], + pub(crate) basis2: [Vector; DIM - 1], /// The impulse applied by this joint on the first body. /// /// The impulse applied to the second body is given by `-impulse`. #[cfg(feature = "dim3")] - pub impulse: Vector5, + pub impulse: Vector5, /// The impulse applied by this joint on the first body. /// /// The impulse applied to the second body is given by `-impulse`. #[cfg(feature = "dim2")] - pub impulse: Vector2, + pub impulse: Vector2, /// Whether or not this joint should enforce translational limits along its axis. pub limits_enabled: bool, /// The min an max relative position of the attached bodies along this joint's axis. - pub limits: [f32; 2], + pub limits: [Real; 2], /// The impulse applied by this joint on the first body to enforce the position limit along this joint's axis. /// /// The impulse applied to the second body is given by `-impulse`. - pub limits_impulse: f32, + pub limits_impulse: Real, // pub motor_enabled: bool, - // pub target_motor_vel: f32, - // pub max_motor_impulse: f32, - // pub motor_impulse: f32, + // pub target_motor_vel: Real, + // pub max_motor_impulse: Real, + // pub motor_impulse: Real, } impl PrismaticJoint { @@ -47,10 +47,10 @@ impl PrismaticJoint { /// in the local-space of the affected bodies. #[cfg(feature = "dim2")] pub fn new( - local_anchor1: Point, - local_axis1: Unit>, - local_anchor2: Point, - local_axis2: Unit>, + local_anchor1: Point, + local_axis1: Unit>, + local_anchor2: Point, + local_axis2: Unit>, ) -> Self { Self { local_anchor1, @@ -61,11 +61,11 @@ impl PrismaticJoint { basis2: local_axis2.orthonormal_basis(), impulse: na::zero(), limits_enabled: false, - limits: [-f32::MAX, f32::MAX], + limits: [-Real::MAX, Real::MAX], limits_impulse: 0.0, // motor_enabled: false, // target_motor_vel: 0.0, - // max_motor_impulse: f32::MAX, + // max_motor_impulse: Real::MAX, // motor_impulse: 0.0, } } @@ -78,12 +78,12 @@ impl PrismaticJoint { /// computed arbitrarily. #[cfg(feature = "dim3")] pub fn new( - local_anchor1: Point, - local_axis1: Unit>, - local_tangent1: Vector, - local_anchor2: Point, - local_axis2: Unit>, - local_tangent2: Vector, + local_anchor1: Point, + local_axis1: Unit>, + local_tangent1: Vector, + local_anchor2: Point, + local_axis2: Unit>, + local_tangent2: Vector, ) -> Self { let basis1 = if let Some(local_bitangent1) = Unit::try_new(local_axis1.cross(&local_tangent1), 1.0e-3) @@ -116,28 +116,28 @@ impl PrismaticJoint { basis2, impulse: na::zero(), limits_enabled: false, - limits: [-f32::MAX, f32::MAX], + limits: [-Real::MAX, Real::MAX], limits_impulse: 0.0, // motor_enabled: false, // target_motor_vel: 0.0, - // max_motor_impulse: f32::MAX, + // max_motor_impulse: Real::MAX, // motor_impulse: 0.0, } } /// The local axis of this joint, expressed in the local-space of the first attached body. - pub fn local_axis1(&self) -> Unit> { + pub fn local_axis1(&self) -> Unit> { self.local_axis1 } /// The local axis of this joint, expressed in the local-space of the second attached body. - pub fn local_axis2(&self) -> Unit> { + pub fn local_axis2(&self) -> Unit> { self.local_axis2 } // FIXME: precompute this? #[cfg(feature = "dim2")] - pub(crate) fn local_frame1(&self) -> Isometry { + pub(crate) fn local_frame1(&self) -> Isometry { use na::{Matrix2, Rotation2, UnitComplex}; let mat = Matrix2::from_columns(&[self.local_axis1.into_inner(), self.basis1[0]]); @@ -149,7 +149,7 @@ impl PrismaticJoint { // FIXME: precompute this? #[cfg(feature = "dim2")] - pub(crate) fn local_frame2(&self) -> Isometry { + pub(crate) fn local_frame2(&self) -> Isometry { use na::{Matrix2, Rotation2, UnitComplex}; let mat = Matrix2::from_columns(&[self.local_axis2.into_inner(), self.basis2[0]]); @@ -161,7 +161,7 @@ impl PrismaticJoint { // FIXME: precompute this? #[cfg(feature = "dim3")] - pub(crate) fn local_frame1(&self) -> Isometry { + pub(crate) fn local_frame1(&self) -> Isometry { use na::{Matrix3, Rotation3, UnitQuaternion}; let mat = Matrix3::from_columns(&[ @@ -177,7 +177,7 @@ impl PrismaticJoint { // FIXME: precompute this? #[cfg(feature = "dim3")] - pub(crate) fn local_frame2(&self) -> Isometry { + pub(crate) fn local_frame2(&self) -> Isometry { use na::{Matrix3, Rotation3, UnitQuaternion}; let mat = Matrix3::from_columns(&[ diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs index cdb424b..ad7db0d 100644 --- a/src/dynamics/joint/revolute_joint.rs +++ b/src/dynamics/joint/revolute_joint.rs @@ -1,4 +1,4 @@ -use crate::math::{Point, Vector}; +use crate::math::{Point, Real, Vector}; use crate::utils::WBasis; use na::{Unit, Vector5}; @@ -7,31 +7,31 @@ use na::{Unit, Vector5}; /// A joint that removes all relative motion between two bodies, except for the rotations along one axis. pub struct RevoluteJoint { /// Where the revolute joint is attached on the first body, expressed in the local space of the first attached body. - pub local_anchor1: Point, + pub local_anchor1: Point, /// Where the revolute joint is attached on the second body, expressed in the local space of the second attached body. - pub local_anchor2: Point, + pub local_anchor2: Point, /// The rotation axis of this revolute joint expressed in the local space of the first attached body. - pub local_axis1: Unit>, + pub local_axis1: Unit>, /// The rotation axis of this revolute joint expressed in the local space of the second attached body. - pub local_axis2: Unit>, + pub local_axis2: Unit>, /// The basis orthonormal to `local_axis1`, expressed in the local space of the first attached body. - pub basis1: [Vector; 2], + pub basis1: [Vector; 2], /// The basis orthonormal to `local_axis2`, expressed in the local space of the second attached body. - pub basis2: [Vector; 2], + pub basis2: [Vector; 2], /// The impulse applied by this joint on the first body. /// /// The impulse applied to the second body is given by `-impulse`. - pub impulse: Vector5, + pub impulse: Vector5, } impl RevoluteJoint { /// Creates a new revolute joint with the given point of applications and axis, all expressed /// in the local-space of the affected bodies. pub fn new( - local_anchor1: Point, - local_axis1: Unit>, - local_anchor2: Point, - local_axis2: Unit>, + local_anchor1: Point, + local_axis1: Unit>, + local_anchor2: Point, + local_axis2: Unit>, ) -> Self { Self { local_anchor1, diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 5128b6f..683cca8 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -2,7 +2,9 @@ use crate::dynamics::MassProperties; use crate::geometry::{ Collider, ColliderHandle, ColliderSet, InteractionGraph, RigidBodyGraphIndex, }; -use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Translation, Vector}; +use crate::math::{ + AngVector, AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, +}; use crate::utils::{self, WCross, WDot}; use num::Zero; @@ -54,24 +56,24 @@ bitflags::bitflags! { #[derive(Debug, Clone)] pub struct RigidBody { /// The world-space position of the rigid-body. - pub(crate) position: Isometry, - pub(crate) predicted_position: Isometry, + pub(crate) position: Isometry, + pub(crate) predicted_position: Isometry, /// The local mass properties of the rigid-body. pub(crate) mass_properties: MassProperties, /// The world-space center of mass of the rigid-body. - pub world_com: Point, + pub world_com: Point, /// The square-root of the inverse angular inertia tensor of the rigid-body. - pub world_inv_inertia_sqrt: AngularInertia, + pub world_inv_inertia_sqrt: AngularInertia, /// The linear velocity of the rigid-body. - pub(crate) linvel: Vector, + pub(crate) linvel: Vector, /// The angular velocity of the rigid-body. - pub(crate) angvel: AngVector, + pub(crate) angvel: AngVector, /// Damping factor for gradually slowing down the translational motion of the rigid-body. - pub linear_damping: f32, + pub linear_damping: Real, /// Damping factor for gradually slowing down the angular motion of the rigid-body. - pub angular_damping: f32, - pub(crate) linacc: Vector, - pub(crate) angacc: AngVector, + pub angular_damping: Real, + pub(crate) linacc: Vector, + pub(crate) angacc: AngVector, pub(crate) colliders: Vec, /// Whether or not this rigid-body is sleeping. pub activation: ActivationStatus, @@ -125,7 +127,7 @@ impl RigidBody { self.active_set_timestamp = 0; } - pub(crate) fn integrate_accelerations(&mut self, dt: f32, gravity: Vector) { + pub(crate) fn integrate_accelerations(&mut self, dt: Real, gravity: Vector) { if self.mass_properties.inv_mass != 0.0 { self.linvel += (gravity + self.linacc) * dt; self.angvel += self.angacc * dt; @@ -184,7 +186,7 @@ impl RigidBody { /// The mass of this rigid body. /// /// Returns zero if this rigid body has an infinite mass. - pub fn mass(&self) -> f32 { + pub fn mass(&self) -> Real { utils::inv(self.mass_properties.inv_mass) } @@ -193,7 +195,7 @@ impl RigidBody { /// If this rigid-body is kinematic this value is set by the `set_next_kinematic_position` /// method and is used for estimating the kinematic body velocity at the next timestep. /// For non-kinematic bodies, this value is currently unspecified. - pub fn predicted_position(&self) -> &Isometry { + pub fn predicted_position(&self) -> &Isometry { &self.predicted_position } @@ -311,13 +313,13 @@ impl RigidBody { !self.linvel.is_zero() || !self.angvel.is_zero() } - fn integrate_velocity(&self, dt: f32) -> Isometry { + fn integrate_velocity(&self, dt: Real) -> Isometry { let com = &self.position * self.mass_properties.local_com; let shift = Translation::from(com.coords); shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse() } - pub(crate) fn integrate(&mut self, dt: f32) { + pub(crate) fn integrate(&mut self, dt: Real) { // TODO: do we want to apply damping before or after the velocity integration? self.linvel *= 1.0 / (1.0 + dt * self.linear_damping); self.angvel *= 1.0 / (1.0 + dt * self.angular_damping); @@ -326,19 +328,19 @@ impl RigidBody { } /// The linear velocity of this rigid-body. - pub fn linvel(&self) -> &Vector { + pub fn linvel(&self) -> &Vector { &self.linvel } /// The angular velocity of this rigid-body. #[cfg(feature = "dim2")] - pub fn angvel(&self) -> f32 { + pub fn angvel(&self) -> Real { self.angvel } /// The angular velocity of this rigid-body. #[cfg(feature = "dim3")] - pub fn angvel(&self) -> &Vector { + pub fn angvel(&self) -> &Vector { &self.angvel } @@ -346,7 +348,7 @@ impl RigidBody { /// /// If `wake_up` is `true` then the rigid-body will be woken up if it was /// put to sleep because it did not move for a while. - pub fn set_linvel(&mut self, linvel: Vector, wake_up: bool) { + pub fn set_linvel(&mut self, linvel: Vector, wake_up: bool) { self.linvel = linvel; if self.is_dynamic() && wake_up { @@ -359,7 +361,7 @@ impl RigidBody { /// If `wake_up` is `true` then the rigid-body will be woken up if it was /// put to sleep because it did not move for a while. #[cfg(feature = "dim2")] - pub fn set_angvel(&mut self, angvel: f32, wake_up: bool) { + pub fn set_angvel(&mut self, angvel: Real, wake_up: bool) { self.angvel = angvel; if self.is_dynamic() && wake_up { @@ -372,7 +374,7 @@ impl RigidBody { /// If `wake_up` is `true` then the rigid-body will be woken up if it was /// put to sleep because it did not move for a while. #[cfg(feature = "dim3")] - pub fn set_angvel(&mut self, angvel: Vector, wake_up: bool) { + pub fn set_angvel(&mut self, angvel: Vector, wake_up: bool) { self.angvel = angvel; if self.is_dynamic() && wake_up { @@ -381,7 +383,7 @@ impl RigidBody { } /// The world-space position of this rigid-body. - pub fn position(&self) -> &Isometry { + pub fn position(&self) -> &Isometry { &self.position } @@ -394,7 +396,7 @@ impl RigidBody { /// /// If `wake_up` is `true` then the rigid-body will be woken up if it was /// put to sleep because it did not move for a while. - pub fn set_position(&mut self, pos: Isometry, wake_up: bool) { + pub fn set_position(&mut self, pos: Isometry, wake_up: bool) { self.changes.insert(RigidBodyChanges::POSITION); self.set_position_internal(pos); @@ -404,7 +406,7 @@ impl RigidBody { } } - pub(crate) fn set_position_internal(&mut self, pos: Isometry) { + pub(crate) fn set_position_internal(&mut self, pos: Isometry) { self.position = pos; // TODO: update the predicted position for dynamic bodies too? @@ -414,13 +416,13 @@ impl RigidBody { } /// If this rigid body is kinematic, sets its future position after the next timestep integration. - pub fn set_next_kinematic_position(&mut self, pos: Isometry) { + pub fn set_next_kinematic_position(&mut self, pos: Isometry) { if self.is_kinematic() { self.predicted_position = pos; } } - pub(crate) fn compute_velocity_from_predicted_position(&mut self, inv_dt: f32) { + pub(crate) fn compute_velocity_from_predicted_position(&mut self, inv_dt: Real) { let dpos = self.predicted_position * self.position.inverse(); #[cfg(feature = "dim2")] { @@ -433,7 +435,7 @@ impl RigidBody { self.linvel = dpos.translation.vector * inv_dt; } - pub(crate) fn update_predicted_position(&mut self, dt: f32) { + pub(crate) fn update_predicted_position(&mut self, dt: Real) { self.predicted_position = self.integrate_velocity(dt) * self.position; } @@ -448,7 +450,7 @@ impl RigidBody { * Application of forces/impulses. */ /// Applies a force at the center-of-mass of this rigid-body. - pub fn apply_force(&mut self, force: Vector, wake_up: bool) { + pub fn apply_force(&mut self, force: Vector, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { self.linacc += force * self.mass_properties.inv_mass; @@ -459,7 +461,7 @@ impl RigidBody { } /// Applies an impulse at the center-of-mass of this rigid-body. - pub fn apply_impulse(&mut self, impulse: Vector, wake_up: bool) { + pub fn apply_impulse(&mut self, impulse: Vector, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { self.linvel += impulse * self.mass_properties.inv_mass; @@ -471,7 +473,7 @@ impl RigidBody { /// Applies a torque at the center-of-mass of this rigid-body. #[cfg(feature = "dim2")] - pub fn apply_torque(&mut self, torque: f32, wake_up: bool) { + pub fn apply_torque(&mut self, torque: Real, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque); @@ -483,7 +485,7 @@ impl RigidBody { /// Applies a torque at the center-of-mass of this rigid-body. #[cfg(feature = "dim3")] - pub fn apply_torque(&mut self, torque: Vector, wake_up: bool) { + pub fn apply_torque(&mut self, torque: Vector, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque); @@ -495,7 +497,7 @@ impl RigidBody { /// Applies an impulsive torque at the center-of-mass of this rigid-body. #[cfg(feature = "dim2")] - pub fn apply_torque_impulse(&mut self, torque_impulse: f32, wake_up: bool) { + pub fn apply_torque_impulse(&mut self, torque_impulse: Real, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { self.angvel += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse); @@ -508,7 +510,7 @@ impl RigidBody { /// Applies an impulsive torque at the center-of-mass of this rigid-body. #[cfg(feature = "dim3")] - pub fn apply_torque_impulse(&mut self, torque_impulse: Vector, wake_up: bool) { + pub fn apply_torque_impulse(&mut self, torque_impulse: Vector, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { self.angvel += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse); @@ -520,7 +522,7 @@ impl RigidBody { } /// Applies a force at the given world-space point of this rigid-body. - pub fn apply_force_at_point(&mut self, force: Vector, point: Point, wake_up: bool) { + pub fn apply_force_at_point(&mut self, force: Vector, point: Point, wake_up: bool) { let torque = (point - self.world_com).gcross(force); self.apply_force(force, wake_up); self.apply_torque(torque, wake_up); @@ -529,8 +531,8 @@ impl RigidBody { /// Applies an impulse at the given world-space point of this rigid-body. pub fn apply_impulse_at_point( &mut self, - impulse: Vector, - point: Point, + impulse: Vector, + point: Point, wake_up: bool, ) { let torque_impulse = (point - self.world_com).gcross(impulse); @@ -539,7 +541,7 @@ impl RigidBody { } /// The velocity of the given world-space point on this rigid-body. - pub fn velocity_at_point(&self, point: &Point) -> Vector { + pub fn velocity_at_point(&self, point: &Point) -> Vector { let dpt = point - self.world_com; self.linvel + self.angvel.gcross(dpt) } @@ -547,11 +549,11 @@ impl RigidBody { /// A builder for rigid-bodies. pub struct RigidBodyBuilder { - position: Isometry, - linvel: Vector, - angvel: AngVector, - linear_damping: f32, - angular_damping: f32, + position: Isometry, + linvel: Vector, + angvel: AngVector, + linear_damping: Real, + angular_damping: Real, body_status: BodyStatus, flags: RigidBodyFlags, mass_properties: MassProperties, @@ -595,7 +597,7 @@ impl RigidBodyBuilder { /// Sets the initial translation of the rigid-body to be created. #[cfg(feature = "dim2")] - pub fn translation(mut self, x: f32, y: f32) -> Self { + pub fn translation(mut self, x: Real, y: Real) -> Self { self.position.translation.x = x; self.position.translation.y = y; self @@ -603,7 +605,7 @@ impl RigidBodyBuilder { /// Sets the initial translation of the rigid-body to be created. #[cfg(feature = "dim3")] - pub fn translation(mut self, x: f32, y: f32, z: f32) -> Self { + pub fn translation(mut self, x: Real, y: Real, z: Real) -> Self { self.position.translation.x = x; self.position.translation.y = y; self.position.translation.z = z; @@ -611,13 +613,13 @@ impl RigidBodyBuilder { } /// Sets the initial orientation of the rigid-body to be created. - pub fn rotation(mut self, angle: AngVector) -> Self { + pub fn rotation(mut self, angle: AngVector) -> Self { self.position.rotation = Rotation::new(angle); self } /// Sets the initial position (translation and orientation) of the rigid-body to be created. - pub fn position(mut self, pos: Isometry) -> Self { + pub fn position(mut self, pos: Isometry) -> Self { self.position = pos; self } @@ -675,7 +677,7 @@ impl RigidBodyBuilder { /// will depends on the initial mass set by this method to which is added /// the contributions of all the colliders with non-zero density attached to /// this rigid-body. - pub fn mass(mut self, mass: f32, colliders_contribution_enabled: bool) -> Self { + pub fn mass(mut self, mass: Real, colliders_contribution_enabled: bool) -> Self { self.mass_properties.inv_mass = utils::inv(mass); self.flags.set( RigidBodyFlags::IGNORE_COLLIDER_MASS, @@ -696,7 +698,7 @@ impl RigidBodyBuilder { #[cfg(feature = "dim2")] pub fn principal_angular_inertia( mut self, - inertia: f32, + inertia: Real, colliders_contribution_enabled: bool, ) -> Self { self.mass_properties.inv_principal_inertia_sqrt = utils::inv(inertia); @@ -712,7 +714,7 @@ impl RigidBodyBuilder { /// Use `self.principal_angular_inertia` instead. #[cfg(feature = "dim2")] #[deprecated(note = "renamed to `principal_angular_inertia`.")] - pub fn principal_inertia(self, inertia: f32, colliders_contribution_enabled: bool) -> Self { + pub fn principal_inertia(self, inertia: Real, colliders_contribution_enabled: bool) -> Self { self.principal_angular_inertia(inertia, colliders_contribution_enabled) } @@ -731,7 +733,7 @@ impl RigidBodyBuilder { #[cfg(feature = "dim3")] pub fn principal_angular_inertia( mut self, - inertia: AngVector, + inertia: AngVector, colliders_contribution_enabled: AngVector, ) -> Self { self.mass_properties.inv_principal_inertia_sqrt = inertia.map(utils::inv); @@ -755,7 +757,7 @@ impl RigidBodyBuilder { #[deprecated(note = "renamed to `principal_angular_inertia`.")] pub fn principal_inertia( self, - inertia: AngVector, + inertia: AngVector, colliders_contribution_enabled: AngVector, ) -> Self { self.principal_angular_inertia(inertia, colliders_contribution_enabled) @@ -765,7 +767,7 @@ impl RigidBodyBuilder { /// /// The higher the linear damping factor is, the more quickly the rigid-body /// will slow-down its translational movement. - pub fn linear_damping(mut self, factor: f32) -> Self { + pub fn linear_damping(mut self, factor: Real) -> Self { self.linear_damping = factor; self } @@ -774,27 +776,27 @@ impl RigidBodyBuilder { /// /// The higher the angular damping factor is, the more quickly the rigid-body /// will slow-down its rotational movement. - pub fn angular_damping(mut self, factor: f32) -> Self { + pub fn angular_damping(mut self, factor: Real) -> Self { self.angular_damping = factor; self } /// Sets the initial linear velocity of the rigid-body to be created. #[cfg(feature = "dim2")] - pub fn linvel(mut self, x: f32, y: f32) -> Self { + pub fn linvel(mut self, x: Real, y: Real) -> Self { self.linvel = Vector::new(x, y); self } /// Sets the initial linear velocity of the rigid-body to be created. #[cfg(feature = "dim3")] - pub fn linvel(mut self, x: f32, y: f32, z: f32) -> Self { + pub fn linvel(mut self, x: Real, y: Real, z: Real) -> Self { self.linvel = Vector::new(x, y, z); self } /// Sets the initial angular velocity of the rigid-body to be created. - pub fn angvel(mut self, angvel: AngVector) -> Self { + pub fn angvel(mut self, angvel: AngVector) -> Self { self.angvel = angvel; self } @@ -845,16 +847,16 @@ impl RigidBodyBuilder { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct ActivationStatus { /// The threshold pseudo-kinetic energy bellow which the body can fall asleep. - pub threshold: f32, + pub threshold: Real, /// The current pseudo-kinetic energy of the body. - pub energy: f32, + pub energy: Real, /// Is this body already sleeping? pub sleeping: bool, } impl ActivationStatus { /// The default amount of energy bellow which a body can be put to sleep by nphysics. - pub fn default_threshold() -> f32 { + pub fn default_threshold() -> Real { 0.01 } diff --git a/src/dynamics/solver/delta_vel.rs b/src/dynamics/solver/delta_vel.rs index c4a424b..4614ed7 100644 --- a/src/dynamics/solver/delta_vel.rs +++ b/src/dynamics/solver/delta_vel.rs @@ -1,9 +1,9 @@ -use crate::math::{AngVector, Vector}; +use crate::math::{AngVector, Real, Vector}; use na::{Scalar, SimdRealField}; #[derive(Copy, Clone, Debug)] //#[repr(align(64))] -pub(crate) struct DeltaVel { +pub(crate) struct DeltaVel { pub linear: Vector, pub angular: AngVector, } diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index b5f8173..b409f98 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -2,7 +2,7 @@ use crate::dynamics::{BodyPair, JointGraphEdge, JointIndex, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; #[cfg(feature = "simd-is-enabled")] use { - crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH}, + crate::math::{Real, SIMD_LAST_INDEX, SIMD_WIDTH}, vec_map::VecMap, }; diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs index 21a537e..8bc9072 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs @@ -1,7 +1,7 @@ use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; #[cfg(feature = "dim2")] use crate::math::SdpMatrix; -use crate::math::{AngularInertia, Isometry, Point, Rotation}; +use crate::math::{AngularInertia, Isometry, Point, Real, Rotation}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[derive(Debug)] @@ -9,17 +9,17 @@ pub(crate) struct BallPositionConstraint { position1: usize, position2: usize, - local_com1: Point, - local_com2: Point, + local_com1: Point, + local_com2: Point, - im1: f32, - im2: f32, + im1: Real, + im2: Real, - ii1: AngularInertia, - ii2: AngularInertia, + ii1: AngularInertia, + ii2: AngularInertia, - local_anchor1: Point, - local_anchor2: Point, + local_anchor1: Point, + local_anchor2: Point, } impl BallPositionConstraint { @@ -38,7 +38,7 @@ impl BallPositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position1 = positions[self.position1 as usize]; let mut position2 = positions[self.position2 as usize]; @@ -95,11 +95,11 @@ impl BallPositionConstraint { #[derive(Debug)] pub(crate) struct BallPositionGroundConstraint { position2: usize, - anchor1: Point, - im2: f32, - ii2: AngularInertia, - local_anchor2: Point, - local_com2: Point, + anchor1: Point, + im2: Real, + ii2: AngularInertia, + local_anchor2: Point, + local_com2: Point, } impl BallPositionGroundConstraint { @@ -133,7 +133,7 @@ impl BallPositionGroundConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position2 = positions[self.position2 as usize]; let anchor2 = position2 * self.local_anchor2; diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs index f1ca4b6..2f2ffc3 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs @@ -1,7 +1,7 @@ use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; #[cfg(feature = "dim2")] use crate::math::SdpMatrix; -use crate::math::{AngularInertia, Isometry, Point, Rotation, SimdReal, SIMD_WIDTH}; +use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, SimdReal, SIMD_WIDTH}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use simba::simd::SimdValue; @@ -60,7 +60,7 @@ impl WBallPositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position1 = Isometry::from(array![|ii| positions[self.position1[ii]]; SIMD_WIDTH]); let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]); @@ -164,7 +164,7 @@ impl WBallPositionGroundConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]); let anchor2 = position2 * self.local_anchor2; diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs index 97ba244..9f47624 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs @@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, }; -use crate::math::{SdpMatrix, Vector}; +use crate::math::{Real, SdpMatrix, Vector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[derive(Debug)] @@ -12,16 +12,16 @@ pub(crate) struct BallVelocityConstraint { joint_id: JointIndex, - rhs: Vector, - pub(crate) impulse: Vector, + rhs: Vector, + pub(crate) impulse: Vector, - gcross1: Vector, - gcross2: Vector, + gcross1: Vector, + gcross2: Vector, - inv_lhs: SdpMatrix, + inv_lhs: SdpMatrix, - im1: f32, - im2: f32, + im1: Real, + im2: Real, } impl BallVelocityConstraint { @@ -91,7 +91,7 @@ impl BallVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; @@ -104,7 +104,7 @@ impl BallVelocityConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; @@ -137,11 +137,11 @@ impl BallVelocityConstraint { pub(crate) struct BallVelocityGroundConstraint { mj_lambda2: usize, joint_id: JointIndex, - rhs: Vector, - impulse: Vector, - gcross2: Vector, - inv_lhs: SdpMatrix, - im2: f32, + rhs: Vector, + impulse: Vector, + gcross2: Vector, + inv_lhs: SdpMatrix, + im2: Real, } impl BallVelocityGroundConstraint { @@ -206,14 +206,14 @@ impl BallVelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; mj_lambda2.linear -= self.im2 * self.impulse; mj_lambda2.angular -= self.gcross2.gcross(self.impulse); mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2); diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs index bbb709e..2806233 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs @@ -3,7 +3,7 @@ use crate::dynamics::{ BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, }; use crate::math::{ - AngVector, AngularInertia, Isometry, Point, SdpMatrix, SimdReal, Vector, SIMD_WIDTH, + AngVector, AngularInertia, Isometry, Point, Real, SdpMatrix, SimdReal, Vector, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use simba::simd::SimdValue; @@ -107,7 +107,7 @@ impl WBallVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -140,7 +140,7 @@ impl WBallVelocityConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1: DeltaVel = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -274,7 +274,7 @@ impl WBallVelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], @@ -293,7 +293,7 @@ impl WBallVelocityGroundConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2: DeltaVel = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs index 24001cd..cf9dcb7 100644 --- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs @@ -1,22 +1,22 @@ use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody}; -use crate::math::{AngularInertia, Isometry, Point, Rotation}; +use crate::math::{AngularInertia, Isometry, Point, Real, Rotation}; use crate::utils::WAngularInertia; #[derive(Debug)] pub(crate) struct FixedPositionConstraint { position1: usize, position2: usize, - local_anchor1: Isometry, - local_anchor2: Isometry, - local_com1: Point, - local_com2: Point, - im1: f32, - im2: f32, - ii1: AngularInertia, - ii2: AngularInertia, + local_anchor1: Isometry, + local_anchor2: Isometry, + local_com1: Point, + local_com2: Point, + im1: Real, + im2: Real, + ii1: AngularInertia, + ii2: AngularInertia, - lin_inv_lhs: f32, - ang_inv_lhs: AngularInertia, + lin_inv_lhs: Real, + ang_inv_lhs: AngularInertia, } impl FixedPositionConstraint { @@ -44,7 +44,7 @@ impl FixedPositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position1 = positions[self.position1 as usize]; let mut position2 = positions[self.position2 as usize]; @@ -81,12 +81,12 @@ impl FixedPositionConstraint { #[derive(Debug)] pub(crate) struct FixedPositionGroundConstraint { position2: usize, - anchor1: Isometry, - local_anchor2: Isometry, - local_com2: Point, - im2: f32, - ii2: AngularInertia, - impulse: f32, + anchor1: Isometry, + local_anchor2: Isometry, + local_com2: Point, + im2: Real, + ii2: AngularInertia, + impulse: Real, } impl FixedPositionGroundConstraint { @@ -118,7 +118,7 @@ impl FixedPositionGroundConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position2 = positions[self.position2 as usize]; // Angular correction. diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs index e4187c8..715b7dd 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs @@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, }; -use crate::math::{AngularInertia, Dim, SpacialVector, Vector}; +use crate::math::{AngularInertia, Dim, Real, SpacialVector, Vector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[cfg(feature = "dim2")] use na::{Matrix3, Vector3}; @@ -16,29 +16,29 @@ pub(crate) struct FixedVelocityConstraint { joint_id: JointIndex, - impulse: SpacialVector, + impulse: SpacialVector, #[cfg(feature = "dim3")] - inv_lhs: Matrix6, // FIXME: replace by Cholesky. + inv_lhs: Matrix6, // FIXME: replace by Cholesky. #[cfg(feature = "dim3")] - rhs: Vector6, + rhs: Vector6, #[cfg(feature = "dim2")] - inv_lhs: Matrix3, // FIXME: replace by Cholesky. + inv_lhs: Matrix3, // FIXME: replace by Cholesky. #[cfg(feature = "dim2")] - rhs: Vector3, + rhs: Vector3, - im1: f32, - im2: f32, + im1: Real, + im2: Real, - ii1: AngularInertia, - ii2: AngularInertia, + ii1: AngularInertia, + ii2: AngularInertia, - ii1_sqrt: AngularInertia, - ii2_sqrt: AngularInertia, + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, - r1: Vector, - r2: Vector, + r1: Vector, + r2: Vector, } impl FixedVelocityConstraint { @@ -128,7 +128,7 @@ impl FixedVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; @@ -152,7 +152,7 @@ impl FixedVelocityConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; @@ -207,22 +207,22 @@ pub(crate) struct FixedVelocityGroundConstraint { joint_id: JointIndex, - impulse: SpacialVector, + impulse: SpacialVector, #[cfg(feature = "dim3")] - inv_lhs: Matrix6, // FIXME: replace by Cholesky. + inv_lhs: Matrix6, // FIXME: replace by Cholesky. #[cfg(feature = "dim3")] - rhs: Vector6, + rhs: Vector6, #[cfg(feature = "dim2")] - inv_lhs: Matrix3, // FIXME: replace by Cholesky. + inv_lhs: Matrix3, // FIXME: replace by Cholesky. #[cfg(feature = "dim2")] - rhs: Vector3, + rhs: Vector3, - im2: f32, - ii2: AngularInertia, - ii2_sqrt: AngularInertia, - r2: Vector, + im2: Real, + ii2: AngularInertia, + ii2_sqrt: AngularInertia, + r2: Vector, } impl FixedVelocityGroundConstraint { @@ -312,7 +312,7 @@ impl FixedVelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); @@ -329,7 +329,7 @@ impl FixedVelocityGroundConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs index 6584ea2..48a6cba 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs @@ -5,8 +5,8 @@ use crate::dynamics::{ FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, }; use crate::math::{ - AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, SimdReal, SpacialVector, Vector, - SIMD_WIDTH, + AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, Real, SimdReal, SpacialVector, + Vector, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[cfg(feature = "dim3")] @@ -158,7 +158,7 @@ impl WFixedVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -202,7 +202,7 @@ impl WFixedVelocityConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1: DeltaVel = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -393,7 +393,7 @@ impl WFixedVelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], @@ -420,7 +420,7 @@ impl WFixedVelocityGroundConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2: DeltaVel = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs index 8b1f8bf..ed6d758 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs @@ -17,6 +17,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet, }; +use crate::math::Real; #[cfg(feature = "simd-is-enabled")] use crate::math::SIMD_WIDTH; @@ -220,7 +221,7 @@ impl AnyJointVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { match self { AnyJointVelocityConstraint::BallConstraint(c) => c.warmstart(mj_lambdas), AnyJointVelocityConstraint::BallGroundConstraint(c) => c.warmstart(mj_lambdas), @@ -254,7 +255,7 @@ impl AnyJointVelocityConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { match self { AnyJointVelocityConstraint::BallConstraint(c) => c.solve(mj_lambdas), AnyJointVelocityConstraint::BallGroundConstraint(c) => c.solve(mj_lambdas), diff --git a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs index 4773687..0ebe88e 100644 --- a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs @@ -7,9 +7,9 @@ use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint}; #[cfg(feature = "simd-is-enabled")] use super::{WBallPositionConstraint, WBallPositionGroundConstraint}; use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet}; -use crate::math::Isometry; #[cfg(feature = "simd-is-enabled")] use crate::math::SIMD_WIDTH; +use crate::math::{Isometry, Real}; pub(crate) enum AnyJointPositionConstraint { BallJoint(BallPositionConstraint), @@ -147,7 +147,7 @@ impl AnyJointPositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { match self { AnyJointPositionConstraint::BallJoint(c) => c.solve(params, positions), AnyJointPositionConstraint::BallGroundConstraint(c) => c.solve(params, positions), diff --git a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs index c3e4b98..2616f6b 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs @@ -1,5 +1,5 @@ use crate::dynamics::{IntegrationParameters, PrismaticJoint, RigidBody}; -use crate::math::{AngularInertia, Isometry, Point, Rotation, Vector}; +use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector}; use crate::utils::WAngularInertia; use na::Unit; @@ -8,22 +8,22 @@ pub(crate) struct PrismaticPositionConstraint { position1: usize, position2: usize, - im1: f32, - im2: f32, + im1: Real, + im2: Real, - ii1: AngularInertia, - ii2: AngularInertia, + ii1: AngularInertia, + ii2: AngularInertia, - lin_inv_lhs: f32, - ang_inv_lhs: AngularInertia, + lin_inv_lhs: Real, + ang_inv_lhs: AngularInertia, - limits: [f32; 2], + limits: [Real; 2], - local_frame1: Isometry, - local_frame2: Isometry, + local_frame1: Isometry, + local_frame2: Isometry, - local_axis1: Unit>, - local_axis2: Unit>, + local_axis1: Unit>, + local_axis2: Unit>, } impl PrismaticPositionConstraint { @@ -52,7 +52,7 @@ impl PrismaticPositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position1 = positions[self.position1 as usize]; let mut position2 = positions[self.position2 as usize]; @@ -99,11 +99,11 @@ impl PrismaticPositionConstraint { #[derive(Debug)] pub(crate) struct PrismaticPositionGroundConstraint { position2: usize, - frame1: Isometry, - local_frame2: Isometry, - axis1: Unit>, - local_axis2: Unit>, - limits: [f32; 2], + frame1: Isometry, + local_frame2: Isometry, + axis1: Unit>, + local_axis2: Unit>, + limits: [Real; 2], } impl PrismaticPositionGroundConstraint { @@ -140,7 +140,7 @@ impl PrismaticPositionGroundConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position2 = positions[self.position2 as usize]; // Angular correction. diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index 9e39bd6..78dc1b5 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody, }; -use crate::math::{AngularInertia, Vector}; +use crate::math::{AngularInertia, Real, Vector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[cfg(feature = "dim3")] use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; @@ -24,37 +24,37 @@ pub(crate) struct PrismaticVelocityConstraint { joint_id: JointIndex, - r1: Vector, - r2: Vector, + r1: Vector, + r2: Vector, #[cfg(feature = "dim3")] - inv_lhs: Matrix5, + inv_lhs: Matrix5, #[cfg(feature = "dim3")] - rhs: Vector5, + rhs: Vector5, #[cfg(feature = "dim3")] - impulse: Vector5, + impulse: Vector5, #[cfg(feature = "dim2")] - inv_lhs: Matrix2, + inv_lhs: Matrix2, #[cfg(feature = "dim2")] - rhs: Vector2, + rhs: Vector2, #[cfg(feature = "dim2")] - impulse: Vector2, + impulse: Vector2, - limits_impulse: f32, - limits_forcedirs: Option<(Vector, Vector)>, - limits_rhs: f32, + limits_impulse: Real, + limits_forcedirs: Option<(Vector, Vector)>, + limits_rhs: Real, #[cfg(feature = "dim2")] - basis1: Vector2, + basis1: Vector2, #[cfg(feature = "dim3")] - basis1: Matrix3x2, + basis1: Matrix3x2, - im1: f32, - im2: f32, + im1: Real, + im2: Real, - ii1_sqrt: AngularInertia, - ii2_sqrt: AngularInertia, + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, } impl PrismaticVelocityConstraint { @@ -191,7 +191,7 @@ impl PrismaticVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; @@ -220,7 +220,7 @@ impl PrismaticVelocityConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; @@ -295,34 +295,34 @@ pub(crate) struct PrismaticVelocityGroundConstraint { joint_id: JointIndex, - r2: Vector, + r2: Vector, #[cfg(feature = "dim2")] - inv_lhs: Matrix2, + inv_lhs: Matrix2, #[cfg(feature = "dim2")] - rhs: Vector2, + rhs: Vector2, #[cfg(feature = "dim2")] - impulse: Vector2, + impulse: Vector2, #[cfg(feature = "dim3")] - inv_lhs: Matrix5, + inv_lhs: Matrix5, #[cfg(feature = "dim3")] - rhs: Vector5, + rhs: Vector5, #[cfg(feature = "dim3")] - impulse: Vector5, + impulse: Vector5, - limits_impulse: f32, - limits_rhs: f32, + limits_impulse: Real, + limits_rhs: Real, - axis2: Vector, + axis2: Vector, #[cfg(feature = "dim2")] - basis1: Vector2, + basis1: Vector2, #[cfg(feature = "dim3")] - basis1: Matrix3x2, - limits_forcedir2: Option>, + basis1: Matrix3x2, + limits_forcedir2: Option>, - im2: f32, - ii2_sqrt: AngularInertia, + im2: Real, + ii2_sqrt: AngularInertia, } impl PrismaticVelocityGroundConstraint { @@ -478,7 +478,7 @@ impl PrismaticVelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let lin_impulse = self.basis1 * self.impulse.fixed_rows::(0).into_owned(); @@ -499,7 +499,7 @@ impl PrismaticVelocityGroundConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; /* diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs index dadf5da..8e0a1ab 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs @@ -5,7 +5,7 @@ use crate::dynamics::{ IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody, }; use crate::math::{ - AngVector, AngularInertia, Isometry, Point, SimdBool, SimdReal, Vector, SIMD_WIDTH, + AngVector, AngularInertia, Isometry, Point, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[cfg(feature = "dim3")] @@ -236,7 +236,7 @@ impl WPrismaticVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -285,7 +285,7 @@ impl WPrismaticVelocityConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -585,7 +585,7 @@ impl WPrismaticVelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], @@ -616,7 +616,7 @@ impl WPrismaticVelocityGroundConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs index 876ce31..e46e0e4 100644 --- a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs @@ -1,5 +1,5 @@ use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody}; -use crate::math::{AngularInertia, Isometry, Point, Rotation, Vector}; +use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector}; use crate::utils::WAngularInertia; use na::Unit; @@ -8,20 +8,20 @@ pub(crate) struct RevolutePositionConstraint { position1: usize, position2: usize, - im1: f32, - im2: f32, + im1: Real, + im2: Real, - ii1: AngularInertia, - ii2: AngularInertia, + ii1: AngularInertia, + ii2: AngularInertia, - lin_inv_lhs: f32, - ang_inv_lhs: AngularInertia, + lin_inv_lhs: Real, + ang_inv_lhs: AngularInertia, - local_anchor1: Point, - local_anchor2: Point, + local_anchor1: Point, + local_anchor2: Point, - local_axis1: Unit>, - local_axis2: Unit>, + local_axis1: Unit>, + local_axis2: Unit>, } impl RevolutePositionConstraint { @@ -49,7 +49,7 @@ impl RevolutePositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position1 = positions[self.position1 as usize]; let mut position2 = positions[self.position2 as usize]; @@ -83,10 +83,10 @@ impl RevolutePositionConstraint { #[derive(Debug)] pub(crate) struct RevolutePositionGroundConstraint { position2: usize, - anchor1: Point, - local_anchor2: Point, - axis1: Unit>, - local_axis2: Unit>, + anchor1: Point, + local_anchor2: Point, + axis1: Unit>, + local_axis2: Unit>, } impl RevolutePositionGroundConstraint { @@ -122,7 +122,7 @@ impl RevolutePositionGroundConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position2 = positions[self.position2 as usize]; let axis2 = position2 * self.local_axis2; diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index caf8e40..e60bd8a 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody, }; -use crate::math::{AngularInertia, Vector}; +use crate::math::{AngularInertia, Real, Vector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; @@ -13,20 +13,20 @@ pub(crate) struct RevoluteVelocityConstraint { joint_id: JointIndex, - r1: Vector, - r2: Vector, + r1: Vector, + r2: Vector, - inv_lhs: Matrix5, - rhs: Vector5, - impulse: Vector5, + inv_lhs: Matrix5, + rhs: Vector5, + impulse: Vector5, - basis1: Matrix3x2, + basis1: Matrix3x2, - im1: f32, - im2: f32, + im1: Real, + im2: Real, - ii1_sqrt: AngularInertia, - ii2_sqrt: AngularInertia, + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, } impl RevoluteVelocityConstraint { @@ -99,7 +99,7 @@ impl RevoluteVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; @@ -120,7 +120,7 @@ impl RevoluteVelocityConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; @@ -165,17 +165,17 @@ pub(crate) struct RevoluteVelocityGroundConstraint { joint_id: JointIndex, - r2: Vector, + r2: Vector, - inv_lhs: Matrix5, - rhs: Vector5, - impulse: Vector5, + inv_lhs: Matrix5, + rhs: Vector5, + impulse: Vector5, - basis1: Matrix3x2, + basis1: Matrix3x2, - im2: f32, + im2: Real, - ii2_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, } impl RevoluteVelocityGroundConstraint { @@ -249,7 +249,7 @@ impl RevoluteVelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); @@ -263,7 +263,7 @@ impl RevoluteVelocityGroundConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs index 61122a4..1b8af80 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs @@ -4,7 +4,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody, }; -use crate::math::{AngVector, AngularInertia, Isometry, Point, SimdReal, Vector, SIMD_WIDTH}; +use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, SIMD_WIDTH}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; @@ -123,7 +123,7 @@ impl WRevoluteVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -164,7 +164,7 @@ impl WRevoluteVelocityConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -330,7 +330,7 @@ impl WRevoluteVelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], @@ -354,7 +354,7 @@ impl WRevoluteVelocityGroundConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs index 3b7ab9f..543b9b7 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -2,7 +2,7 @@ use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver}; use crate::dynamics::solver::ParallelPositionSolver; use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; -use crate::math::Isometry; +use crate::math::{Isometry, Real}; use crate::utils::WAngularInertia; use rayon::Scope; use std::sync::atomic::{AtomicUsize, Ordering}; @@ -119,8 +119,8 @@ impl ThreadContext { } pub struct ParallelIslandSolver { - mj_lambdas: Vec>, - positions: Vec>, + mj_lambdas: Vec>, + positions: Vec>, parallel_groups: ParallelInteractionGroups, parallel_joint_groups: ParallelInteractionGroups, parallel_velocity_solver: ParallelVelocitySolver, @@ -199,9 +199,9 @@ impl ParallelIslandSolver { scope.spawn(move |_| { // Transmute *mut -> &mut - let mj_lambdas: &mut Vec> = + let mj_lambdas: &mut Vec> = unsafe { std::mem::transmute(mj_lambdas.load(Ordering::Relaxed)) }; - let positions: &mut Vec> = + let positions: &mut Vec> = unsafe { std::mem::transmute(positions.load(Ordering::Relaxed)) }; let bodies: &mut RigidBodySet = unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) }; diff --git a/src/dynamics/solver/parallel_position_solver.rs b/src/dynamics/solver/parallel_position_solver.rs index b7a7876..dfcd0d4 100644 --- a/src/dynamics/solver/parallel_position_solver.rs +++ b/src/dynamics/solver/parallel_position_solver.rs @@ -4,7 +4,7 @@ use crate::dynamics::solver::categorization::{categorize_joints, categorize_posi use crate::dynamics::solver::{InteractionGroups, PositionConstraint, PositionGroundConstraint}; use crate::dynamics::{IntegrationParameters, JointGraphEdge, RigidBodySet}; use crate::geometry::ContactManifold; -use crate::math::Isometry; +use crate::math::{Isometry, Real}; #[cfg(feature = "simd-is-enabled")] use crate::{ dynamics::solver::{WPositionConstraint, WPositionGroundConstraint}, @@ -500,7 +500,7 @@ impl ParallelPositionSolver { &mut self, thread: &ThreadContext, params: &IntegrationParameters, - positions: &mut [Isometry], + positions: &mut [Isometry], ) { if self.part.constraint_descs.len() == 0 { return; diff --git a/src/dynamics/solver/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs index 6e8c681..ca2a987 100644 --- a/src/dynamics/solver/parallel_velocity_solver.rs +++ b/src/dynamics/solver/parallel_velocity_solver.rs @@ -317,7 +317,7 @@ impl ParallelVelocitySolver { params: &IntegrationParameters, manifolds_all: &mut [&mut ContactManifold], joints_all: &mut [JointGraphEdge], - mj_lambdas: &mut [DeltaVel], + mj_lambdas: &mut [DeltaVel], ) { if self.part.constraint_descs.len() == 0 && self.joint_part.constraint_descs.len() == 0 { return; diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs index 843fd1a..e7188f8 100644 --- a/src/dynamics/solver/position_constraint.rs +++ b/src/dynamics/solver/position_constraint.rs @@ -4,7 +4,7 @@ use crate::dynamics::solver::{WPositionConstraint, WPositionGroundConstraint}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::ContactManifold; use crate::math::{ - AngularInertia, Isometry, Point, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS, + AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS, }; use crate::utils::{WAngularInertia, WCross, WDot}; @@ -20,7 +20,7 @@ pub(crate) enum AnyPositionConstraint { } impl AnyPositionConstraint { - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { match self { #[cfg(feature = "simd-is-enabled")] AnyPositionConstraint::GroupedGround(c) => c.solve(params, positions), @@ -37,17 +37,17 @@ pub(crate) struct PositionConstraint { pub rb1: usize, pub rb2: usize, // NOTE: the points are relative to the center of masses. - pub local_p1: [Point; MAX_MANIFOLD_POINTS], - pub local_p2: [Point; MAX_MANIFOLD_POINTS], - pub dists: [f32; MAX_MANIFOLD_POINTS], - pub local_n1: Vector, + pub local_p1: [Point; MAX_MANIFOLD_POINTS], + pub local_p2: [Point; MAX_MANIFOLD_POINTS], + pub dists: [Real; MAX_MANIFOLD_POINTS], + pub local_n1: Vector, pub num_contacts: u8, - pub im1: f32, - pub im2: f32, - pub ii1: AngularInertia, - pub ii2: AngularInertia, - pub erp: f32, - pub max_linear_correction: f32, + pub im1: Real, + pub im2: Real, + pub ii1: AngularInertia, + pub ii2: AngularInertia, + pub erp: Real, + pub max_linear_correction: Real, } impl PositionConstraint { @@ -112,7 +112,7 @@ impl PositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { // FIXME: can we avoid most of the multiplications by pos1/pos2? // Compute jacobians. let mut pos1 = positions[self.rb1]; diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs index 7df7d5e..2bb74c9 100644 --- a/src/dynamics/solver/position_constraint_wide.rs +++ b/src/dynamics/solver/position_constraint_wide.rs @@ -2,8 +2,8 @@ use super::AnyPositionConstraint; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::ContactManifold; use crate::math::{ - AngularInertia, Isometry, Point, Rotation, SimdReal, Translation, Vector, MAX_MANIFOLD_POINTS, - SIMD_WIDTH, + AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Translation, Vector, + MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WDot}; @@ -94,7 +94,7 @@ impl WPositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { // FIXME: can we avoid most of the multiplications by pos1/pos2? // Compute jacobians. let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]); diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs index fcfdcdb..a8523f5 100644 --- a/src/dynamics/solver/position_ground_constraint.rs +++ b/src/dynamics/solver/position_ground_constraint.rs @@ -2,22 +2,22 @@ use super::AnyPositionConstraint; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::ContactManifold; use crate::math::{ - AngularInertia, Isometry, Point, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS, + AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS, }; use crate::utils::{WAngularInertia, WCross, WDot}; pub(crate) struct PositionGroundConstraint { pub rb2: usize, // NOTE: the points are relative to the center of masses. - pub p1: [Point; MAX_MANIFOLD_POINTS], - pub local_p2: [Point; MAX_MANIFOLD_POINTS], - pub dists: [f32; MAX_MANIFOLD_POINTS], - pub n1: Vector, + pub p1: [Point; MAX_MANIFOLD_POINTS], + pub local_p2: [Point; MAX_MANIFOLD_POINTS], + pub dists: [Real; MAX_MANIFOLD_POINTS], + pub n1: Vector, pub num_contacts: u8, - pub im2: f32, - pub ii2: AngularInertia, - pub erp: f32, - pub max_linear_correction: f32, + pub im2: Real, + pub ii2: AngularInertia, + pub erp: Real, + pub max_linear_correction: Real, } impl PositionGroundConstraint { @@ -79,7 +79,7 @@ impl PositionGroundConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { // FIXME: can we avoid most of the multiplications by pos1/pos2? // Compute jacobians. let mut pos2 = positions[self.rb2]; diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs index dbea1db..c6f1c3f 100644 --- a/src/dynamics/solver/position_ground_constraint_wide.rs +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -2,8 +2,8 @@ use super::AnyPositionConstraint; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::ContactManifold; use crate::math::{ - AngularInertia, Isometry, Point, Rotation, SimdReal, Translation, Vector, MAX_MANIFOLD_POINTS, - SIMD_WIDTH, + AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Translation, Vector, + MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WDot}; @@ -92,7 +92,7 @@ impl WPositionGroundConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { // FIXME: can we avoid most of the multiplications by pos1/pos2? // Compute jacobians. let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs index cf23168..df0e3fc 100644 --- a/src/dynamics/solver/position_solver.rs +++ b/src/dynamics/solver/position_solver.rs @@ -1,9 +1,9 @@ use super::AnyJointPositionConstraint; use crate::dynamics::{solver::AnyPositionConstraint, IntegrationParameters, RigidBodySet}; -use crate::math::Isometry; +use crate::math::{Isometry, Real}; pub(crate) struct PositionSolver { - positions: Vec>, + positions: Vec>, } impl PositionSolver { diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs index 92faa3b..1965d6d 100644 --- a/src/dynamics/solver/solver_constraints.rs +++ b/src/dynamics/solver/solver_constraints.rs @@ -2,11 +2,12 @@ use super::{ AnyJointVelocityConstraint, InteractionGroups, VelocityConstraint, VelocityGroundConstraint, }; #[cfg(feature = "simd-is-enabled")] -use super::{WVelocityConstraint, WVelocityGroundConstraint}; +use super::{ + WPositionConstraint, WPositionGroundConstraint, WVelocityConstraint, WVelocityGroundConstraint, +}; use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints}; use crate::dynamics::solver::{ - AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint, - PositionGroundConstraint, WPositionConstraint, WPositionGroundConstraint, + AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint, PositionGroundConstraint, }; use crate::dynamics::{ solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 6614b48..2cb3975 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -4,7 +4,7 @@ use crate::dynamics::solver::VelocityGroundConstraint; use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; -use crate::math::{AngVector, Vector, DIM, MAX_MANIFOLD_POINTS}; +use crate::math::{AngVector, Real, Vector, DIM, MAX_MANIFOLD_POINTS}; use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; use simba::simd::SimdPartialOrd; @@ -40,7 +40,7 @@ impl AnyVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { match self { AnyVelocityConstraint::NongroupedGround(c) => c.warmstart(mj_lambdas), AnyVelocityConstraint::Nongrouped(c) => c.warmstart(mj_lambdas), @@ -52,7 +52,7 @@ impl AnyVelocityConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { match self { AnyVelocityConstraint::NongroupedGround(c) => c.solve(mj_lambdas), AnyVelocityConstraint::Nongrouped(c) => c.solve(mj_lambdas), @@ -79,11 +79,11 @@ impl AnyVelocityConstraint { #[derive(Copy, Clone, Debug)] pub(crate) struct VelocityConstraintElementPart { - pub gcross1: AngVector, - pub gcross2: AngVector, - pub rhs: f32, - pub impulse: f32, - pub r: f32, + pub gcross1: AngVector, + pub gcross2: AngVector, + pub rhs: Real, + pub impulse: Real, + pub r: Real, } #[cfg(not(target_arch = "wasm32"))] @@ -117,10 +117,10 @@ impl VelocityConstraintElement { #[derive(Copy, Clone, Debug)] pub(crate) struct VelocityConstraint { - pub dir1: Vector, // Non-penetration force direction for the first body. - pub im1: f32, - pub im2: f32, - pub limit: f32, + pub dir1: Vector, // Non-penetration force direction for the first body. + pub im1: Real, + pub im2: Real, + pub limit: Real, pub mj_lambda1: usize, pub mj_lambda2: usize, pub manifold_id: ContactManifoldIndex, @@ -300,7 +300,7 @@ impl VelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel::zero(); let mut mj_lambda2 = DeltaVel::zero(); @@ -331,7 +331,7 @@ impl VelocityConstraint { mj_lambdas[self.mj_lambda2 as usize].angular += mj_lambda2.angular; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index c75d926..b86e6a6 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -2,7 +2,7 @@ use super::{AnyVelocityConstraint, DeltaVel}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ - AngVector, AngularInertia, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, + AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; use num::Zero; @@ -199,7 +199,7 @@ impl WVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -249,7 +249,7 @@ impl WVelocityConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index fa2a23e..37d4e3a 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -1,5 +1,5 @@ use super::{AnyVelocityConstraint, DeltaVel}; -use crate::math::{AngVector, Vector, DIM, MAX_MANIFOLD_POINTS}; +use crate::math::{AngVector, Real, Vector, DIM, MAX_MANIFOLD_POINTS}; use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; @@ -8,10 +8,10 @@ use simba::simd::SimdPartialOrd; #[derive(Copy, Clone, Debug)] pub(crate) struct VelocityGroundConstraintElementPart { - pub gcross2: AngVector, - pub rhs: f32, - pub impulse: f32, - pub r: f32, + pub gcross2: AngVector, + pub rhs: Real, + pub impulse: Real, + pub r: Real, } #[cfg(not(target_arch = "wasm32"))] @@ -44,9 +44,9 @@ impl VelocityGroundConstraintElement { #[derive(Copy, Clone, Debug)] pub(crate) struct VelocityGroundConstraint { - pub dir1: Vector, // Non-penetration force direction for the first body. - pub im2: f32, - pub limit: f32, + pub dir1: Vector, // Non-penetration force direction for the first body. + pub im2: Real, + pub limit: Real, pub mj_lambda2: usize, pub manifold_id: ContactManifoldIndex, pub manifold_contact_id: usize, @@ -207,7 +207,7 @@ impl VelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel::zero(); let tangents1 = self.dir1.orthonormal_basis(); @@ -227,7 +227,7 @@ impl VelocityGroundConstraint { mj_lambdas[self.mj_lambda2 as usize].angular += mj_lambda2.angular; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; // Solve friction. diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index feee344..1c85a90 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -2,7 +2,7 @@ use super::{AnyVelocityConstraint, DeltaVel}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ - AngVector, AngularInertia, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, + AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; use num::Zero; @@ -189,7 +189,7 @@ impl WVelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], @@ -219,7 +219,7 @@ impl WVelocityGroundConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { linear: Vector::from( array![ |ii| mj_lambdas[ self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index 332d809..dfb97b0 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -4,10 +4,11 @@ use crate::dynamics::{ IntegrationParameters, JointGraphEdge, RigidBodySet, }; use crate::geometry::ContactManifold; +use crate::math::Real; use crate::utils::WAngularInertia; pub(crate) struct VelocitySolver { - pub mj_lambdas: Vec>, + pub mj_lambdas: Vec>, } impl VelocitySolver { diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs index dbcc271..1ec4372 100644 --- a/src/geometry/broad_phase_multi_sap.rs +++ b/src/geometry/broad_phase_multi_sap.rs @@ -1,7 +1,7 @@ use crate::data::pubsub::Subscription; use crate::dynamics::RigidBodySet; use crate::geometry::{ColliderHandle, ColliderSet, RemovedCollider}; -use crate::math::{Point, Vector, DIM}; +use crate::math::{Point, Real, Vector, DIM}; use bit_vec::BitVec; use cdl::bounding_volume::{BoundingVolume, AABB}; use cdl::utils::hashmap::HashMap; @@ -10,8 +10,8 @@ use std::ops::{Index, IndexMut}; const NUM_SENTINELS: usize = 1; const NEXT_FREE_SENTINEL: u32 = u32::MAX; -const SENTINEL_VALUE: f32 = f32::MAX; -const CELL_WIDTH: f32 = 20.0; +const SENTINEL_VALUE: Real = Real::MAX; +const CELL_WIDTH: Real = 20.0; #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -63,12 +63,12 @@ fn sort2(a: u32, b: u32) -> (u32, u32) { } } -fn point_key(point: Point) -> Point { +fn point_key(point: Point) -> Point { (point / CELL_WIDTH).coords.map(|e| e.floor() as i32).into() } fn region_aabb(index: Point) -> AABB { - let mins = index.coords.map(|i| i as f32 * CELL_WIDTH).into(); + let mins = index.coords.map(|i| i as Real * CELL_WIDTH).into(); let maxs = mins + Vector::repeat(CELL_WIDTH); AABB::new(mins, maxs) } @@ -76,7 +76,7 @@ fn region_aabb(index: Point) -> AABB { #[derive(Copy, Clone, Debug, PartialEq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] struct Endpoint { - value: f32, + value: Real, packed_flag_proxy: u32, } @@ -86,14 +86,14 @@ const START_SENTINEL_TAG: u32 = u32::MAX; const END_SENTINEL_TAG: u32 = u32::MAX ^ START_FLAG_MASK; impl Endpoint { - pub fn start_endpoint(value: f32, proxy: u32) -> Self { + pub fn start_endpoint(value: Real, proxy: u32) -> Self { Self { value, packed_flag_proxy: proxy | START_FLAG_MASK, } } - pub fn end_endpoint(value: f32, proxy: u32) -> Self { + pub fn end_endpoint(value: Real, proxy: u32) -> Self { Self { value, packed_flag_proxy: proxy & PROXY_MASK, @@ -134,15 +134,15 @@ impl Endpoint { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone)] struct SAPAxis { - min_bound: f32, - max_bound: f32, + min_bound: Real, + max_bound: Real, endpoints: Vec, #[cfg_attr(feature = "serde-serialize", serde(skip))] new_endpoints: Vec<(Endpoint, usize)>, // Workspace } impl SAPAxis { - fn new(min_bound: f32, max_bound: f32) -> Self { + fn new(min_bound: Real, max_bound: Real) -> Self { assert!(min_bound <= max_bound); Self { @@ -620,7 +620,7 @@ impl BroadPhase { pub(crate) fn update_aabbs( &mut self, - prediction_distance: f32, + prediction_distance: Real, bodies: &RigidBodySet, colliders: &mut ColliderSet, ) { diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index b2ae812..129d444 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -20,25 +20,25 @@ pub struct ContactData { /// The impulse, along the contact normal, applied by this contact to the first collider's rigid-body. /// /// The impulse applied to the second collider's rigid-body is given by `-impulse`. - pub impulse: f32, + pub impulse: Real, /// The friction impulse along the vector orthonormal to the contact normal, applied to the first /// collider's rigid-body. #[cfg(feature = "dim2")] - pub tangent_impulse: f32, + pub tangent_impulse: Real, /// The friction impulses along the basis orthonormal to the contact normal, applied to the first /// collider's rigid-body. #[cfg(feature = "dim3")] - pub tangent_impulse: [f32; 2], + pub tangent_impulse: [Real; 2], } impl ContactData { #[cfg(feature = "dim2")] - pub(crate) fn zero_tangent_impulse() -> f32 { + pub(crate) fn zero_tangent_impulse() -> Real { 0.0 } #[cfg(feature = "dim3")] - pub(crate) fn zero_tangent_impulse() -> [f32; 2] { + pub(crate) fn zero_tangent_impulse() -> [Real; 2] { [0.0, 0.0] } } @@ -87,7 +87,7 @@ pub struct ContactManifoldData { // The following are set by the narrow-phase. /// The pair of body involved in this contact manifold. pub body_pair: BodyPair, - pub(crate) warmstart_multiplier: f32, + pub(crate) warmstart_multiplier: Real, // The two following are set by the constraints solver. pub(crate) constraint_index: usize, pub(crate) position_constraint_index: usize, @@ -140,7 +140,7 @@ impl ContactManifoldData { self.solver_contacts.len() } - pub(crate) fn min_warmstart_multiplier() -> f32 { + pub(crate) fn min_warmstart_multiplier() -> Real { // Multiplier used to reduce the amount of warm-starting. // This coefficient increases exponentially over time, until it reaches 1.0. // This will reduce significant overshoot at the timesteps that diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index b94ce6f..3fda8a6 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -10,7 +10,7 @@ use crate::geometry::{ ProximityPairFilter, RemovedCollider, SolverContact, SolverFlags, }; use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph}; -use crate::math::Vector; +use crate::math::{Real, Vector}; use crate::pipeline::EventHandler; use cdl::query::{DefaultQueryDispatcher, PersistentQueryDispatcher}; use std::collections::HashMap; @@ -452,7 +452,7 @@ impl NarrowPhase { pub(crate) fn compute_contacts( &mut self, - prediction_distance: f32, + prediction_distance: Real, bodies: &RigidBodySet, colliders: &ColliderSet, pair_filter: Option<&dyn ContactPairFilter>, diff --git a/src/lib.rs b/src/lib.rs index 5ae57f8..476f537 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -11,10 +11,15 @@ // FIXME: deny that #![allow(missing_docs)] -#[cfg(feature = "dim2")] +#[cfg(all(feature = "dim2", feature = "f32"))] pub extern crate cdl2d as cdl; -#[cfg(feature = "dim3")] +#[cfg(all(feature = "dim2", feature = "f64"))] +pub extern crate cdl2d_f64 as cdl; +#[cfg(all(feature = "dim3", feature = "f32"))] pub extern crate cdl3d as cdl; +#[cfg(all(feature = "dim3", feature = "f64"))] +pub extern crate cdl3d_f64 as cdl; + pub extern crate crossbeam; pub extern crate nalgebra as na; #[cfg(feature = "serde")] diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index 06c581b..8855af0 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -5,6 +5,7 @@ use crate::geometry::{ BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactPairFilter, NarrowPhase, ProximityPairFilter, }; +use crate::math::Real; use crate::pipeline::EventHandler; /// The collision pipeline, responsible for performing collision detection between colliders. @@ -38,7 +39,7 @@ impl CollisionPipeline { /// Executes one step of the collision detection. pub fn step( &mut self, - prediction_distance: f32, + prediction_distance: Real, broad_phase: &mut BroadPhase, narrow_phase: &mut NarrowPhase, bodies: &mut RigidBodySet, diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index e56f8e0..718dd10 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -10,7 +10,7 @@ use crate::geometry::{ BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, ContactPairFilter, NarrowPhase, ProximityPairFilter, }; -use crate::math::Vector; +use crate::math::{Real, Vector}; use crate::pipeline::EventHandler; /// The physics pipeline, responsible for stepping the whole physics simulation. @@ -62,7 +62,7 @@ impl PhysicsPipeline { /// Executes one timestep of the physics simulation. pub fn step( &mut self, - gravity: &Vector, + gravity: &Vector, integration_parameters: &IntegrationParameters, broad_phase: &mut BroadPhase, narrow_phase: &mut NarrowPhase, @@ -250,7 +250,7 @@ impl PhysicsPipeline { mod test { use crate::dynamics::{IntegrationParameters, JointSet, RigidBodyBuilder, RigidBodySet}; use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet, NarrowPhase}; - use crate::math::Vector; + use crate::math::{Real, Vector}; use crate::pipeline::PhysicsPipeline; #[test] diff --git a/src/utils.rs b/src/utils.rs index 932c6e2..3f76c8b 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -6,9 +6,13 @@ use num::Zero; use simba::simd::SimdValue; use cdl::utils::SdpMatrix3; -use {crate::math::SimdReal, na::SimdPartialOrd, num::One}; +use { + crate::math::{Real, SimdReal}, + na::SimdPartialOrd, + num::One, +}; -pub(crate) fn inv(val: f32) -> f32 { +pub(crate) fn inv(val: Real) -> Real { if val == 0.0 { 0.0 } else { @@ -23,10 +27,11 @@ pub trait WSign: Sized { fn copy_sign_to(self, to: Rhs) -> Rhs; } -impl WSign for f32 { +impl WSign for Real { fn copy_sign_to(self, to: Self) -> Self { - let signbit: u32 = (-0.0f32).to_bits(); - f32::from_bits((signbit & self.to_bits()) | ((!signbit) & to.to_bits())) + const MINUS_ZERO: Real = -0.0; + let signbit = MINUS_ZERO.to_bits(); + Real::from_bits((signbit & self.to_bits()) | ((!signbit) & to.to_bits())) } } @@ -75,8 +80,8 @@ pub(crate) trait WComponent: Sized { fn max_component(self) -> Self::Element; } -impl WComponent for f32 { - type Element = f32; +impl WComponent for Real { + type Element = Real; fn min_component(self) -> Self::Element { self @@ -87,7 +92,7 @@ impl WComponent for f32 { } impl WComponent for SimdReal { - type Element = f32; + type Element = Real; fn min_component(self) -> Self::Element { self.simd_horizontal_min() @@ -221,8 +226,8 @@ pub(crate) trait WCrossMatrix: Sized { fn gcross_matrix(self) -> Self::CrossMat; } -impl WCrossMatrix for Vector3 { - type CrossMat = Matrix3; +impl WCrossMatrix for Vector3 { + type CrossMat = Matrix3; #[inline] #[rustfmt::skip] @@ -235,8 +240,8 @@ impl WCrossMatrix for Vector3 { } } -impl WCrossMatrix for Vector2 { - type CrossMat = Vector2; +impl WCrossMatrix for Vector2 { + type CrossMat = Vector2; #[inline] fn gcross_matrix(self) -> Self::CrossMat { @@ -249,26 +254,26 @@ pub(crate) trait WCross: Sized { fn gcross(&self, rhs: Rhs) -> Self::Result; } -impl WCross> for Vector3 { +impl WCross> for Vector3 { type Result = Self; - fn gcross(&self, rhs: Vector3) -> Self::Result { + fn gcross(&self, rhs: Vector3) -> Self::Result { self.cross(&rhs) } } -impl WCross> for Vector2 { - type Result = f32; +impl WCross> for Vector2 { + type Result = Real; - fn gcross(&self, rhs: Vector2) -> Self::Result { + fn gcross(&self, rhs: Vector2) -> Self::Result { self.x * rhs.y - self.y * rhs.x } } -impl WCross> for f32 { - type Result = Vector2; +impl WCross> for Real { + type Result = Vector2; - fn gcross(&self, rhs: Vector2) -> Self::Result { + fn gcross(&self, rhs: Vector2) -> Self::Result { Vector2::new(-rhs.y * *self, rhs.x * *self) } } @@ -278,26 +283,26 @@ pub(crate) trait WDot: Sized { fn gdot(&self, rhs: Rhs) -> Self::Result; } -impl WDot> for Vector3 { - type Result = f32; +impl WDot> for Vector3 { + type Result = Real; - fn gdot(&self, rhs: Vector3) -> Self::Result { + fn gdot(&self, rhs: Vector3) -> Self::Result { self.x * rhs.x + self.y * rhs.y + self.z * rhs.z } } -impl WDot> for Vector2 { - type Result = f32; +impl WDot> for Vector2 { + type Result = Real; - fn gdot(&self, rhs: Vector2) -> Self::Result { + fn gdot(&self, rhs: Vector2) -> Self::Result { self.x * rhs.x + self.y * rhs.y } } -impl WDot for f32 { - type Result = f32; +impl WDot for Real { + type Result = Real; - fn gdot(&self, rhs: f32) -> Self::Result { + fn gdot(&self, rhs: Real) -> Self::Result { *self * rhs } } @@ -387,10 +392,10 @@ pub(crate) trait WAngularInertia { fn into_matrix(self) -> Self::AngMatrix; } -impl WAngularInertia for f32 { - type AngVector = f32; - type LinVector = Vector2; - type AngMatrix = f32; +impl WAngularInertia for Real { + type AngVector = Real; + type LinVector = Vector2; + type AngMatrix = Real; fn inverse(&self) -> Self { if *self != 0.0 { @@ -400,14 +405,14 @@ impl WAngularInertia for f32 { } } - fn transform_lin_vector(&self, pt: Vector2) -> Vector2 { + fn transform_lin_vector(&self, pt: Vector2) -> Vector2 { *self * pt } - fn transform_vector(&self, pt: f32) -> f32 { + fn transform_vector(&self, pt: Real) -> Real { *self * pt } - fn squared(&self) -> f32 { + fn squared(&self) -> Real { *self * *self } @@ -452,10 +457,10 @@ impl WAngularInertia for SimdReal { } } -impl WAngularInertia for SdpMatrix3 { - type AngVector = Vector3; - type LinVector = Vector3; - type AngMatrix = Matrix3; +impl WAngularInertia for SdpMatrix3 { + type AngVector = Vector3; + type LinVector = Vector3; + type AngMatrix = Matrix3; fn inverse(&self) -> Self { let minor_m12_m23 = self.m22 * self.m33 - self.m23 * self.m23; @@ -490,11 +495,11 @@ impl WAngularInertia for SdpMatrix3 { } } - fn transform_lin_vector(&self, v: Vector3) -> Vector3 { + fn transform_lin_vector(&self, v: Vector3) -> Vector3 { self.transform_vector(v) } - fn transform_vector(&self, v: Vector3) -> Vector3 { + fn transform_vector(&self, v: Vector3) -> Vector3 { let x = self.m11 * v.x + self.m12 * v.y + self.m13 * v.z; let y = self.m12 * v.x + self.m22 * v.y + self.m23 * v.z; let z = self.m13 * v.x + self.m23 * v.y + self.m33 * v.z; @@ -502,7 +507,7 @@ impl WAngularInertia for SdpMatrix3 { } #[rustfmt::skip] - fn into_matrix(self) -> Matrix3 { + fn into_matrix(self) -> Matrix3 { Matrix3::new( self.m11, self.m12, self.m13, self.m12, self.m22, self.m23, @@ -511,7 +516,7 @@ impl WAngularInertia for SdpMatrix3 { } #[rustfmt::skip] - fn transform_matrix(&self, m: &Matrix3) -> Matrix3 { + fn transform_matrix(&self, m: &Matrix3) -> Matrix3 { *self * *m } }