From aa61fe65e3ff0289ecab57b4053a3410cf6d4a87 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 4 Jan 2021 15:14:25 +0100 Subject: Add support of 64-bits reals. --- src/dynamics/solver/delta_vel.rs | 4 +- src/dynamics/solver/interaction_groups.rs | 2 +- .../joint_constraint/ball_position_constraint.rs | 32 +++++----- .../ball_position_constraint_wide.rs | 6 +- .../joint_constraint/ball_velocity_constraint.rs | 34 +++++----- .../ball_velocity_constraint_wide.rs | 10 +-- .../joint_constraint/fixed_position_constraint.rs | 40 ++++++------ .../joint_constraint/fixed_velocity_constraint.rs | 54 ++++++++-------- .../fixed_velocity_constraint_wide.rs | 12 ++-- .../solver/joint_constraint/joint_constraint.rs | 5 +- .../joint_constraint/joint_position_constraint.rs | 4 +- .../prismatic_position_constraint.rs | 38 +++++------ .../prismatic_velocity_constraint.rs | 74 +++++++++++----------- .../prismatic_velocity_constraint_wide.rs | 10 +-- .../revolute_position_constraint.rs | 34 +++++----- .../revolute_velocity_constraint.rs | 44 ++++++------- .../revolute_velocity_constraint_wide.rs | 10 +-- src/dynamics/solver/parallel_island_solver.rs | 10 +-- src/dynamics/solver/parallel_position_solver.rs | 4 +- src/dynamics/solver/parallel_velocity_solver.rs | 2 +- src/dynamics/solver/position_constraint.rs | 26 ++++---- src/dynamics/solver/position_constraint_wide.rs | 6 +- src/dynamics/solver/position_ground_constraint.rs | 20 +++--- .../solver/position_ground_constraint_wide.rs | 6 +- src/dynamics/solver/position_solver.rs | 4 +- src/dynamics/solver/solver_constraints.rs | 7 +- src/dynamics/solver/velocity_constraint.rs | 28 ++++---- src/dynamics/solver/velocity_constraint_wide.rs | 6 +- src/dynamics/solver/velocity_ground_constraint.rs | 20 +++--- .../solver/velocity_ground_constraint_wide.rs | 6 +- src/dynamics/solver/velocity_solver.rs | 3 +- 31 files changed, 282 insertions(+), 279 deletions(-) (limited to 'src/dynamics/solver') diff --git a/src/dynamics/solver/delta_vel.rs b/src/dynamics/solver/delta_vel.rs index c4a424b..4614ed7 100644 --- a/src/dynamics/solver/delta_vel.rs +++ b/src/dynamics/solver/delta_vel.rs @@ -1,9 +1,9 @@ -use crate::math::{AngVector, Vector}; +use crate::math::{AngVector, Real, Vector}; use na::{Scalar, SimdRealField}; #[derive(Copy, Clone, Debug)] //#[repr(align(64))] -pub(crate) struct DeltaVel { +pub(crate) struct DeltaVel { pub linear: Vector, pub angular: AngVector, } diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index b5f8173..b409f98 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -2,7 +2,7 @@ use crate::dynamics::{BodyPair, JointGraphEdge, JointIndex, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; #[cfg(feature = "simd-is-enabled")] use { - crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH}, + crate::math::{Real, SIMD_LAST_INDEX, SIMD_WIDTH}, vec_map::VecMap, }; diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs index 21a537e..8bc9072 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs @@ -1,7 +1,7 @@ use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; #[cfg(feature = "dim2")] use crate::math::SdpMatrix; -use crate::math::{AngularInertia, Isometry, Point, Rotation}; +use crate::math::{AngularInertia, Isometry, Point, Real, Rotation}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[derive(Debug)] @@ -9,17 +9,17 @@ pub(crate) struct BallPositionConstraint { position1: usize, position2: usize, - local_com1: Point, - local_com2: Point, + local_com1: Point, + local_com2: Point, - im1: f32, - im2: f32, + im1: Real, + im2: Real, - ii1: AngularInertia, - ii2: AngularInertia, + ii1: AngularInertia, + ii2: AngularInertia, - local_anchor1: Point, - local_anchor2: Point, + local_anchor1: Point, + local_anchor2: Point, } impl BallPositionConstraint { @@ -38,7 +38,7 @@ impl BallPositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position1 = positions[self.position1 as usize]; let mut position2 = positions[self.position2 as usize]; @@ -95,11 +95,11 @@ impl BallPositionConstraint { #[derive(Debug)] pub(crate) struct BallPositionGroundConstraint { position2: usize, - anchor1: Point, - im2: f32, - ii2: AngularInertia, - local_anchor2: Point, - local_com2: Point, + anchor1: Point, + im2: Real, + ii2: AngularInertia, + local_anchor2: Point, + local_com2: Point, } impl BallPositionGroundConstraint { @@ -133,7 +133,7 @@ impl BallPositionGroundConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position2 = positions[self.position2 as usize]; let anchor2 = position2 * self.local_anchor2; diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs index f1ca4b6..2f2ffc3 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs @@ -1,7 +1,7 @@ use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; #[cfg(feature = "dim2")] use crate::math::SdpMatrix; -use crate::math::{AngularInertia, Isometry, Point, Rotation, SimdReal, SIMD_WIDTH}; +use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, SimdReal, SIMD_WIDTH}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use simba::simd::SimdValue; @@ -60,7 +60,7 @@ impl WBallPositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position1 = Isometry::from(array![|ii| positions[self.position1[ii]]; SIMD_WIDTH]); let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]); @@ -164,7 +164,7 @@ impl WBallPositionGroundConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]); let anchor2 = position2 * self.local_anchor2; diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs index 97ba244..9f47624 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs @@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, }; -use crate::math::{SdpMatrix, Vector}; +use crate::math::{Real, SdpMatrix, Vector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[derive(Debug)] @@ -12,16 +12,16 @@ pub(crate) struct BallVelocityConstraint { joint_id: JointIndex, - rhs: Vector, - pub(crate) impulse: Vector, + rhs: Vector, + pub(crate) impulse: Vector, - gcross1: Vector, - gcross2: Vector, + gcross1: Vector, + gcross2: Vector, - inv_lhs: SdpMatrix, + inv_lhs: SdpMatrix, - im1: f32, - im2: f32, + im1: Real, + im2: Real, } impl BallVelocityConstraint { @@ -91,7 +91,7 @@ impl BallVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; @@ -104,7 +104,7 @@ impl BallVelocityConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; @@ -137,11 +137,11 @@ impl BallVelocityConstraint { pub(crate) struct BallVelocityGroundConstraint { mj_lambda2: usize, joint_id: JointIndex, - rhs: Vector, - impulse: Vector, - gcross2: Vector, - inv_lhs: SdpMatrix, - im2: f32, + rhs: Vector, + impulse: Vector, + gcross2: Vector, + inv_lhs: SdpMatrix, + im2: Real, } impl BallVelocityGroundConstraint { @@ -206,14 +206,14 @@ impl BallVelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; mj_lambda2.linear -= self.im2 * self.impulse; mj_lambda2.angular -= self.gcross2.gcross(self.impulse); mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2); diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs index bbb709e..2806233 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs @@ -3,7 +3,7 @@ use crate::dynamics::{ BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, }; use crate::math::{ - AngVector, AngularInertia, Isometry, Point, SdpMatrix, SimdReal, Vector, SIMD_WIDTH, + AngVector, AngularInertia, Isometry, Point, Real, SdpMatrix, SimdReal, Vector, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use simba::simd::SimdValue; @@ -107,7 +107,7 @@ impl WBallVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -140,7 +140,7 @@ impl WBallVelocityConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1: DeltaVel = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -274,7 +274,7 @@ impl WBallVelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], @@ -293,7 +293,7 @@ impl WBallVelocityGroundConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2: DeltaVel = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs index 24001cd..cf9dcb7 100644 --- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs @@ -1,22 +1,22 @@ use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody}; -use crate::math::{AngularInertia, Isometry, Point, Rotation}; +use crate::math::{AngularInertia, Isometry, Point, Real, Rotation}; use crate::utils::WAngularInertia; #[derive(Debug)] pub(crate) struct FixedPositionConstraint { position1: usize, position2: usize, - local_anchor1: Isometry, - local_anchor2: Isometry, - local_com1: Point, - local_com2: Point, - im1: f32, - im2: f32, - ii1: AngularInertia, - ii2: AngularInertia, - - lin_inv_lhs: f32, - ang_inv_lhs: AngularInertia, + local_anchor1: Isometry, + local_anchor2: Isometry, + local_com1: Point, + local_com2: Point, + im1: Real, + im2: Real, + ii1: AngularInertia, + ii2: AngularInertia, + + lin_inv_lhs: Real, + ang_inv_lhs: AngularInertia, } impl FixedPositionConstraint { @@ -44,7 +44,7 @@ impl FixedPositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position1 = positions[self.position1 as usize]; let mut position2 = positions[self.position2 as usize]; @@ -81,12 +81,12 @@ impl FixedPositionConstraint { #[derive(Debug)] pub(crate) struct FixedPositionGroundConstraint { position2: usize, - anchor1: Isometry, - local_anchor2: Isometry, - local_com2: Point, - im2: f32, - ii2: AngularInertia, - impulse: f32, + anchor1: Isometry, + local_anchor2: Isometry, + local_com2: Point, + im2: Real, + ii2: AngularInertia, + impulse: Real, } impl FixedPositionGroundConstraint { @@ -118,7 +118,7 @@ impl FixedPositionGroundConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position2 = positions[self.position2 as usize]; // Angular correction. diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs index e4187c8..715b7dd 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs @@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, }; -use crate::math::{AngularInertia, Dim, SpacialVector, Vector}; +use crate::math::{AngularInertia, Dim, Real, SpacialVector, Vector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[cfg(feature = "dim2")] use na::{Matrix3, Vector3}; @@ -16,29 +16,29 @@ pub(crate) struct FixedVelocityConstraint { joint_id: JointIndex, - impulse: SpacialVector, + impulse: SpacialVector, #[cfg(feature = "dim3")] - inv_lhs: Matrix6, // FIXME: replace by Cholesky. + inv_lhs: Matrix6, // FIXME: replace by Cholesky. #[cfg(feature = "dim3")] - rhs: Vector6, + rhs: Vector6, #[cfg(feature = "dim2")] - inv_lhs: Matrix3, // FIXME: replace by Cholesky. + inv_lhs: Matrix3, // FIXME: replace by Cholesky. #[cfg(feature = "dim2")] - rhs: Vector3, + rhs: Vector3, - im1: f32, - im2: f32, + im1: Real, + im2: Real, - ii1: AngularInertia, - ii2: AngularInertia, + ii1: AngularInertia, + ii2: AngularInertia, - ii1_sqrt: AngularInertia, - ii2_sqrt: AngularInertia, + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, - r1: Vector, - r2: Vector, + r1: Vector, + r2: Vector, } impl FixedVelocityConstraint { @@ -128,7 +128,7 @@ impl FixedVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; @@ -152,7 +152,7 @@ impl FixedVelocityConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; @@ -207,22 +207,22 @@ pub(crate) struct FixedVelocityGroundConstraint { joint_id: JointIndex, - impulse: SpacialVector, + impulse: SpacialVector, #[cfg(feature = "dim3")] - inv_lhs: Matrix6, // FIXME: replace by Cholesky. + inv_lhs: Matrix6, // FIXME: replace by Cholesky. #[cfg(feature = "dim3")] - rhs: Vector6, + rhs: Vector6, #[cfg(feature = "dim2")] - inv_lhs: Matrix3, // FIXME: replace by Cholesky. + inv_lhs: Matrix3, // FIXME: replace by Cholesky. #[cfg(feature = "dim2")] - rhs: Vector3, + rhs: Vector3, - im2: f32, - ii2: AngularInertia, - ii2_sqrt: AngularInertia, - r2: Vector, + im2: Real, + ii2: AngularInertia, + ii2_sqrt: AngularInertia, + r2: Vector, } impl FixedVelocityGroundConstraint { @@ -312,7 +312,7 @@ impl FixedVelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); @@ -329,7 +329,7 @@ impl FixedVelocityGroundConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs index 6584ea2..48a6cba 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs @@ -5,8 +5,8 @@ use crate::dynamics::{ FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, }; use crate::math::{ - AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, SimdReal, SpacialVector, Vector, - SIMD_WIDTH, + AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, Real, SimdReal, SpacialVector, + Vector, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[cfg(feature = "dim3")] @@ -158,7 +158,7 @@ impl WFixedVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -202,7 +202,7 @@ impl WFixedVelocityConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1: DeltaVel = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -393,7 +393,7 @@ impl WFixedVelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], @@ -420,7 +420,7 @@ impl WFixedVelocityGroundConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2: DeltaVel = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs index 8b1f8bf..ed6d758 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs @@ -17,6 +17,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet, }; +use crate::math::Real; #[cfg(feature = "simd-is-enabled")] use crate::math::SIMD_WIDTH; @@ -220,7 +221,7 @@ impl AnyJointVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { match self { AnyJointVelocityConstraint::BallConstraint(c) => c.warmstart(mj_lambdas), AnyJointVelocityConstraint::BallGroundConstraint(c) => c.warmstart(mj_lambdas), @@ -254,7 +255,7 @@ impl AnyJointVelocityConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { match self { AnyJointVelocityConstraint::BallConstraint(c) => c.solve(mj_lambdas), AnyJointVelocityConstraint::BallGroundConstraint(c) => c.solve(mj_lambdas), diff --git a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs index 4773687..0ebe88e 100644 --- a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs @@ -7,9 +7,9 @@ use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint}; #[cfg(feature = "simd-is-enabled")] use super::{WBallPositionConstraint, WBallPositionGroundConstraint}; use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet}; -use crate::math::Isometry; #[cfg(feature = "simd-is-enabled")] use crate::math::SIMD_WIDTH; +use crate::math::{Isometry, Real}; pub(crate) enum AnyJointPositionConstraint { BallJoint(BallPositionConstraint), @@ -147,7 +147,7 @@ impl AnyJointPositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { match self { AnyJointPositionConstraint::BallJoint(c) => c.solve(params, positions), AnyJointPositionConstraint::BallGroundConstraint(c) => c.solve(params, positions), diff --git a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs index c3e4b98..2616f6b 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs @@ -1,5 +1,5 @@ use crate::dynamics::{IntegrationParameters, PrismaticJoint, RigidBody}; -use crate::math::{AngularInertia, Isometry, Point, Rotation, Vector}; +use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector}; use crate::utils::WAngularInertia; use na::Unit; @@ -8,22 +8,22 @@ pub(crate) struct PrismaticPositionConstraint { position1: usize, position2: usize, - im1: f32, - im2: f32, + im1: Real, + im2: Real, - ii1: AngularInertia, - ii2: AngularInertia, + ii1: AngularInertia, + ii2: AngularInertia, - lin_inv_lhs: f32, - ang_inv_lhs: AngularInertia, + lin_inv_lhs: Real, + ang_inv_lhs: AngularInertia, - limits: [f32; 2], + limits: [Real; 2], - local_frame1: Isometry, - local_frame2: Isometry, + local_frame1: Isometry, + local_frame2: Isometry, - local_axis1: Unit>, - local_axis2: Unit>, + local_axis1: Unit>, + local_axis2: Unit>, } impl PrismaticPositionConstraint { @@ -52,7 +52,7 @@ impl PrismaticPositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position1 = positions[self.position1 as usize]; let mut position2 = positions[self.position2 as usize]; @@ -99,11 +99,11 @@ impl PrismaticPositionConstraint { #[derive(Debug)] pub(crate) struct PrismaticPositionGroundConstraint { position2: usize, - frame1: Isometry, - local_frame2: Isometry, - axis1: Unit>, - local_axis2: Unit>, - limits: [f32; 2], + frame1: Isometry, + local_frame2: Isometry, + axis1: Unit>, + local_axis2: Unit>, + limits: [Real; 2], } impl PrismaticPositionGroundConstraint { @@ -140,7 +140,7 @@ impl PrismaticPositionGroundConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position2 = positions[self.position2 as usize]; // Angular correction. diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index 9e39bd6..78dc1b5 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody, }; -use crate::math::{AngularInertia, Vector}; +use crate::math::{AngularInertia, Real, Vector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[cfg(feature = "dim3")] use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; @@ -24,37 +24,37 @@ pub(crate) struct PrismaticVelocityConstraint { joint_id: JointIndex, - r1: Vector, - r2: Vector, + r1: Vector, + r2: Vector, #[cfg(feature = "dim3")] - inv_lhs: Matrix5, + inv_lhs: Matrix5, #[cfg(feature = "dim3")] - rhs: Vector5, + rhs: Vector5, #[cfg(feature = "dim3")] - impulse: Vector5, + impulse: Vector5, #[cfg(feature = "dim2")] - inv_lhs: Matrix2, + inv_lhs: Matrix2, #[cfg(feature = "dim2")] - rhs: Vector2, + rhs: Vector2, #[cfg(feature = "dim2")] - impulse: Vector2, + impulse: Vector2, - limits_impulse: f32, - limits_forcedirs: Option<(Vector, Vector)>, - limits_rhs: f32, + limits_impulse: Real, + limits_forcedirs: Option<(Vector, Vector)>, + limits_rhs: Real, #[cfg(feature = "dim2")] - basis1: Vector2, + basis1: Vector2, #[cfg(feature = "dim3")] - basis1: Matrix3x2, + basis1: Matrix3x2, - im1: f32, - im2: f32, + im1: Real, + im2: Real, - ii1_sqrt: AngularInertia, - ii2_sqrt: AngularInertia, + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, } impl PrismaticVelocityConstraint { @@ -191,7 +191,7 @@ impl PrismaticVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; @@ -220,7 +220,7 @@ impl PrismaticVelocityConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; @@ -295,34 +295,34 @@ pub(crate) struct PrismaticVelocityGroundConstraint { joint_id: JointIndex, - r2: Vector, + r2: Vector, #[cfg(feature = "dim2")] - inv_lhs: Matrix2, + inv_lhs: Matrix2, #[cfg(feature = "dim2")] - rhs: Vector2, + rhs: Vector2, #[cfg(feature = "dim2")] - impulse: Vector2, + impulse: Vector2, #[cfg(feature = "dim3")] - inv_lhs: Matrix5, + inv_lhs: Matrix5, #[cfg(feature = "dim3")] - rhs: Vector5, + rhs: Vector5, #[cfg(feature = "dim3")] - impulse: Vector5, + impulse: Vector5, - limits_impulse: f32, - limits_rhs: f32, + limits_impulse: Real, + limits_rhs: Real, - axis2: Vector, + axis2: Vector, #[cfg(feature = "dim2")] - basis1: Vector2, + basis1: Vector2, #[cfg(feature = "dim3")] - basis1: Matrix3x2, - limits_forcedir2: Option>, + basis1: Matrix3x2, + limits_forcedir2: Option>, - im2: f32, - ii2_sqrt: AngularInertia, + im2: Real, + ii2_sqrt: AngularInertia, } impl PrismaticVelocityGroundConstraint { @@ -478,7 +478,7 @@ impl PrismaticVelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let lin_impulse = self.basis1 * self.impulse.fixed_rows::(0).into_owned(); @@ -499,7 +499,7 @@ impl PrismaticVelocityGroundConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; /* diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs index dadf5da..8e0a1ab 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs @@ -5,7 +5,7 @@ use crate::dynamics::{ IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody, }; use crate::math::{ - AngVector, AngularInertia, Isometry, Point, SimdBool, SimdReal, Vector, SIMD_WIDTH, + AngVector, AngularInertia, Isometry, Point, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[cfg(feature = "dim3")] @@ -236,7 +236,7 @@ impl WPrismaticVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -285,7 +285,7 @@ impl WPrismaticVelocityConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -585,7 +585,7 @@ impl WPrismaticVelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], @@ -616,7 +616,7 @@ impl WPrismaticVelocityGroundConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs index 876ce31..e46e0e4 100644 --- a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs @@ -1,5 +1,5 @@ use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody}; -use crate::math::{AngularInertia, Isometry, Point, Rotation, Vector}; +use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector}; use crate::utils::WAngularInertia; use na::Unit; @@ -8,20 +8,20 @@ pub(crate) struct RevolutePositionConstraint { position1: usize, position2: usize, - im1: f32, - im2: f32, + im1: Real, + im2: Real, - ii1: AngularInertia, - ii2: AngularInertia, + ii1: AngularInertia, + ii2: AngularInertia, - lin_inv_lhs: f32, - ang_inv_lhs: AngularInertia, + lin_inv_lhs: Real, + ang_inv_lhs: AngularInertia, - local_anchor1: Point, - local_anchor2: Point, + local_anchor1: Point, + local_anchor2: Point, - local_axis1: Unit>, - local_axis2: Unit>, + local_axis1: Unit>, + local_axis2: Unit>, } impl RevolutePositionConstraint { @@ -49,7 +49,7 @@ impl RevolutePositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position1 = positions[self.position1 as usize]; let mut position2 = positions[self.position2 as usize]; @@ -83,10 +83,10 @@ impl RevolutePositionConstraint { #[derive(Debug)] pub(crate) struct RevolutePositionGroundConstraint { position2: usize, - anchor1: Point, - local_anchor2: Point, - axis1: Unit>, - local_axis2: Unit>, + anchor1: Point, + local_anchor2: Point, + axis1: Unit>, + local_axis2: Unit>, } impl RevolutePositionGroundConstraint { @@ -122,7 +122,7 @@ impl RevolutePositionGroundConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position2 = positions[self.position2 as usize]; let axis2 = position2 * self.local_axis2; diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index caf8e40..e60bd8a 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody, }; -use crate::math::{AngularInertia, Vector}; +use crate::math::{AngularInertia, Real, Vector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; @@ -13,20 +13,20 @@ pub(crate) struct RevoluteVelocityConstraint { joint_id: JointIndex, - r1: Vector, - r2: Vector, + r1: Vector, + r2: Vector, - inv_lhs: Matrix5, - rhs: Vector5, - impulse: Vector5, + inv_lhs: Matrix5, + rhs: Vector5, + impulse: Vector5, - basis1: Matrix3x2, + basis1: Matrix3x2, - im1: f32, - im2: f32, + im1: Real, + im2: Real, - ii1_sqrt: AngularInertia, - ii2_sqrt: AngularInertia, + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, } impl RevoluteVelocityConstraint { @@ -99,7 +99,7 @@ impl RevoluteVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; @@ -120,7 +120,7 @@ impl RevoluteVelocityConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; @@ -165,17 +165,17 @@ pub(crate) struct RevoluteVelocityGroundConstraint { joint_id: JointIndex, - r2: Vector, + r2: Vector, - inv_lhs: Matrix5, - rhs: Vector5, - impulse: Vector5, + inv_lhs: Matrix5, + rhs: Vector5, + impulse: Vector5, - basis1: Matrix3x2, + basis1: Matrix3x2, - im2: f32, + im2: Real, - ii2_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, } impl RevoluteVelocityGroundConstraint { @@ -249,7 +249,7 @@ impl RevoluteVelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); @@ -263,7 +263,7 @@ impl RevoluteVelocityGroundConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs index 61122a4..1b8af80 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs @@ -4,7 +4,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody, }; -use crate::math::{AngVector, AngularInertia, Isometry, Point, SimdReal, Vector, SIMD_WIDTH}; +use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, SIMD_WIDTH}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; @@ -123,7 +123,7 @@ impl WRevoluteVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -164,7 +164,7 @@ impl WRevoluteVelocityConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -330,7 +330,7 @@ impl WRevoluteVelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], @@ -354,7 +354,7 @@ impl WRevoluteVelocityGroundConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs index 3b7ab9f..543b9b7 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -2,7 +2,7 @@ use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver}; use crate::dynamics::solver::ParallelPositionSolver; use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; -use crate::math::Isometry; +use crate::math::{Isometry, Real}; use crate::utils::WAngularInertia; use rayon::Scope; use std::sync::atomic::{AtomicUsize, Ordering}; @@ -119,8 +119,8 @@ impl ThreadContext { } pub struct ParallelIslandSolver { - mj_lambdas: Vec>, - positions: Vec>, + mj_lambdas: Vec>, + positions: Vec>, parallel_groups: ParallelInteractionGroups, parallel_joint_groups: ParallelInteractionGroups, parallel_velocity_solver: ParallelVelocitySolver, @@ -199,9 +199,9 @@ impl ParallelIslandSolver { scope.spawn(move |_| { // Transmute *mut -> &mut - let mj_lambdas: &mut Vec> = + let mj_lambdas: &mut Vec> = unsafe { std::mem::transmute(mj_lambdas.load(Ordering::Relaxed)) }; - let positions: &mut Vec> = + let positions: &mut Vec> = unsafe { std::mem::transmute(positions.load(Ordering::Relaxed)) }; let bodies: &mut RigidBodySet = unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) }; diff --git a/src/dynamics/solver/parallel_position_solver.rs b/src/dynamics/solver/parallel_position_solver.rs index b7a7876..dfcd0d4 100644 --- a/src/dynamics/solver/parallel_position_solver.rs +++ b/src/dynamics/solver/parallel_position_solver.rs @@ -4,7 +4,7 @@ use crate::dynamics::solver::categorization::{categorize_joints, categorize_posi use crate::dynamics::solver::{InteractionGroups, PositionConstraint, PositionGroundConstraint}; use crate::dynamics::{IntegrationParameters, JointGraphEdge, RigidBodySet}; use crate::geometry::ContactManifold; -use crate::math::Isometry; +use crate::math::{Isometry, Real}; #[cfg(feature = "simd-is-enabled")] use crate::{ dynamics::solver::{WPositionConstraint, WPositionGroundConstraint}, @@ -500,7 +500,7 @@ impl ParallelPositionSolver { &mut self, thread: &ThreadContext, params: &IntegrationParameters, - positions: &mut [Isometry], + positions: &mut [Isometry], ) { if self.part.constraint_descs.len() == 0 { return; diff --git a/src/dynamics/solver/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs index 6e8c681..ca2a987 100644 --- a/src/dynamics/solver/parallel_velocity_solver.rs +++ b/src/dynamics/solver/parallel_velocity_solver.rs @@ -317,7 +317,7 @@ impl ParallelVelocitySolver { params: &IntegrationParameters, manifolds_all: &mut [&mut ContactManifold], joints_all: &mut [JointGraphEdge], - mj_lambdas: &mut [DeltaVel], + mj_lambdas: &mut [DeltaVel], ) { if self.part.constraint_descs.len() == 0 && self.joint_part.constraint_descs.len() == 0 { return; diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs index 843fd1a..e7188f8 100644 --- a/src/dynamics/solver/position_constraint.rs +++ b/src/dynamics/solver/position_constraint.rs @@ -4,7 +4,7 @@ use crate::dynamics::solver::{WPositionConstraint, WPositionGroundConstraint}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::ContactManifold; use crate::math::{ - AngularInertia, Isometry, Point, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS, + AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS, }; use crate::utils::{WAngularInertia, WCross, WDot}; @@ -20,7 +20,7 @@ pub(crate) enum AnyPositionConstraint { } impl AnyPositionConstraint { - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { match self { #[cfg(feature = "simd-is-enabled")] AnyPositionConstraint::GroupedGround(c) => c.solve(params, positions), @@ -37,17 +37,17 @@ pub(crate) struct PositionConstraint { pub rb1: usize, pub rb2: usize, // NOTE: the points are relative to the center of masses. - pub local_p1: [Point; MAX_MANIFOLD_POINTS], - pub local_p2: [Point; MAX_MANIFOLD_POINTS], - pub dists: [f32; MAX_MANIFOLD_POINTS], - pub local_n1: Vector, + pub local_p1: [Point; MAX_MANIFOLD_POINTS], + pub local_p2: [Point; MAX_MANIFOLD_POINTS], + pub dists: [Real; MAX_MANIFOLD_POINTS], + pub local_n1: Vector, pub num_contacts: u8, - pub im1: f32, - pub im2: f32, - pub ii1: AngularInertia, - pub ii2: AngularInertia, - pub erp: f32, - pub max_linear_correction: f32, + pub im1: Real, + pub im2: Real, + pub ii1: AngularInertia, + pub ii2: AngularInertia, + pub erp: Real, + pub max_linear_correction: Real, } impl PositionConstraint { @@ -112,7 +112,7 @@ impl PositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { // FIXME: can we avoid most of the multiplications by pos1/pos2? // Compute jacobians. let mut pos1 = positions[self.rb1]; diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs index 7df7d5e..2bb74c9 100644 --- a/src/dynamics/solver/position_constraint_wide.rs +++ b/src/dynamics/solver/position_constraint_wide.rs @@ -2,8 +2,8 @@ use super::AnyPositionConstraint; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::ContactManifold; use crate::math::{ - AngularInertia, Isometry, Point, Rotation, SimdReal, Translation, Vector, MAX_MANIFOLD_POINTS, - SIMD_WIDTH, + AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Translation, Vector, + MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WDot}; @@ -94,7 +94,7 @@ impl WPositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { // FIXME: can we avoid most of the multiplications by pos1/pos2? // Compute jacobians. let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]); diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs index fcfdcdb..a8523f5 100644 --- a/src/dynamics/solver/position_ground_constraint.rs +++ b/src/dynamics/solver/position_ground_constraint.rs @@ -2,22 +2,22 @@ use super::AnyPositionConstraint; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::ContactManifold; use crate::math::{ - AngularInertia, Isometry, Point, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS, + AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS, }; use crate::utils::{WAngularInertia, WCross, WDot}; pub(crate) struct PositionGroundConstraint { pub rb2: usize, // NOTE: the points are relative to the center of masses. - pub p1: [Point; MAX_MANIFOLD_POINTS], - pub local_p2: [Point; MAX_MANIFOLD_POINTS], - pub dists: [f32; MAX_MANIFOLD_POINTS], - pub n1: Vector, + pub p1: [Point; MAX_MANIFOLD_POINTS], + pub local_p2: [Point; MAX_MANIFOLD_POINTS], + pub dists: [Real; MAX_MANIFOLD_POINTS], + pub n1: Vector, pub num_contacts: u8, - pub im2: f32, - pub ii2: AngularInertia, - pub erp: f32, - pub max_linear_correction: f32, + pub im2: Real, + pub ii2: AngularInertia, + pub erp: Real, + pub max_linear_correction: Real, } impl PositionGroundConstraint { @@ -79,7 +79,7 @@ impl PositionGroundConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { // FIXME: can we avoid most of the multiplications by pos1/pos2? // Compute jacobians. let mut pos2 = positions[self.rb2]; diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs index dbea1db..c6f1c3f 100644 --- a/src/dynamics/solver/position_ground_constraint_wide.rs +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -2,8 +2,8 @@ use super::AnyPositionConstraint; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::ContactManifold; use crate::math::{ - AngularInertia, Isometry, Point, Rotation, SimdReal, Translation, Vector, MAX_MANIFOLD_POINTS, - SIMD_WIDTH, + AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Translation, Vector, + MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WDot}; @@ -92,7 +92,7 @@ impl WPositionGroundConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { // FIXME: can we avoid most of the multiplications by pos1/pos2? // Compute jacobians. let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs index cf23168..df0e3fc 100644 --- a/src/dynamics/solver/position_solver.rs +++ b/src/dynamics/solver/position_solver.rs @@ -1,9 +1,9 @@ use super::AnyJointPositionConstraint; use crate::dynamics::{solver::AnyPositionConstraint, IntegrationParameters, RigidBodySet}; -use crate::math::Isometry; +use crate::math::{Isometry, Real}; pub(crate) struct PositionSolver { - positions: Vec>, + positions: Vec>, } impl PositionSolver { diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs index 92faa3b..1965d6d 100644 --- a/src/dynamics/solver/solver_constraints.rs +++ b/src/dynamics/solver/solver_constraints.rs @@ -2,11 +2,12 @@ use super::{ AnyJointVelocityConstraint, InteractionGroups, VelocityConstraint, VelocityGroundConstraint, }; #[cfg(feature = "simd-is-enabled")] -use super::{WVelocityConstraint, WVelocityGroundConstraint}; +use super::{ + WPositionConstraint, WPositionGroundConstraint, WVelocityConstraint, WVelocityGroundConstraint, +}; use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints}; use crate::dynamics::solver::{ - AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint, - PositionGroundConstraint, WPositionConstraint, WPositionGroundConstraint, + AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint, PositionGroundConstraint, }; use crate::dynamics::{ solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 6614b48..2cb3975 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -4,7 +4,7 @@ use crate::dynamics::solver::VelocityGroundConstraint; use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; -use crate::math::{AngVector, Vector, DIM, MAX_MANIFOLD_POINTS}; +use crate::math::{AngVector, Real, Vector, DIM, MAX_MANIFOLD_POINTS}; use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; use simba::simd::SimdPartialOrd; @@ -40,7 +40,7 @@ impl AnyVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { match self { AnyVelocityConstraint::NongroupedGround(c) => c.warmstart(mj_lambdas), AnyVelocityConstraint::Nongrouped(c) => c.warmstart(mj_lambdas), @@ -52,7 +52,7 @@ impl AnyVelocityConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { match self { AnyVelocityConstraint::NongroupedGround(c) => c.solve(mj_lambdas), AnyVelocityConstraint::Nongrouped(c) => c.solve(mj_lambdas), @@ -79,11 +79,11 @@ impl AnyVelocityConstraint { #[derive(Copy, Clone, Debug)] pub(crate) struct VelocityConstraintElementPart { - pub gcross1: AngVector, - pub gcross2: AngVector, - pub rhs: f32, - pub impulse: f32, - pub r: f32, + pub gcross1: AngVector, + pub gcross2: AngVector, + pub rhs: Real, + pub impulse: Real, + pub r: Real, } #[cfg(not(target_arch = "wasm32"))] @@ -117,10 +117,10 @@ impl VelocityConstraintElement { #[derive(Copy, Clone, Debug)] pub(crate) struct VelocityConstraint { - pub dir1: Vector, // Non-penetration force direction for the first body. - pub im1: f32, - pub im2: f32, - pub limit: f32, + pub dir1: Vector, // Non-penetration force direction for the first body. + pub im1: Real, + pub im2: Real, + pub limit: Real, pub mj_lambda1: usize, pub mj_lambda2: usize, pub manifold_id: ContactManifoldIndex, @@ -300,7 +300,7 @@ impl VelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel::zero(); let mut mj_lambda2 = DeltaVel::zero(); @@ -331,7 +331,7 @@ impl VelocityConstraint { mj_lambdas[self.mj_lambda2 as usize].angular += mj_lambda2.angular; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index c75d926..b86e6a6 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -2,7 +2,7 @@ use super::{AnyVelocityConstraint, DeltaVel}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ - AngVector, AngularInertia, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, + AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; use num::Zero; @@ -199,7 +199,7 @@ impl WVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -249,7 +249,7 @@ impl WVelocityConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index fa2a23e..37d4e3a 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -1,5 +1,5 @@ use super::{AnyVelocityConstraint, DeltaVel}; -use crate::math::{AngVector, Vector, DIM, MAX_MANIFOLD_POINTS}; +use crate::math::{AngVector, Real, Vector, DIM, MAX_MANIFOLD_POINTS}; use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; @@ -8,10 +8,10 @@ use simba::simd::SimdPartialOrd; #[derive(Copy, Clone, Debug)] pub(crate) struct VelocityGroundConstraintElementPart { - pub gcross2: AngVector, - pub rhs: f32, - pub impulse: f32, - pub r: f32, + pub gcross2: AngVector, + pub rhs: Real, + pub impulse: Real, + pub r: Real, } #[cfg(not(target_arch = "wasm32"))] @@ -44,9 +44,9 @@ impl VelocityGroundConstraintElement { #[derive(Copy, Clone, Debug)] pub(crate) struct VelocityGroundConstraint { - pub dir1: Vector, // Non-penetration force direction for the first body. - pub im2: f32, - pub limit: f32, + pub dir1: Vector, // Non-penetration force direction for the first body. + pub im2: Real, + pub limit: Real, pub mj_lambda2: usize, pub manifold_id: ContactManifoldIndex, pub manifold_contact_id: usize, @@ -207,7 +207,7 @@ impl VelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel::zero(); let tangents1 = self.dir1.orthonormal_basis(); @@ -227,7 +227,7 @@ impl VelocityGroundConstraint { mj_lambdas[self.mj_lambda2 as usize].angular += mj_lambda2.angular; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; // Solve friction. diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index feee344..1c85a90 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -2,7 +2,7 @@ use super::{AnyVelocityConstraint, DeltaVel}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ - AngVector, AngularInertia, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, + AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; use num::Zero; @@ -189,7 +189,7 @@ impl WVelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], @@ -219,7 +219,7 @@ impl WVelocityGroundConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { linear: Vector::from( array![ |ii| mj_lambdas[ self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index 332d809..dfb97b0 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -4,10 +4,11 @@ use crate::dynamics::{ IntegrationParameters, JointGraphEdge, RigidBodySet, }; use crate::geometry::ContactManifold; +use crate::math::Real; use crate::utils::WAngularInertia; pub(crate) struct VelocitySolver { - pub mj_lambdas: Vec>, + pub mj_lambdas: Vec>, } impl VelocitySolver { -- cgit