diff options
Diffstat (limited to 'src/dynamics/solver')
31 files changed, 282 insertions, 279 deletions
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<N: Scalar> { +pub(crate) struct DeltaVel<N: Scalar + Copy> { pub linear: Vector<N>, pub angular: AngVector<N>, } 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<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; 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<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; 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<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); 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<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], 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<f32>, - local_anchor2: Isometry<f32>, - local_com1: Point<f32>, - local_com2: Point<f32>, - im1: f32, - im2: f32, - ii1: AngularInertia<f32>, - ii2: AngularInertia<f32>, - - lin_inv_lhs: f32, - ang_inv_lhs: 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: 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. 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<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); 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<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], 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<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), 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<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), 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<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. 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 PrismaticVelocityCons |