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 +++++------ 5 files changed, 179 insertions(+), 179 deletions(-) (limited to 'src/dynamics/solver/joint_constraint') 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, -- cgit