nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add support of 64-bits reals.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-01-04 15:14:25 +01:00
parent a1aa8855f7
commit aa61fe65e3
55 changed files with 656 additions and 518 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
Cargo.toml
View File

@@ -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

2
benchmarks2d/Cargo.toml
View File

@@ -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"

2
benchmarks3d/Cargo.toml
View File

@@ -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"

57
build/rapier2d-f64/Cargo.toml Normal file
View File

@@ -0,0 +1,57 @@
[package]
name = "rapier2d-f64"
version = "0.4.2"
authors = [ "Sébastien Crozet <developer@crozet.re>" ]
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" ] }

7
build/rapier2d/Cargo.toml
View File

@@ -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"

56
build/rapier3d-f64/Cargo.toml Normal file
View File

@@ -0,0 +1,56 @@
[package]
name = "rapier3d-f64"
version = "0.4.2"
authors = [ "Sébastien Crozet <developer@crozet.re>" ]
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" ] }

7
build/rapier3d/Cargo.toml
View File

@@ -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"

2
build/rapier_testbed2d/Cargo.toml
View File

@@ -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"

2
build/rapier_testbed3d/Cargo.toml
View File

@@ -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"

2
examples2d/Cargo.toml
View File

@@ -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"

2
examples3d/Cargo.toml
View File

@@ -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"

56
src/dynamics/integration_parameters.rs
View File

@@ -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

16
src/dynamics/joint/ball_joint.rs
View File

