From cc6d1b973002b4d366bc81ec6bf9e8240ad7b404 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 14 Dec 2020 15:51:43 +0100 Subject: Outsource the Shape trait, wquadtree, and shape types. --- .../ball_position_constraint_wide.rs | 44 ++++---- .../ball_velocity_constraint_wide.rs | 64 ++++++------ .../fixed_velocity_constraint_wide.rs | 76 +++++++------- .../prismatic_velocity_constraint_wide.rs | 114 ++++++++++----------- .../revolute_velocity_constraint_wide.rs | 60 +++++------ src/dynamics/solver/position_constraint_wide.rs | 48 ++++----- .../solver/position_ground_constraint_wide.rs | 40 ++++---- src/dynamics/solver/velocity_constraint_wide.rs | 67 ++++++------ .../solver/velocity_ground_constraint_wide.rs | 59 ++++++----- 9 files changed, 285 insertions(+), 287 deletions(-) (limited to 'src/dynamics/solver') 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 c552d57..f1ca4b6 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, SimdFloat, SIMD_WIDTH}; +use crate::math::{AngularInertia, Isometry, Point, Rotation, SimdReal, SIMD_WIDTH}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use simba::simd::SimdValue; @@ -10,17 +10,17 @@ pub(crate) struct WBallPositionConstraint { position1: [usize; SIMD_WIDTH], position2: [usize; SIMD_WIDTH], - local_com1: Point, - local_com2: Point, + local_com1: Point, + local_com2: Point, - im1: SimdFloat, - im2: SimdFloat, + im1: SimdReal, + im2: SimdReal, - ii1: AngularInertia, - ii2: AngularInertia, + ii1: AngularInertia, + ii2: AngularInertia, - local_anchor1: Point, - local_anchor2: Point, + local_anchor1: Point, + local_anchor2: Point, } impl WBallPositionConstraint { @@ -31,13 +31,13 @@ impl WBallPositionConstraint { ) -> Self { let local_com1 = Point::from(array![|ii| rbs1[ii].mass_properties.local_com; SIMD_WIDTH]); let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]); - let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii1 = AngularInertia::::from( + let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1 = AngularInertia::::from( array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ) .squared(); - let ii2 = AngularInertia::::from( + let ii2 = AngularInertia::::from( array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ) .squared(); @@ -97,7 +97,7 @@ impl WBallPositionConstraint { }; let inv_lhs = lhs.inverse_unchecked(); - let impulse = inv_lhs * -(err * SimdFloat::splat(params.joint_erp)); + let impulse = inv_lhs * -(err * SimdReal::splat(params.joint_erp)); position1.translation.vector += impulse * self.im1; position2.translation.vector -= impulse * self.im2; @@ -120,11 +120,11 @@ impl WBallPositionConstraint { #[derive(Debug)] pub(crate) struct WBallPositionGroundConstraint { position2: [usize; SIMD_WIDTH], - anchor1: Point, - im2: SimdFloat, - ii2: AngularInertia, - local_anchor2: Point, - local_com2: Point, + anchor1: Point, + im2: SimdReal, + ii2: AngularInertia, + local_anchor2: Point, + local_com2: Point, } impl WBallPositionGroundConstraint { @@ -141,8 +141,8 @@ impl WBallPositionGroundConstraint { } else { cparams[ii].local_anchor1 }; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2 = AngularInertia::::from( + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2 = AngularInertia::::from( array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ) .squared(); @@ -186,7 +186,7 @@ impl WBallPositionGroundConstraint { }; let inv_lhs = lhs.inverse_unchecked(); - let impulse = inv_lhs * -(err * SimdFloat::splat(params.joint_erp)); + let impulse = inv_lhs * -(err * SimdReal::splat(params.joint_erp)); position2.translation.vector -= impulse * self.im2; let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse)); 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 b96f3b8..bbb709e 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, SimdFloat, Vector, SIMD_WIDTH, + AngVector, AngularInertia, Isometry, Point, SdpMatrix, SimdReal, Vector, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use simba::simd::SimdValue; @@ -15,16 +15,16 @@ pub(crate) struct WBallVelocityConstraint { joint_id: [JointIndex; SIMD_WIDTH], - 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: SimdFloat, - im2: SimdFloat, + im1: SimdReal, + im2: SimdReal, } impl WBallVelocityConstraint { @@ -37,20 +37,20 @@ impl WBallVelocityConstraint { ) -> Self { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii1_sqrt = AngularInertia::::from( + let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1_sqrt = AngularInertia::::from( array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -62,8 +62,8 @@ impl WBallVelocityConstraint { let anchor1 = position1 * local_anchor1 - world_com1; let anchor2 = position2 * local_anchor2 - world_com2; - let vel1: Vector = linvel1 + angvel1.gcross(anchor1); - let vel2: Vector = linvel2 + angvel2.gcross(anchor2); + let vel1: Vector = linvel1 + angvel1.gcross(anchor1); + let vel2: Vector = linvel2 + angvel2.gcross(anchor2); let rhs = -(vel1 - vel2); let lhs; @@ -99,7 +99,7 @@ impl WBallVelocityConstraint { mj_lambda2, im1, im2, - impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + impulse: impulse * SimdReal::splat(params.warmstart_coeff), gcross1, gcross2, rhs, @@ -141,7 +141,7 @@ impl WBallVelocityConstraint { } pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda1: DeltaVel = DeltaVel { + let mut mj_lambda1: DeltaVel = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], ), @@ -149,7 +149,7 @@ impl WBallVelocityConstraint { array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], ), }; - let mut mj_lambda2: DeltaVel = DeltaVel { + let mut mj_lambda2: DeltaVel = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], ), @@ -195,11 +195,11 @@ impl WBallVelocityConstraint { pub(crate) struct WBallVelocityGroundConstraint { mj_lambda2: [usize; SIMD_WIDTH], joint_id: [JointIndex; SIMD_WIDTH], - rhs: Vector, - pub(crate) impulse: Vector, - gcross2: Vector, - inv_lhs: SdpMatrix, - im2: SimdFloat, + rhs: Vector, + pub(crate) impulse: Vector, + gcross2: Vector, + inv_lhs: SdpMatrix, + im2: SimdReal, } impl WBallVelocityGroundConstraint { @@ -213,7 +213,7 @@ impl WBallVelocityGroundConstraint { ) -> Self { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); let local_anchor1 = Point::from( array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH], @@ -221,10 +221,10 @@ impl WBallVelocityGroundConstraint { let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -237,8 +237,8 @@ impl WBallVelocityGroundConstraint { let anchor1 = position1 * local_anchor1 - world_com1; let anchor2 = position2 * local_anchor2 - world_com2; - let vel1: Vector = linvel1 + angvel1.gcross(anchor1); - let vel2: Vector = linvel2 + angvel2.gcross(anchor2); + let vel1: Vector = linvel1 + angvel1.gcross(anchor1); + let vel2: Vector = linvel2 + angvel2.gcross(anchor2); let rhs = vel2 - vel1; let lhs; @@ -267,7 +267,7 @@ impl WBallVelocityGroundConstraint { joint_id, mj_lambda2, im2, - impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + impulse: impulse * SimdReal::splat(params.warmstart_coeff), gcross2, rhs, inv_lhs, @@ -294,7 +294,7 @@ impl WBallVelocityGroundConstraint { } pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda2: DeltaVel = 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_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs index 7c87b2c..79c69c6 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs @@ -5,7 +5,7 @@ use crate::dynamics::{ FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, }; use crate::math::{ - AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, SimdFloat, SpacialVector, Vector, + AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, SimdReal, SpacialVector, Vector, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; @@ -24,29 +24,29 @@ pub(crate) struct WFixedVelocityConstraint { joint_id: [JointIndex; SIMD_WIDTH], - 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, + inv_lhs: Matrix3, #[cfg(feature = "dim2")] - rhs: Vector3, + rhs: Vector3, - im1: SimdFloat, - im2: SimdFloat, + im1: SimdReal, + im2: SimdReal, - 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 WFixedVelocityConstraint { @@ -59,20 +59,20 @@ impl WFixedVelocityConstraint { ) -> Self { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii1_sqrt = AngularInertia::::from( + let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1_sqrt = AngularInertia::::from( array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -150,7 +150,7 @@ impl WFixedVelocityConstraint { ii2, ii1_sqrt, ii2_sqrt, - impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + impulse: impulse * SimdReal::splat(params.warmstart_coeff), inv_lhs, r1, r2, @@ -203,7 +203,7 @@ impl WFixedVelocityConstraint { } pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda1: DeltaVel = DeltaVel { + let mut mj_lambda1: DeltaVel = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], ), @@ -211,7 +211,7 @@ impl WFixedVelocityConstraint { array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], ), }; - let mut mj_lambda2: DeltaVel = DeltaVel { + let mut mj_lambda2: DeltaVel = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], ), @@ -279,22 +279,22 @@ pub(crate) struct WFixedVelocityGroundConstraint { joint_id: [JointIndex; SIMD_WIDTH], - 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, + inv_lhs: Matrix3, #[cfg(feature = "dim2")] - rhs: Vector3, + rhs: Vector3, - im2: SimdFloat, - ii2: AngularInertia, - ii2_sqrt: AngularInertia, - r2: Vector, + im2: SimdReal, + ii2: AngularInertia, + ii2_sqrt: AngularInertia, + r2: Vector, } impl WFixedVelocityGroundConstraint { @@ -308,15 +308,15 @@ impl WFixedVelocityGroundConstraint { ) -> Self { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -386,7 +386,7 @@ impl WFixedVelocityGroundConstraint { im2, ii2, ii2_sqrt, - impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + impulse: impulse * SimdReal::splat(params.warmstart_coeff), inv_lhs, r2, rhs, @@ -421,7 +421,7 @@ impl WFixedVelocityGroundConstraint { } pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda2: DeltaVel = 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/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs index bb81f23..c05c08e 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, SimdFloat, Vector, SIMD_WIDTH, + AngVector, AngularInertia, Isometry, Point, SimdBool, SimdReal, Vector, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[cfg(feature = "dim3")] @@ -28,37 +28,37 @@ pub(crate) struct WPrismaticVelocityConstraint { joint_id: [JointIndex; SIMD_WIDTH], - 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: SimdFloat, - limits_forcedirs: Option<(Vector, Vector)>, - limits_rhs: SimdFloat, + limits_impulse: SimdReal, + limits_forcedirs: Option<(Vector, Vector)>, + limits_rhs: SimdReal, #[cfg(feature = "dim2")] - basis1: Vector2, + basis1: Vector2, #[cfg(feature = "dim3")] - basis1: Matrix3x2, + basis1: Matrix3x2, - im1: SimdFloat, - im2: SimdFloat, + im1: SimdReal, + im2: SimdReal, - ii1_sqrt: AngularInertia, - ii2_sqrt: AngularInertia, + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, } impl WPrismaticVelocityConstraint { @@ -71,20 +71,20 @@ impl WPrismaticVelocityConstraint { ) -> Self { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii1_sqrt = AngularInertia::::from( + let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1_sqrt = AngularInertia::::from( array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -199,14 +199,14 @@ impl WPrismaticVelocityConstraint { // FIXME: we should allow both limits to be active at // the same time + allow predictive constraint activation. - let min_limit = SimdFloat::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]); - let max_limit = SimdFloat::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]); - let lim_impulse = SimdFloat::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]); + let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]); + let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]); + let lim_impulse = SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]); let min_enabled = dist.simd_lt(min_limit); let max_enabled = dist.simd_gt(max_limit); - let _0: SimdFloat = na::zero(); - let _1: SimdFloat = na::one(); + let _0: SimdReal = na::zero(); + let _1: SimdReal = na::one(); let sign = _1.select(min_enabled, (-_1).select(max_enabled, _0)); if sign != _0 { @@ -224,8 +224,8 @@ impl WPrismaticVelocityConstraint { ii1_sqrt, im2, ii2_sqrt, - impulse: impulse * SimdFloat::splat(params.warmstart_coeff), - limits_impulse: limits_impulse * SimdFloat::splat(params.warmstart_coeff), + impulse: impulse * SimdReal::splat(params.warmstart_coeff), + limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff), limits_forcedirs, limits_rhs, basis1, @@ -383,34 +383,34 @@ pub(crate) struct WPrismaticVelocityGroundConstraint { joint_id: [JointIndex; SIMD_WIDTH], - 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: SimdFloat, - limits_rhs: SimdFloat, + limits_impulse: SimdReal, + limits_rhs: SimdReal, - 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: SimdFloat, - ii2_sqrt: AngularInertia, + im2: SimdReal, + ii2_sqrt: AngularInertia, } impl WPrismaticVelocityGroundConstraint { @@ -424,15 +424,15 @@ impl WPrismaticVelocityGroundConstraint { ) -> Self { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -551,14 +551,14 @@ impl WPrismaticVelocityGroundConstraint { // FIXME: we should allow both limits to be active at // the same time + allow predictive constraint activation. - let min_limit = SimdFloat::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]); - let max_limit = SimdFloat::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]); - let lim_impulse = SimdFloat::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]); + let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]); + let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]); + let lim_impulse = SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]); let use_min = dist.simd_lt(min_limit); let use_max = dist.simd_gt(max_limit); - let _0: SimdFloat = na::zero(); - let _1: SimdFloat = na::one(); + let _0: SimdReal = na::zero(); + let _1: SimdReal = na::one(); let sign = _1.select(use_min, (-_1).select(use_max, _0)); if sign != _0 { @@ -573,8 +573,8 @@ impl WPrismaticVelocityGroundConstraint { mj_lambda2, im2, ii2_sqrt, - impulse: impulse * SimdFloat::splat(params.warmstart_coeff), - limits_impulse: limits_impulse * SimdFloat::splat(params.warmstart_coeff), + impulse: impulse * SimdReal::splat(params.warmstart_coeff), + limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff), basis1, inv_lhs, rhs, 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 5eeac18..61122a4 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, SimdFloat, Vector, SIMD_WIDTH}; +use crate::math::{AngVector, AngularInertia, Isometry, Point, SimdReal, Vector, SIMD_WIDTH}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; @@ -15,20 +15,20 @@ pub(crate) struct WRevoluteVelocityConstraint { joint_id: [JointIndex; SIMD_WIDTH], - 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: SimdFloat, - im2: SimdFloat, + im1: SimdReal, + im2: SimdReal, - ii1_sqrt: AngularInertia, - ii2_sqrt: AngularInertia, + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, } impl WRevoluteVelocityConstraint { @@ -41,20 +41,20 @@ impl WRevoluteVelocityConstraint { ) -> Self { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii1_sqrt = AngularInertia::::from( + let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1_sqrt = AngularInertia::::from( array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -115,7 +115,7 @@ impl WRevoluteVelocityConstraint { basis1, im2, ii2_sqrt, - impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + impulse: impulse * SimdReal::splat(params.warmstart_coeff), inv_lhs, rhs, r1, @@ -231,17 +231,17 @@ pub(crate) struct WRevoluteVelocityGroundConstraint { joint_id: [JointIndex; SIMD_WIDTH], - r2: Vector, + r2: Vector, - inv_lhs: Matrix5, - rhs: Vector5, - impulse: Vector5, + inv_lhs: Matrix5, + rhs: Vector5, + impulse: Vector5, - basis1: Matrix3x2, + basis1: Matrix3x2, - im2: SimdFloat, + im2: SimdReal, - ii2_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, } impl WRevoluteVelocityGroundConstraint { @@ -255,15 +255,15 @@ impl WRevoluteVelocityGroundConstraint { ) -> Self { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -322,7 +322,7 @@ impl WRevoluteVelocityGroundConstraint { mj_lambda2, im2, ii2_sqrt, - impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + impulse: impulse * SimdReal::splat(params.warmstart_coeff), basis1, inv_lhs, rhs, diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs index ecc0fc3..260d495 100644 --- a/src/dynamics/solver/position_constraint_wide.rs +++ b/src/dynamics/solver/position_constraint_wide.rs @@ -2,7 +2,7 @@ use super::AnyPositionConstraint; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, KinematicsCategory}; use crate::math::{ - AngularInertia, Isometry, Point, Rotation, SimdFloat, Translation, Vector, MAX_MANIFOLD_POINTS, + AngularInertia, Isometry, Point, Rotation, SimdReal, Translation, Vector, MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WDot}; @@ -14,16 +14,16 @@ pub(crate) struct WPositionConstraint { pub rb1: [usize; SIMD_WIDTH], pub rb2: [usize; SIMD_WIDTH], // 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 local_n1: Vector, - pub radius: SimdFloat, - pub im1: SimdFloat, - pub im2: SimdFloat, - pub ii1: AngularInertia, - pub ii2: AngularInertia, - pub erp: SimdFloat, - pub max_linear_correction: SimdFloat, + pub local_p1: [Point; MAX_MANIFOLD_POINTS], + pub local_p2: [Point; MAX_MANIFOLD_POINTS], + pub local_n1: Vector, + pub radius: SimdReal, + pub im1: SimdReal, + pub im2: SimdReal, + pub ii1: AngularInertia, + pub ii2: AngularInertia, + pub erp: SimdReal, + pub max_linear_correction: SimdReal, pub num_contacts: u8, } @@ -38,18 +38,18 @@ impl WPositionConstraint { let rbs1 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH]; let rbs2 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH]; - let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let sqrt_ii1: AngularInertia = + let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let sqrt_ii1: AngularInertia = AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let sqrt_ii2: AngularInertia = + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let sqrt_ii2: AngularInertia = AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); let local_n1 = Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]); let local_n2 = Vector::from(array![|ii| manifolds[ii].local_n2; SIMD_WIDTH]); - let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); - let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]); + let radius1 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); + let radius2 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]); let delta1 = Isometry::from(array![|ii| manifolds[ii].data.delta1; SIMD_WIDTH]); let delta2 = Isometry::from(array![|ii| manifolds[ii].data.delta2; SIMD_WIDTH]); @@ -57,7 +57,7 @@ impl WPositionConstraint { let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let radius = radius1 + radius2 /*- SimdFloat::splat(params.allowed_linear_error)*/; + let radius = radius1 + radius2 /*- SimdReal::splat(params.allowed_linear_error)*/; for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH]; @@ -74,8 +74,8 @@ impl WPositionConstraint { im2, ii1: sqrt_ii1.squared(), ii2: sqrt_ii2.squared(), - erp: SimdFloat::splat(params.erp), - max_linear_correction: SimdFloat::splat(params.max_linear_correction), + erp: SimdReal::splat(params.erp), + max_linear_correction: SimdReal::splat(params.max_linear_correction), num_contacts: num_points as u8, }; @@ -119,7 +119,7 @@ impl WPositionConstraint { // Compute jacobians. let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]); let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); - let allowed_err = SimdFloat::splat(params.allowed_linear_error); + let allowed_err = SimdReal::splat(params.allowed_linear_error); let target_dist = self.radius - allowed_err; for k in 0..self.num_contacts as usize { @@ -133,7 +133,7 @@ impl WPositionConstraint { let dist = sqdist.simd_sqrt(); let n = dpos / dist; let err = ((dist - target_dist) * self.erp) - .simd_clamp(-self.max_linear_correction, SimdFloat::zero()); + .simd_clamp(-self.max_linear_correction, SimdReal::zero()); let dp1 = p1.coords - pos1.translation.vector; let dp2 = p2.coords - pos2.translation.vector; @@ -173,7 +173,7 @@ impl WPositionConstraint { // Compute jacobians. let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]); let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); - let allowed_err = SimdFloat::splat(params.allowed_linear_error); + let allowed_err = SimdReal::splat(params.allowed_linear_error); let target_dist = self.radius - allowed_err; for k in 0..self.num_contacts as usize { @@ -188,7 +188,7 @@ impl WPositionConstraint { // NOTE: only works for the point-point case. let p1 = p2 - n1 * dist; let err = ((dist - target_dist) * self.erp) - .simd_clamp(-self.max_linear_correction, SimdFloat::zero()); + .simd_clamp(-self.max_linear_correction, SimdReal::zero()); let dp1 = p1.coords - pos1.translation.vector; let dp2 = p2.coords - pos2.translation.vector; diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs index 1609709..2531b2e 100644 --- a/src/dynamics/solver/position_ground_constraint_wide.rs +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -2,7 +2,7 @@ use super::AnyPositionConstraint; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, KinematicsCategory}; use crate::math::{ - AngularInertia, Isometry, Point, Rotation, SimdFloat, Translation, Vector, MAX_MANIFOLD_POINTS, + AngularInertia, Isometry, Point, Rotation, SimdReal, Translation, Vector, MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WDot}; @@ -13,14 +13,14 @@ use simba::simd::{SimdBool as _, SimdComplexField, SimdPartialOrd, SimdValue}; pub(crate) struct WPositionGroundConstraint { pub rb2: [usize; SIMD_WIDTH], // NOTE: the points are relative to the center of masses. - pub p1: [Point; MAX_MANIFOLD_POINTS], - pub local_p2: [Point; MAX_MANIFOLD_POINTS], - pub n1: Vector, - pub radius: SimdFloat, - pub im2: SimdFloat, - pub ii2: AngularInertia, - pub erp: SimdFloat, - pub max_linear_correction: SimdFloat, + pub p1: [Point; MAX_MANIFOLD_POINTS], + pub local_p2: [Point; MAX_MANIFOLD_POINTS], + pub n1: Vector, + pub radius: SimdReal, + pub im2: SimdReal, + pub ii2: AngularInertia, + pub erp: SimdReal, + pub max_linear_correction: SimdReal, pub num_contacts: u8, } @@ -45,8 +45,8 @@ impl WPositionGroundConstraint { } } - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let sqrt_ii2: AngularInertia = + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let sqrt_ii2: AngularInertia = AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); let local_n1 = Vector::from( @@ -63,15 +63,15 @@ impl WPositionGroundConstraint { array![|ii| if flipped[ii] { manifolds[ii].data.delta1 } else { manifolds[ii].data.delta2 }; SIMD_WIDTH], ); - let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); - let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]); + let radius1 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); + let radius2 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]); let coll_pos1 = delta1 * Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]); let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let radius = radius1 + radius2 /*- SimdFloat::splat(params.allowed_linear_error)*/; + let radius = radius1 + radius2 /*- SimdReal::splat(params.allowed_linear_error)*/; let n1 = coll_pos1 * local_n1; @@ -87,8 +87,8 @@ impl WPositionGroundConstraint { radius, im2, ii2: sqrt_ii2.squared(), - erp: SimdFloat::splat(params.erp), - max_linear_correction: SimdFloat::splat(params.max_linear_correction), + erp: SimdReal::splat(params.erp), + max_linear_correction: SimdReal::splat(params.max_linear_correction), num_contacts: num_points as u8, }; @@ -131,7 +131,7 @@ impl WPositionGroundConstraint { // 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]); - let allowed_err = SimdFloat::splat(params.allowed_linear_error); + let allowed_err = SimdReal::splat(params.allowed_linear_error); let target_dist = self.radius - allowed_err; for k in 0..self.num_contacts as usize { @@ -145,7 +145,7 @@ impl WPositionGroundConstraint { let dist = sqdist.simd_sqrt(); let n = dpos / dist; let err = ((dist - target_dist) * self.erp) - .simd_clamp(-self.max_linear_correction, SimdFloat::zero()); + .simd_clamp(-self.max_linear_correction, SimdReal::zero()); let dp2 = p2.coords - pos2.translation.vector; let gcross2 = -dp2.gcross(n); let ii_gcross2 = self.ii2.transform_vector(gcross2); @@ -173,7 +173,7 @@ impl WPositionGroundConstraint { // 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]); - let allowed_err = SimdFloat::splat(params.allowed_linear_error); + let allowed_err = SimdReal::splat(params.allowed_linear_error); let target_dist = self.radius - allowed_err; for k in 0..self.num_contacts as usize { @@ -186,7 +186,7 @@ impl WPositionGroundConstraint { // NOTE: this condition does not seem to be useful perfomancewise? if dist.simd_lt(target_dist).any() { let err = ((dist - target_dist) * self.erp) - .simd_clamp(-self.max_linear_correction, SimdFloat::zero()); + .simd_clamp(-self.max_linear_correction, SimdReal::zero()); let dp2 = p2.coords - pos2.translation.vector; let gcross2 = -dp2.gcross(n1); diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index a4efb9a..11e7a9d 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, Isometry, Point, SimdFloat, Vector, DIM, MAX_MANIFOLD_POINTS, + AngVector, AngularInertia, Isometry, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; @@ -11,11 +11,11 @@ use simba::simd::{SimdPartialOrd, SimdValue}; #[derive(Copy, Clone, Debug)] pub(crate) struct WVelocityConstraintElementPart { - pub gcross1: AngVector, - pub gcross2: AngVector, - pub rhs: SimdFloat, - pub impulse: SimdFloat, - pub r: SimdFloat, + pub gcross1: AngVector, + pub gcross2: AngVector, + pub rhs: SimdReal, + pub impulse: SimdReal, + pub r: SimdReal, } impl WVelocityConstraintElementPart { @@ -23,9 +23,9 @@ impl WVelocityConstraintElementPart { Self { gcross1: AngVector::zero(), gcross2: AngVector::zero(), - rhs: SimdFloat::zero(), - impulse: SimdFloat::zero(), - r: SimdFloat::zero(), + rhs: SimdReal::zero(), + impulse: SimdReal::zero(), + r: SimdReal::zero(), } } } @@ -47,12 +47,12 @@ impl WVelocityConstraintElement { #[derive(Copy, Clone, Debug)] pub(crate) struct WVelocityConstraint { - pub dir1: Vector, // Non-penetration force direction for the first body. + pub dir1: Vector, // Non-penetration force direction for the first body. pub elements: [WVelocityConstraintElement; MAX_MANIFOLD_POINTS], pub num_contacts: u8, - pub im1: SimdFloat, - pub im2: SimdFloat, - pub limit: SimdFloat, + pub im1: SimdReal, + pub im2: SimdReal, + pub limit: SimdReal, pub mj_lambda1: [usize; SIMD_WIDTH], pub mj_lambda2: [usize; SIMD_WIDTH], pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], @@ -68,29 +68,29 @@ impl WVelocityConstraint { out_constraints: &mut Vec, push: bool, ) { - let inv_dt = SimdFloat::splat(params.inv_dt()); + let inv_dt = SimdReal::splat(params.inv_dt()); let rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH]; let rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH]; let delta1 = Isometry::from(array![|ii| manifolds[ii].data.delta1; SIMD_WIDTH]); let delta2 = Isometry::from(array![|ii| manifolds[ii].data.delta2; SIMD_WIDTH]); - let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii1: AngularInertia = + let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1: AngularInertia = AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2: AngularInertia = + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2: AngularInertia = AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); @@ -103,14 +103,13 @@ impl WVelocityConstraint { let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let friction = SimdFloat::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]); - let restitution = SimdFloat::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]); - let restitution_velocity_threshold = - SimdFloat::splat(params.restitution_velocity_threshold); + let friction = SimdReal::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]); + let restitution = SimdReal::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]); + let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold); let warmstart_multiplier = - SimdFloat::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); - let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff); + SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); + let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff); for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH]; @@ -137,10 +136,10 @@ impl WVelocityConstraint { let p2 = coll_pos2 * Point::from(array![|ii| manifold_points[ii][k].local_p2; SIMD_WIDTH]); - let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); + let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); let impulse = - SimdFloat::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); + SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); let dp1 = p1 - world_com1; let dp2 = p2 - world_com2; @@ -153,13 +152,13 @@ impl WVelocityConstraint { let gcross1 = ii1.transform_vector(dp1.gcross(force_dir1)); let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); - let r = SimdFloat::splat(1.0) + let r = SimdReal::splat(1.0) / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); let mut rhs = (vel1 - vel2).dot(&force_dir1); let use_restitution = rhs.simd_le(-restitution_velocity_threshold); let rhs_with_restitution = rhs + rhs * restitution; rhs = rhs_with_restitution.select(use_restitution, rhs); - rhs += dist.simd_max(SimdFloat::zero()) * inv_dt; + rhs += dist.simd_max(SimdReal::zero()) * inv_dt; constraint.elements[k].normal_part = WVelocityConstraintElementPart { gcross1, @@ -175,17 +174,17 @@ impl WVelocityConstraint { for j in 0..DIM - 1 { #[cfg(feature = "dim2")] - let impulse = SimdFloat::from( + let impulse = SimdReal::from( array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH], ); #[cfg(feature = "dim3")] - let impulse = SimdFloat::from( + let impulse = SimdReal::from( array![|ii| manifold_points[ii][k].data.tangent_impulse[j]; SIMD_WIDTH], ); let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j])); let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); - let r = SimdFloat::splat(1.0) + let r = SimdReal::splat(1.0) / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); let rhs = (vel1 - vel2).dot(&tangents1[j]); @@ -309,7 +308,7 @@ impl WVelocityConstraint { - self.dir1.dot(&mj_lambda2.linear) + elt.gcross2.gdot(mj_lambda2.angular) + elt.rhs; - let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdFloat::zero()); + let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdReal::zero()); let dlambda = new_impulse - elt.impulse; elt.impulse = new_impulse; diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index 84c4182..ad16efe 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, Isometry, Point, SimdFloat, Vector, DIM, MAX_MANIFOLD_POINTS, + AngVector, AngularInertia, Isometry, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; @@ -11,19 +11,19 @@ use simba::simd::{SimdPartialOrd, SimdValue}; #[derive(Copy, Clone, Debug)] pub(crate) struct WVelocityGroundConstraintElementPart { - pub gcross2: AngVector, - pub rhs: SimdFloat, - pub impulse: SimdFloat, - pub r: SimdFloat, + pub gcross2: AngVector, + pub rhs: SimdReal, + pub impulse: SimdReal, + pub r: SimdReal, } impl WVelocityGroundConstraintElementPart { pub fn zero() -> Self { Self { gcross2: AngVector::zero(), - rhs: SimdFloat::zero(), - impulse: SimdFloat::zero(), - r: SimdFloat::zero(), + rhs: SimdReal::zero(), + impulse: SimdReal::zero(), + r: SimdReal::zero(), } } } @@ -45,11 +45,11 @@ impl WVelocityGroundConstraintElement { #[derive(Copy, Clone, Debug)] pub(crate) struct WVelocityGroundConstraint { - pub dir1: Vector, // Non-penetration force direction for the first body. + pub dir1: Vector, // Non-penetration force direction for the first body. pub elements: [WVelocityGroundConstraintElement; MAX_MANIFOLD_POINTS], pub num_contacts: u8, - pub im2: SimdFloat, - pub limit: SimdFloat, + pub im2: SimdReal, + pub limit: SimdReal, pub mj_lambda2: [usize; SIMD_WIDTH], pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], pub manifold_contact_id: usize, @@ -64,7 +64,7 @@ impl WVelocityGroundConstraint { out_constraints: &mut Vec, push: bool, ) { - let inv_dt = SimdFloat::splat(params.inv_dt()); + let inv_dt = SimdReal::splat(params.inv_dt()); let mut rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH]; let mut rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH]; let mut flipped = [false; SIMD_WIDTH]; @@ -76,15 +76,15 @@ impl WVelocityGroundConstraint { } } - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2: AngularInertia = + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2: AngularInertia = AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); @@ -109,14 +109,13 @@ impl WVelocityGroundConstraint { let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let friction = SimdFloat::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]); - let restitution = SimdFloat::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]); - let restitution_velocity_threshold = - SimdFloat::splat(params.restitution_velocity_threshold); + let friction = SimdReal::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]); + let restitution = SimdReal::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]); + let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold); let warmstart_multiplier = - SimdFloat::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); - let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff); + SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); + let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff); for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH]; @@ -143,10 +142,10 @@ impl WVelocityGroundConstraint { array![|ii| if flipped[ii] { manifold_points[ii][k].local_p1 } else { manifold_points[ii][k].local_p2 }; SIMD_WIDTH],