@@ -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<f32>,
pub local_anchor1: Point<Real>,
/// Where the ball joint is attached on the first body, expressed in the first body local frame.
pub local_anchor2: Point<f32>,
pub local_anchor2: Point<Real>,
/// The impulse applied by this joint on the first body.
///
/// The impulse applied to the second body is given by `-impulse`.
pub impulse: Vector<f32>,
pub impulse: Vector<Real>,
}
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<f32>, local_anchor2: Point<f32>) -> Self {
pub fn new(local_anchor1: Point<Real>, local_anchor2: Point<Real>) -> Self {
Self::with_impulse(local_anchor1, local_anchor2, Vector::zeros())
}
pub(crate) fn with_impulse(
local_anchor1: Point<f32>,
local_anchor2: Point<f32>,
impulse: Vector<f32>,
local_anchor1: Point<Real>,
local_anchor2: Point<Real>,
impulse: Vector<Real>,
) -> Self {
Self {
local_anchor1,

10
src/dynamics/joint/fixed_joint.rs
View File

@@ -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<f32>,
pub local_anchor1: Isometry<Real>,
/// 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<f32>,
pub local_anchor2: Isometry<Real>,
/// 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<f32>,
pub impulse: SpacialVector<Real>,
}
impl FixedJoint {
/// Creates a new fixed joint from the frames of reference of both bodies.
pub fn new(local_anchor1: Isometry<f32>, local_anchor2: Isometry<f32>) -> Self {
pub fn new(local_anchor1: Isometry<Real>, local_anchor2: Isometry<Real>) -> Self {
Self {
local_anchor1,
local_anchor2,

68
src/dynamics/joint/prismatic_joint.rs
View File

@@ -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<f32>,
pub local_anchor1: Point<Real>,
/// Where the prismatic joint is attached on the second body, expressed in the local space of the second attached body.
pub local_anchor2: Point<f32>,
pub(crate) local_axis1: Unit<Vector<f32>>,
pub(crate) local_axis2: Unit<Vector<f32>>,
pub(crate) basis1: [Vector<f32>; DIM - 1],
pub(crate) basis2: [Vector<f32>; DIM - 1],
pub local_anchor2: Point<Real>,
pub(crate) local_axis1: Unit<Vector<Real>>,
pub(crate) local_axis2: Unit<Vector<Real>>,
pub(crate) basis1: [Vector<Real>; DIM - 1],
pub(crate) basis2: [Vector<Real>; 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<f32>,
pub impulse: Vector5<Real>,
/// 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<f32>,
pub impulse: Vector2<Real>,
/// 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<f32>,
local_axis1: Unit<Vector<f32>>,
local_anchor2: Point<f32>,
local_axis2: Unit<Vector<f32>>,
local_anchor1: Point<Real>,
local_axis1: Unit<Vector<Real>>,
local_anchor2: Point<Real>,
local_axis2: Unit<Vector<Real>>,
) -> 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<f32>,
local_axis1: Unit<Vector<f32>>,
local_tangent1: Vector<f32>,
local_anchor2: Point<f32>,
local_axis2: Unit<Vector<f32>>,
local_tangent2: Vector<f32>,
local_anchor1: Point<Real>,
local_axis1: Unit<Vector<Real>>,
local_tangent1: Vector<Real>,
local_anchor2: Point<Real>,
local_axis2: Unit<Vector<Real>>,
local_tangent2: Vector<Real>,
) -> 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<Vector<f32>> {
pub fn local_axis1(&self) -> Unit<Vector<Real>> {
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<Vector<f32>> {
pub fn local_axis2(&self) -> Unit<Vector<Real>> {
self.local_axis2
}
// FIXME: precompute this?
#[cfg(feature = "dim2")]
pub(crate) fn local_frame1(&self) -> Isometry<f32> {
pub(crate) fn local_frame1(&self) -> Isometry<Real> {
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<f32> {
pub(crate) fn local_frame2(&self) -> Isometry<Real> {
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<f32> {
pub(crate) fn local_frame1(&self) -> Isometry<Real> {
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<f32> {
pub(crate) fn local_frame2(&self) -> Isometry<Real> {
use na::{Matrix3, Rotation3, UnitQuaternion};
let mat = Matrix3::from_columns(&[

24
src/dynamics/joint/revolute_joint.rs
View File

@@ -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<f32>,
pub local_anchor1: Point<Real>,
/// Where the revolute joint is attached on the second body, expressed in the local space of the second attached body.
pub local_anchor2: Point<f32>,
pub local_anchor2: Point<Real>,
/// The rotation axis of this revolute joint expressed in the local space of the first attached body.
pub local_axis1: Unit<Vector<f32>>,
pub local_axis1: Unit<Vector<Real>>,
/// The rotation axis of this revolute joint expressed in the local space of the second attached body.
pub local_axis2: Unit<Vector<f32>>,
pub local_axis2: Unit<Vector<Real>>,
/// The basis orthonormal to `local_axis1`, expressed in the local space of the first attached body.
pub basis1: [Vector<f32>; 2],
pub basis1: [Vector<Real>; 2],
/// The basis orthonormal to `local_axis2`, expressed in the local space of the second attached body.
pub basis2: [Vector<f32>; 2],
pub basis2: [Vector<Real>; 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<f32>,
pub impulse: Vector5<Real>,
}
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<f32>,
local_axis1: Unit<Vector<f32>>,
local_anchor2: Point<f32>,
local_axis2: Unit<Vector<f32>>,
local_anchor1: Point<Real>,
local_axis1: Unit<Vector<Real>>,
local_anchor2: Point<Real>,
local_axis2: Unit<Vector<Real>>,
) -> Self {
Self {
local_anchor1,

122
src/dynamics/rigid_body.rs
View File

@@ -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<f32>,
pub(crate) predicted_position: Isometry<f32>,
pub(crate) position: Isometry<Real>,
pub(crate) predicted_position: Isometry<Real>,
/// 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<f32>,
pub world_com: Point<Real>,
/// The square-root of the inverse angular inertia tensor of the rigid-body.
pub world_inv_inertia_sqrt: AngularInertia<f32>,
pub world_inv_inertia_sqrt: AngularInertia<Real>,
/// The linear velocity of the rigid-body.
pub(crate) linvel: Vector<f32>,
pub(crate) linvel: Vector<Real>,
/// The angular velocity of the rigid-body.
pub(crate) angvel: AngVector<f32>,
pub(crate) angvel: AngVector<Real>,
/// 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<f32>,
pub(crate) angacc: AngVector<f32>,
pub angular_damping: Real,
pub(crate) linacc: Vector<Real>,
pub(crate) angacc: AngVector<Real>,
pub(crate) colliders: Vec<ColliderHandle>,
/// 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<f32>) {
pub(crate) fn integrate_accelerations(&mut self, dt: Real, gravity: Vector<Real>) {
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<f32> {
pub fn predicted_position(&self) -> &Isometry<Real> {
&self.predicted_position
}
@@ -311,13 +313,13 @@ impl RigidBody {
!self.linvel.is_zero() || !self.angvel.is_zero()
}
fn integrate_velocity(&self, dt: f32) -> Isometry<f32> {
fn integrate_velocity(&self, dt: Real) -> Isometry<Real> {
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<f32> {
pub fn linvel(&self) -> &Vector<Real> {
&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<f32> {
pub fn angvel(&self) -> &Vector<Real> {
&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<f32>, wake_up: bool) {
pub fn set_linvel(&mut self, linvel: Vector<Real>, 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<f32>, wake_up: bool) {
pub fn set_angvel(&mut self, angvel: Vector<Real>, 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<f32> {
pub fn position(&self) -> &Isometry<Real> {
&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<f32>, wake_up: bool) {
pub fn set_position(&mut self, pos: Isometry<Real>, 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<f32>) {
pub(crate) fn set_position_internal(&mut self, pos: Isometry<Real>) {
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<f32>) {
pub fn set_next_kinematic_position(&mut self, pos: Isometry<Real>) {
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<f32>, wake_up: bool) {
pub fn apply_force(&mut self, force: Vector<Real>, 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<f32>, wake_up: bool) {
pub fn apply_impulse(&mut self, impulse: Vector<Real>, 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<f32>, wake_up: bool) {
pub fn apply_torque(&mut self, torque: Vector<Real>, 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<f32>, wake_up: bool) {
pub fn apply_torque_impulse(&mut self, torque_impulse: Vector<Real>, 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<f32>, point: Point<f32>, wake_up: bool) {
pub fn apply_force_at_point(&mut self, force: Vector<Real>, point: Point<Real>, 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<f32>,
point: Point<f32>,
impulse: Vector<Real>,
point: Point<Real>,
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<f32>) -> Vector<f32> {
pub fn velocity_at_point(&self, point: &Point<Real>) -> Vector<Real> {
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<f32>,
linvel: Vector<f32>,
angvel: AngVector<f32>,
linear_damping: f32,
angular_damping: f32,
position: Isometry<Real>,
linvel: Vector<Real>,
angvel: AngVector<Real>,
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<f32>) -> Self {
pub fn rotation(mut self, angle: AngVector<Real>) -> 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<f32>) -> Self {
pub fn position(mut self, pos: Isometry<Real>) -> 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<f32>,
inertia: AngVector<Real>,
colliders_contribution_enabled: AngVector<bool>,
) -> 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<f32>,
inertia: AngVector<Real>,
colliders_contribution_enabled: AngVector<bool>,
) -> 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<f32>) -> Self {
pub fn angvel(mut self, angvel: AngVector<Real>) -> 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
}

4
src/dynamics/solver/delta_vel.rs
View File

@@ -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<N: Scalar> {
pub(crate) struct DeltaVel<N: Scalar + Copy> {
pub linear: Vector<N>,
pub angular: AngVector<N>,
}

2
src/dynamics/solver/interaction_groups.rs
View File

@@ -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,
};

32
src/dynamics/solver/joint_constraint/ball_position_constraint.rs
View File

@@ -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<f32>,
local_com2: Point<f32>,
local_com1: Point<Real>,
local_com2: Point<Real>,
im1: f32,
im2: f32,
im1: Real,
im2: Real,
ii1: AngularInertia<f32>,
ii2: AngularInertia<f32>,
ii1: AngularInertia<Real>,
ii2: AngularInertia<Real>,
local_anchor1: Point<f32>,
local_anchor2: Point<f32>,
local_anchor1: Point<Real>,
local_anchor2: Point<Real>,
}
impl BallPositionConstraint {
@@ -38,7 +38,7 @@ impl BallPositionConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
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<f32>,
im2: f32,
ii2: AngularInertia<f32>,
local_anchor2: Point<f32>,
local_com2: Point<f32>,
anchor1: Point<Real>,
im2: Real,
ii2: AngularInertia<Real>,
local_anchor2: Point<Real>,
local_com2: Point<Real>,
}
impl BallPositionGroundConstraint {
@@ -133,7 +133,7 @@ impl BallPositionGroundConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position2 = positions[self.position2 as usize];
let anchor2 = position2 * self.local_anchor2;

6
src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
View File

@@ -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<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
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<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]);
let anchor2 = position2 * self.local_anchor2;

34
src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
View File

@@ -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<f32>,
pub(crate) impulse: Vector<f32>,
rhs: Vector<Real>,
pub(crate) impulse: Vector<Real>,
gcross1: Vector<f32>,
gcross2: Vector<f32>,
gcross1: Vector<Real>,
gcross2: Vector<Real>,
inv_lhs: SdpMatrix<f32>,
inv_lhs: SdpMatrix<Real>,
im1: f32,
im2: f32,
im1: Real,
im2: Real,
}
impl BallVelocityConstraint {
@@ -91,7 +91,7 @@ impl BallVelocityConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>,
impulse: Vector<f32>,
gcross2: Vector<f32>,
inv_lhs: SdpMatrix<f32>,
im2: f32,
rhs: Vector<Real>,
impulse: Vector<Real>,
gcross2: Vector<Real>,
inv_lhs: SdpMatrix<Real>,
im2: Real,
}
impl BallVelocityGroundConstraint {
@@ -206,14 +206,14 @@ impl BallVelocityGroundConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2);

10
src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
View File

@@ -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<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1: DeltaVel<SimdReal> = 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<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],

38
src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
View File

@@ -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<f32>,
local_anchor2: Isometry<f32>,
local_com1: Point<f32>,
local_com2: Point<f32>,
im1: f32,
im2: f32,
ii1: AngularInertia<f32>,
ii2: AngularInertia<f32>,
local_anchor1: Isometry<Real>,
local_anchor2: Isometry<Real>,
local_com1: Point<Real>,
local_com2: Point<Real>,
im1: Real,
im2: Real,
ii1: AngularInertia<Real>,
ii2: AngularInertia<Real>,
lin_inv_lhs: f32,
ang_inv_lhs: AngularInertia<f32>,
lin_inv_lhs: Real,
ang_inv_lhs: AngularInertia<Real>,
}
impl FixedPositionConstraint {
@@ -44,7 +44,7 @@ impl FixedPositionConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
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<f32>,
local_anchor2: Isometry<f32>,
local_com2: Point<f32>,
im2: f32,
ii2: AngularInertia<f32>,
impulse: f32,
anchor1: Isometry<Real>,
local_anchor2: Isometry<Real>,
local_com2: Point<Real>,
im2: Real,
ii2: AngularInertia<Real>,
impulse: Real,
}
impl FixedPositionGroundConstraint {
@@ -118,7 +118,7 @@ impl FixedPositionGroundConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position2 = positions[self.position2 as usize];
// Angular correction.

54
src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
View File

@@ -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<f32>,
impulse: SpacialVector<Real>,
#[cfg(feature = "dim3")]
inv_lhs: Matrix6<f32>, // FIXME: replace by Cholesky.
inv_lhs: Matrix6<Real>, // FIXME: replace by Cholesky.
#[cfg(feature = "dim3")]
rhs: Vector6<f32>,
rhs: Vector6<Real>,
#[cfg(feature = "dim2")]
inv_lhs: Matrix3<f32>, // FIXME: replace by Cholesky.
inv_lhs: Matrix3<Real>, // FIXME: replace by Cholesky.
#[cfg(feature = "dim2")]
rhs: Vector3<f32>,
rhs: Vector3<Real>,
im1: f32,
im2: f32,
im1: Real,
im2: Real,
ii1: AngularInertia<f32>,
ii2: AngularInertia<f32>,
ii1: AngularInertia<Real>,
ii2: AngularInertia<Real>,
ii1_sqrt: AngularInertia<f32>,
ii2_sqrt: AngularInertia<f32>,
ii1_sqrt: AngularInertia<Real>,
ii2_sqrt: AngularInertia<Real>,
r1: Vector<f32>,
r2: Vector<f32>,
r1: Vector<Real>,
r2: Vector<Real>,
}
impl FixedVelocityConstraint {
@@ -128,7 +128,7 @@ impl FixedVelocityConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>,
impulse: SpacialVector<Real>,
#[cfg(feature = "dim3")]
inv_lhs: Matrix6<f32>, // FIXME: replace by Cholesky.
inv_lhs: Matrix6<Real>, // FIXME: replace by Cholesky.
#[cfg(feature = "dim3")]
rhs: Vector6<f32>,
rhs: Vector6<Real>,
#[cfg(feature = "dim2")]
inv_lhs: Matrix3<f32>, // FIXME: replace by Cholesky.
inv_lhs: Matrix3<Real>, // FIXME: replace by Cholesky.
#[cfg(feature = "dim2")]
rhs: Vector3<f32>,
rhs: Vector3<Real>,
im2: f32,
ii2: AngularInertia<f32>,
ii2_sqrt: AngularInertia<f32>,
r2: Vector<f32>,
im2: Real,
ii2: AngularInertia<Real>,
ii2_sqrt: AngularInertia<Real>,
r2: Vector<Real>,
}
impl FixedVelocityGroundConstraint {
@@ -312,7 +312,7 @@ impl FixedVelocityGroundConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
let lin_impulse = self.impulse.fixed_rows::<Dim>(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<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

12
src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
View File

@@ -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<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1: DeltaVel<SimdReal> = 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<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],

5
src/dynamics/solver/joint_constraint/joint_constraint.rs
View File

@@ -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<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
match self {
AnyJointVelocityConstraint::BallConstraint(c) => c.solve(mj_lambdas),
AnyJointVelocityConstraint::BallGroundConstraint(c) => c.solve(mj_lambdas),

4
src/dynamics/solver/joint_constraint/joint_position_constraint.rs
View File

@@ -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<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
match self {
AnyJointPositionConstraint::BallJoint(c) => c.solve(params, positions),
AnyJointPositionConstraint::BallGroundConstraint(c) => c.solve(params, positions),

38
src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs
View File

@@ -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<f32>,
ii2: AngularInertia<f32>,
ii1: AngularInertia<Real>,
ii2: AngularInertia<Real>,
lin_inv_lhs: f32,
ang_inv_lhs: AngularInertia<f32>,
lin_inv_lhs: Real,
ang_inv_lhs: AngularInertia<Real>,
limits: [f32; 2],
limits: [Real; 2],
local_frame1: Isometry<f32>,
local_frame2: Isometry<f32>,
local_frame1: Isometry<Real>,
local_frame2: Isometry<Real>,
local_axis1: Unit<Vector<f32>>,
local_axis2: Unit<Vector<f32>>,
local_axis1: Unit<Vector<Real>>,
local_axis2: Unit<Vector<Real>>,
}
impl PrismaticPositionConstraint {
@@ -52,7 +52,7 @@ impl PrismaticPositionConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
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<f32>,
local_frame2: Isometry<f32>,
axis1: Unit<Vector<f32>>,
local_axis2: Unit<Vector<f32>>,
limits: [f32; 2],
frame1: Isometry<Real>,
local_frame2: Isometry<Real>,
axis1: Unit<Vector<Real>>,
local_axis2: Unit<Vector<Real>>,
limits: [Real; 2],
}
impl PrismaticPositionGroundConstraint {
@@ -140,7 +140,7 @@ impl PrismaticPositionGroundConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position2 = positions[self.position2 as usize];
// Angular correction.

74
src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
View File

@@ -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<f32>,
r2: Vector<f32>,
r1: Vector<Real>,
r2: Vector<Real>,
#[cfg(feature = "dim3")]
inv_lhs: Matrix5<f32>,
inv_lhs: Matrix5<Real>,
#[cfg(feature = "dim3")]
rhs: Vector5<f32>,
rhs: Vector5<Real>,
#[cfg(feature = "dim3")]
impulse: Vector5<f32>,
impulse: Vector5<Real>,
#[cfg(feature = "dim2")]
inv_lhs: Matrix2<f32>,
inv_lhs: Matrix2<Real>,
#[cfg(feature = "dim2")]
rhs: Vector2<f32>,
rhs: Vector2<Real>,
#[cfg(feature = "dim2")]
impulse: Vector2<f32>,
impulse: Vector2<Real>,
limits_impulse: f32,
limits_forcedirs: Option<(Vector<f32>, Vector<f32>)>,
limits_rhs: f32,
limits_impulse: Real,
limits_forcedirs: Option<(Vector<Real>, Vector<Real>)>,
limits_rhs: Real,
#[cfg(feature = "dim2")]
basis1: Vector2<f32>,
basis1: Vector2<Real>,
#[cfg(feature = "dim3")]
basis1: Matrix3x2<f32>,
basis1: Matrix3x2<Real>,
im1: f32,
im2: f32,
im1: Real,
im2: Real,
ii1_sqrt: AngularInertia<f32>,
ii2_sqrt: AngularInertia<f32>,
ii1_sqrt: AngularInertia<Real>,
ii2_sqrt: AngularInertia<Real>,
}
impl PrismaticVelocityConstraint {
@@ -191,7 +191,7 @@ impl PrismaticVelocityConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>,
r2: Vector<Real>,
#[cfg(feature = "dim2")]
inv_lhs: Matrix2<f32>,
inv_lhs: Matrix2<Real>,
#[cfg(feature = "dim2")]
rhs: Vector2<f32>,
rhs: Vector2<Real>,
#[cfg(feature = "dim2")]
impulse: Vector2<f32>,
impulse: Vector2<Real>,
#[cfg(feature = "dim3")]
inv_lhs: Matrix5<f32>,
inv_lhs: Matrix5<Real>,
#[cfg(feature = "dim3")]
rhs: Vector5<f32>,
rhs: Vector5<Real>,
#[cfg(feature = "dim3")]
impulse: Vector5<f32>,
impulse: Vector5<Real>,
limits_impulse: f32,
limits_rhs: f32,
limits_impulse: Real,
limits_rhs: Real,
axis2: Vector<f32>,
axis2: Vector<Real>,
#[cfg(feature = "dim2")]
basis1: Vector2<f32>,
basis1: Vector2<Real>,
#[cfg(feature = "dim3")]
basis1: Matrix3x2<f32>,
limits_forcedir2: Option<Vector<f32>>,
basis1: Matrix3x2<Real>,
limits_forcedir2: Option<Vector<Real>>,
im2: f32,
ii2_sqrt: AngularInertia<f32>,
im2: Real,
ii2_sqrt: AngularInertia<Real>,
}
impl PrismaticVelocityGroundConstraint {
@@ -478,7 +478,7 @@ impl PrismaticVelocityGroundConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LinImpulseDim>(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<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
/*

10
src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs
View File

@@ -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<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],

34
src/dynamics/solver/joint_constraint/revolute_position_constraint.rs
View File

@@ -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<f32>,
ii2: AngularInertia<f32>,
ii1: AngularInertia<Real>,
ii2: AngularInertia<Real>,
lin_inv_lhs: f32,
ang_inv_lhs: AngularInertia<f32>,
lin_inv_lhs: Real,
ang_inv_lhs: AngularInertia<Real>,
local_anchor1: Point<f32>,
local_anchor2: Point<f32>,
local_anchor1: Point<Real>,
local_anchor2: Point<Real>,
local_axis1: Unit<Vector<f32>>,
local_axis2: Unit<Vector<f32>>,
local_axis1: Unit<Vector<Real>>,
local_axis2: Unit<Vector<Real>>,
}
impl RevolutePositionConstraint {
@@ -49,7 +49,7 @@ impl RevolutePositionConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
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<f32>,
local_anchor2: Point<f32>,
axis1: Unit<Vector<f32>>,
local_axis2: Unit<Vector<f32>>,
anchor1: Point<Real>,
local_anchor2: Point<Real>,
axis1: Unit<Vector<Real>>,
local_axis2: Unit<Vector<Real>>,
}
impl RevolutePositionGroundConstraint {
@@ -122,7 +122,7 @@ impl RevolutePositionGroundConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position2 = positions[self.position2 as usize];
let axis2 = position2 * self.local_axis2;

44
src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
View File

@@ -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<f32>,
r2: Vector<f32>,
r1: Vector<Real>,
r2: Vector<Real>,
inv_lhs: Matrix5<f32>,
rhs: Vector5<f32>,
impulse: Vector5<f32>,
inv_lhs: Matrix5<Real>,
rhs: Vector5<Real>,
impulse: Vector5<Real>,
basis1: Matrix3x2<f32>,
basis1: Matrix3x2<Real>,
im1: f32,
im2: f32,
im1: Real,
im2: Real,
ii1_sqrt: AngularInertia<f32>,
ii2_sqrt: AngularInertia<f32>,
ii1_sqrt: AngularInertia<Real>,
ii2_sqrt: AngularInertia<Real>,
}
impl RevoluteVelocityConstraint {
@@ -99,7 +99,7 @@ impl RevoluteVelocityConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>,
r2: Vector<Real>,
inv_lhs: Matrix5<f32>,
rhs: Vector5<f32>,
impulse: Vector5<f32>,
inv_lhs: Matrix5<Real>,
rhs: Vector5<Real>,
impulse: Vector5<Real>,
basis1: Matrix3x2<f32>,
basis1: Matrix3x2<Real>,
im2: f32,
im2: Real,
ii2_sqrt: AngularInertia<f32>,
ii2_sqrt: AngularInertia<Real>,
}
impl RevoluteVelocityGroundConstraint {
@@ -249,7 +249,7 @@ impl RevoluteVelocityGroundConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
let lin_impulse = self.impulse.fixed_rows::<U3>(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<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

10
src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs
View File

@@ -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<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],

10
src/dynamics/solver/parallel_island_solver.rs
View File

@@ -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<DeltaVel<f32>>,
positions: Vec<Isometry<f32>>,
mj_lambdas: Vec<DeltaVel<Real>>,
positions: Vec<Isometry<Real>>,
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<DeltaVel<f32>> =
let mj_lambdas: &mut Vec<DeltaVel<Real>> =
unsafe { std::mem::transmute(mj_lambdas.load(Ordering::Relaxed)) };
let positions: &mut Vec<Isometry<f32>> =
let positions: &mut Vec<Isometry<Real>> =
unsafe { std::mem::transmute(positions.load(Ordering::Relaxed)) };
let bodies: &mut RigidBodySet =
unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };

4
src/dynamics/solver/parallel_position_solver.rs
View File

@@ -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<f32>],
positions: &mut [Isometry<Real>],
) {
if self.part.constraint_descs.len() == 0 {
return;

2
src/dynamics/solver/parallel_velocity_solver.rs
View File

@@ -317,7 +317,7 @@ impl ParallelVelocitySolver {
params: &IntegrationParameters,
manifolds_all: &mut [&mut ContactManifold],
joints_all: &mut [JointGraphEdge],
mj_lambdas: &mut [DeltaVel<f32>],
mj_lambdas: &mut [DeltaVel<Real>],
) {
if self.part.constraint_descs.len() == 0 && self.joint_part.constraint_descs.len() == 0 {
return;

26
src/dynamics/solver/position_constraint.rs
View File

@@ -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<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
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<f32>; MAX_MANIFOLD_POINTS],
pub local_p2: [Point<f32>; MAX_MANIFOLD_POINTS],
pub dists: [f32; MAX_MANIFOLD_POINTS],
pub local_n1: Vector<f32>,
pub local_p1: [Point<Real>; MAX_MANIFOLD_POINTS],
pub local_p2: [Point<Real>; MAX_MANIFOLD_POINTS],
pub dists: [Real; MAX_MANIFOLD_POINTS],
pub local_n1: Vector<Real>,
pub num_contacts: u8,
pub im1: f32,
pub im2: f32,
pub ii1: AngularInertia<f32>,
pub ii2: AngularInertia<f32>,
pub erp: f32,
pub max_linear_correction: f32,
pub im1: Real,
pub im2: Real,
pub ii1: AngularInertia<Real>,
pub ii2: AngularInertia<Real>,
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<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
// FIXME: can we avoid most of the multiplications by pos1/pos2?
// Compute jacobians.
let mut pos1 = positions[self.rb1];

6
src/dynamics/solver/position_constraint_wide.rs
View File

@@ -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<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
// 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]);

20
src/dynamics/solver/position_ground_constraint.rs
View File

@@ -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<f32>; MAX_MANIFOLD_POINTS],
pub local_p2: [Point<f32>; MAX_MANIFOLD_POINTS],
pub dists: [f32; MAX_MANIFOLD_POINTS],
pub n1: Vector<f32>,
pub p1: [Point<Real>; MAX_MANIFOLD_POINTS],
pub local_p2: [Point<Real>; MAX_MANIFOLD_POINTS],
pub dists: [Real; MAX_MANIFOLD_POINTS],
pub n1: Vector<Real>,
pub num_contacts: u8,
pub im2: f32,
pub ii2: AngularInertia<f32>,
pub erp: f32,
pub max_linear_correction: f32,
pub im2: Real,
pub ii2: AngularInertia<Real>,
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<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
// FIXME: can we avoid most of the multiplications by pos1/pos2?
// Compute jacobians.
let mut pos2 = positions[self.rb2];

6
src/dynamics/solver/position_ground_constraint_wide.rs
View File

@@ -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<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
// 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]);

4
src/dynamics/solver/position_solver.rs
View File

@@ -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<Isometry<f32>>,
positions: Vec<Isometry<Real>>,
}
impl PositionSolver {

7
src/dynamics/solver/solver_constraints.rs
View File

@@ -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,

28
src/dynamics/solver/velocity_constraint.rs
View File

@@ -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<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>,
pub gcross2: AngVector<f32>,
pub rhs: f32,
pub impulse: f32,
pub r: f32,
pub gcross1: AngVector<Real>,
pub gcross2: AngVector<Real>,
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<f32>, // Non-penetration force direction for the first body.
pub im1: f32,
pub im2: f32,
pub limit: f32,
pub dir1: Vector<Real>, // 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<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];

6
src/dynamics/solver/velocity_constraint_wide.rs
View File

@@ -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<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],

20
src/dynamics/solver/velocity_ground_constraint.rs
View File

@@ -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<f32>,
pub rhs: f32,
pub impulse: f32,
pub r: f32,
pub gcross2: AngVector<Real>,
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<f32>, // Non-penetration force direction for the first body.
pub im2: f32,
pub limit: f32,
pub dir1: Vector<Real>, // 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<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
// Solve friction.

6
src/dynamics/solver/velocity_ground_constraint_wide.rs
View File

@@ -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<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![ |ii| mj_lambdas[ self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],

3
src/dynamics/solver/velocity_solver.rs
View File

@@ -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<DeltaVel<f32>>,
pub mj_lambdas: Vec<DeltaVel<Real>>,
}
impl VelocitySolver {

24
src/geometry/broad_phase_multi_sap.rs
View File

@@ -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<f32>) -> Point<i32> {
fn point_key(point: Point<Real>) -> Point<i32> {
(point / CELL_WIDTH).coords.map(|e| e.floor() as i32).into()
}
fn region_aabb(index: Point<i32>) -> 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<i32>) -> 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<Endpoint>,
#[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,
) {

14
src/geometry/contact_pair.rs
View File

@@ -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

4
src/geometry/narrow_phase.rs
View File

@@ -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>,

9
src/lib.rs
View File

@@ -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")]

3
src/pipeline/collision_pipeline.rs
View File

@@ -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,

6
src/pipeline/physics_pipeline.rs
View File

@@ -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<f32>,
gravity: &Vector<Real>,
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]

93
src/utils.rs
View File

@@ -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<Rhs>: Sized {
fn copy_sign_to(self, to: Rhs) -> Rhs;
}
impl WSign<f32> for f32 {
impl WSign<Real> 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<f32> {
type CrossMat = Matrix3<f32>;
impl WCrossMatrix for Vector3<Real> {
type CrossMat = Matrix3<Real>;
#[inline]
#[rustfmt::skip]
@@ -235,8 +240,8 @@ impl WCrossMatrix for Vector3<f32> {
}
}
impl WCrossMatrix for Vector2<f32> {
type CrossMat = Vector2<f32>;
impl WCrossMatrix for Vector2<Real> {
type CrossMat = Vector2<Real>;
#[inline]
fn gcross_matrix(self) -> Self::CrossMat {
@@ -249,26 +254,26 @@ pub(crate) trait WCross<Rhs>: Sized {
fn gcross(&self, rhs: Rhs) -> Self::Result;
}
impl WCross<Vector3<f32>> for Vector3<f32> {
impl WCross<Vector3<Real>> for Vector3<Real> {
type Result = Self;
fn gcross(&self, rhs: Vector3<f32>) -> Self::Result {
fn gcross(&self, rhs: Vector3<Real>) -> Self::Result {
self.cross(&rhs)
}
}
impl WCross<Vector2<f32>> for Vector2<f32> {
type Result = f32;
impl WCross<Vector2<Real>> for Vector2<Real> {
type Result = Real;
fn gcross(&self, rhs: Vector2<f32>) -> Self::Result {
fn gcross(&self, rhs: Vector2<Real>) -> Self::Result {
self.x * rhs.y - self.y * rhs.x
}
}
impl WCross<Vector2<f32>> for f32 {
type Result = Vector2<f32>;
impl WCross<Vector2<Real>> for Real {
type Result = Vector2<Real>;
fn gcross(&self, rhs: Vector2<f32>) -> Self::Result {
fn gcross(&self, rhs: Vector2<Real>) -> Self::Result {
Vector2::new(-rhs.y * *self, rhs.x * *self)
}
}
@@ -278,26 +283,26 @@ pub(crate) trait WDot<Rhs>: Sized {
fn gdot(&self, rhs: Rhs) -> Self::Result;
}
impl WDot<Vector3<f32>> for Vector3<f32> {
type Result = f32;
impl WDot<Vector3<Real>> for Vector3<Real> {
type Result = Real;
fn gdot(&self, rhs: Vector3<f32>) -> Self::Result {
fn gdot(&self, rhs: Vector3<Real>) -> Self::Result {
self.x * rhs.x + self.y * rhs.y + self.z * rhs.z
}
}
impl WDot<Vector2<f32>> for Vector2<f32> {
type Result = f32;
impl WDot<Vector2<Real>> for Vector2<Real> {
type Result = Real;
fn gdot(&self, rhs: Vector2<f32>) -> Self::Result {
fn gdot(&self, rhs: Vector2<Real>) -> Self::Result {
self.x * rhs.x + self.y * rhs.y
}
}
impl WDot<f32> for f32 {
type Result = f32;
impl WDot<Real> 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<N> {
fn into_matrix(self) -> Self::AngMatrix;
}
impl WAngularInertia<f32> for f32 {
type AngVector = f32;
type LinVector = Vector2<f32>;
type AngMatrix = f32;
impl WAngularInertia<Real> for Real {
type AngVector = Real;
type LinVector = Vector2<Real>;
type AngMatrix = Real;
fn inverse(&self) -> Self {
if *self != 0.0 {
@@ -400,14 +405,14 @@ impl WAngularInertia<f32> for f32 {
}
}
fn transform_lin_vector(&self, pt: Vector2<f32>) -> Vector2<f32> {
fn transform_lin_vector(&self, pt: Vector2<Real>) -> Vector2<Real> {
*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<SimdReal> for SimdReal {
}
}
impl WAngularInertia<f32> for SdpMatrix3<f32> {
type AngVector = Vector3<f32>;
type LinVector = Vector3<f32>;
type AngMatrix = Matrix3<f32>;
impl WAngularInertia<Real> for SdpMatrix3<Real> {
type AngVector = Vector3<Real>;
type LinVector = Vector3<Real>;
type AngMatrix = Matrix3<Real>;
fn inverse(&self) -> Self {
let minor_m12_m23 = self.m22 * self.m33 - self.m23 * self.m23;
@@ -490,11 +495,11 @@ impl WAngularInertia<f32> for SdpMatrix3<f32> {
}
}
fn transform_lin_vector(&self, v: Vector3<f32>) -> Vector3<f32> {
fn transform_lin_vector(&self, v: Vector3<Real>) -> Vector3<Real> {
self.transform_vector(v)
}
fn transform_vector(&self, v: Vector3<f32>) -> Vector3<f32> {
fn transform_vector(&self, v: Vector3<Real>) -> Vector3<Real> {
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<f32> for SdpMatrix3<f32> {
}
#[rustfmt::skip]
fn into_matrix(self) -> Matrix3<f32> {
fn into_matrix(self) -> Matrix3<Real> {
Matrix3::new(
self.m11, self.m12, self.m13,
self.m12, self.m22, self.m23,
@@ -511,7 +516,7 @@ impl WAngularInertia<f32> for SdpMatrix3<f32> {
}
#[rustfmt::skip]
fn transform_matrix(&self, m: &Matrix3<f32>) -> Matrix3<f32> {
fn transform_matrix(&self, m: &Matrix3<Real>) -> Matrix3<Real> {
*self * *m
}
}