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 From e231bacec608fa5efd24f7a876572927dbd6c9c4 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Thu, 17 Dec 2020 10:24:36 +0100 Subject: Move all the contact manifold computations out of Rapier. --- src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs | 2 +- src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs | 2 +- .../solver/joint_constraint/prismatic_velocity_constraint_wide.rs | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) (limited to 'src/dynamics/solver/joint_constraint') 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 79c69c6..ce42da8 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs @@ -13,7 +13,7 @@ use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::{Cholesky, Matrix6, Vector6, U3}; #[cfg(feature = "dim2")] use { - crate::utils::SdpMatrix3, + eagl::utils::SdpMatrix3, na::{Matrix3, Vector3}, }; diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index 49cfc7a..a2c7c2c 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -8,7 +8,7 @@ use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; #[cfg(feature = "dim2")] use { - crate::utils::SdpMatrix2, + eagl::utils::SdpMatrix2, na::{Matrix2, Vector2}, }; 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 c05c08e..86e0c78 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs @@ -12,7 +12,7 @@ use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; #[cfg(feature = "dim2")] use { - crate::utils::SdpMatrix2, + eagl::utils::SdpMatrix2, na::{Matrix2, Vector2}, }; -- cgit From 8fe2df126a279a435cc544b150aadf8f7b757868 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Thu, 17 Dec 2020 18:37:16 +0100 Subject: Remove some irrelevant code. --- src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs | 2 +- src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs | 2 +- .../solver/joint_constraint/prismatic_velocity_constraint_wide.rs | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) (limited to 'src/dynamics/solver/joint_constraint') 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 ce42da8..6584ea2 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs @@ -13,7 +13,7 @@ use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::{Cholesky, Matrix6, Vector6, U3}; #[cfg(feature = "dim2")] use { - eagl::utils::SdpMatrix3, + cdl::utils::SdpMatrix3, na::{Matrix3, Vector3}, }; diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index a2c7c2c..9e39bd6 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -8,7 +8,7 @@ use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; #[cfg(feature = "dim2")] use { - eagl::utils::SdpMatrix2, + cdl::utils::SdpMatrix2, na::{Matrix2, Vector2}, }; 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 86e0c78..dadf5da 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs @@ -12,7 +12,7 @@ use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; #[cfg(feature = "dim2")] use { - eagl::utils::SdpMatrix2, + cdl::utils::SdpMatrix2, na::{Matrix2, Vector2}, }; -- cgit From aa61fe65e3ff0289ecab57b4053a3410cf6d4a87 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 4 Jan 2021 15:14:25 +0100 Subject: Add support of 64-bits reals. --- .../joint_constraint/ball_position_constraint.rs | 32 +++++----- .../ball_position_constraint_wide.rs | 6 +- .../joint_constraint/ball_velocity_constraint.rs | 34 +++++----- .../ball_velocity_constraint_wide.rs | 10 +-- .../joint_constraint/fixed_position_constraint.rs | 40 ++++++------ .../joint_constraint/fixed_velocity_constraint.rs | 54 ++++++++-------- .../fixed_velocity_constraint_wide.rs | 12 ++-- .../solver/joint_constraint/joint_constraint.rs | 5 +- .../joint_constraint/joint_position_constraint.rs | 4 +- .../prismatic_position_constraint.rs | 38 +++++------ .../prismatic_velocity_constraint.rs | 74 +++++++++++----------- .../prismatic_velocity_constraint_wide.rs | 10 +-- .../revolute_position_constraint.rs | 34 +++++----- .../revolute_velocity_constraint.rs | 44 ++++++------- .../revolute_velocity_constraint_wide.rs | 10 +-- 15 files changed, 204 insertions(+), 203 deletions(-) (limited to 'src/dynamics/solver/joint_constraint') diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs index 21a537e..8bc9072 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs @@ -1,7 +1,7 @@ use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; #[cfg(feature = "dim2")] use crate::math::SdpMatrix; -use crate::math::{AngularInertia, Isometry, Point, Rotation}; +use crate::math::{AngularInertia, Isometry, Point, Real, Rotation}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[derive(Debug)] @@ -9,17 +9,17 @@ pub(crate) struct BallPositionConstraint { position1: usize, position2: usize, - local_com1: Point, - local_com2: Point, + local_com1: Point, + local_com2: Point, - im1: f32, - im2: f32, + im1: Real, + im2: Real, - ii1: AngularInertia, - ii2: AngularInertia, + ii1: AngularInertia, + ii2: AngularInertia, - local_anchor1: Point, - local_anchor2: Point, + local_anchor1: Point, + local_anchor2: Point, } impl BallPositionConstraint { @@ -38,7 +38,7 @@ impl BallPositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position1 = positions[self.position1 as usize]; let mut position2 = positions[self.position2 as usize]; @@ -95,11 +95,11 @@ impl BallPositionConstraint { #[derive(Debug)] pub(crate) struct BallPositionGroundConstraint { position2: usize, - anchor1: Point, - im2: f32, - ii2: AngularInertia, - local_anchor2: Point, - local_com2: Point, + anchor1: Point, + im2: Real, + ii2: AngularInertia, + local_anchor2: Point, + local_com2: Point, } impl BallPositionGroundConstraint { @@ -133,7 +133,7 @@ impl BallPositionGroundConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position2 = positions[self.position2 as usize]; let anchor2 = position2 * self.local_anchor2; diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs index f1ca4b6..2f2ffc3 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs @@ -1,7 +1,7 @@ use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; #[cfg(feature = "dim2")] use crate::math::SdpMatrix; -use crate::math::{AngularInertia, Isometry, Point, Rotation, SimdReal, SIMD_WIDTH}; +use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, SimdReal, SIMD_WIDTH}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use simba::simd::SimdValue; @@ -60,7 +60,7 @@ impl WBallPositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position1 = Isometry::from(array![|ii| positions[self.position1[ii]]; SIMD_WIDTH]); let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]); @@ -164,7 +164,7 @@ impl WBallPositionGroundConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]); let anchor2 = position2 * self.local_anchor2; diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs index 97ba244..9f47624 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs @@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, }; -use crate::math::{SdpMatrix, Vector}; +use crate::math::{Real, SdpMatrix, Vector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[derive(Debug)] @@ -12,16 +12,16 @@ pub(crate) struct BallVelocityConstraint { joint_id: JointIndex, - rhs: Vector, - pub(crate) impulse: Vector, + rhs: Vector, + pub(crate) impulse: Vector, - gcross1: Vector, - gcross2: Vector, + gcross1: Vector, + gcross2: Vector, - inv_lhs: SdpMatrix, + inv_lhs: SdpMatrix, - im1: f32, - im2: f32, + im1: Real, + im2: Real, } impl BallVelocityConstraint { @@ -91,7 +91,7 @@ impl BallVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; @@ -104,7 +104,7 @@ impl BallVelocityConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; @@ -137,11 +137,11 @@ impl BallVelocityConstraint { pub(crate) struct BallVelocityGroundConstraint { mj_lambda2: usize, joint_id: JointIndex, - rhs: Vector, - impulse: Vector, - gcross2: Vector, - inv_lhs: SdpMatrix, - im2: f32, + rhs: Vector, + impulse: Vector, + gcross2: Vector, + inv_lhs: SdpMatrix, + im2: Real, } impl BallVelocityGroundConstraint { @@ -206,14 +206,14 @@ impl BallVelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; mj_lambda2.linear -= self.im2 * self.impulse; mj_lambda2.angular -= self.gcross2.gcross(self.impulse); mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2); diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs index bbb709e..2806233 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs @@ -3,7 +3,7 @@ use crate::dynamics::{ BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, }; use crate::math::{ - AngVector, AngularInertia, Isometry, Point, SdpMatrix, SimdReal, Vector, SIMD_WIDTH, + AngVector, AngularInertia, Isometry, Point, Real, SdpMatrix, SimdReal, Vector, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use simba::simd::SimdValue; @@ -107,7 +107,7 @@ impl WBallVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -140,7 +140,7 @@ impl WBallVelocityConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1: DeltaVel = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -274,7 +274,7 @@ impl WBallVelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], @@ -293,7 +293,7 @@ impl WBallVelocityGroundConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2: DeltaVel = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs index 24001cd..cf9dcb7 100644 --- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs @@ -1,22 +1,22 @@ use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody}; -use crate::math::{AngularInertia, Isometry, Point, Rotation}; +use crate::math::{AngularInertia, Isometry, Point, Real, Rotation}; use crate::utils::WAngularInertia; #[derive(Debug)] pub(crate) struct FixedPositionConstraint { position1: usize, position2: usize, - local_anchor1: Isometry, - local_anchor2: Isometry, - local_com1: Point, - local_com2: Point, - im1: f32, - im2: f32, - ii1: AngularInertia, - ii2: AngularInertia, - - lin_inv_lhs: f32, - ang_inv_lhs: AngularInertia, + local_anchor1: Isometry, + local_anchor2: Isometry, + local_com1: Point, + local_com2: Point, + im1: Real, + im2: Real, + ii1: AngularInertia, + ii2: AngularInertia, + + lin_inv_lhs: Real, + ang_inv_lhs: AngularInertia, } impl FixedPositionConstraint { @@ -44,7 +44,7 @@ impl FixedPositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position1 = positions[self.position1 as usize]; let mut position2 = positions[self.position2 as usize]; @@ -81,12 +81,12 @@ impl FixedPositionConstraint { #[derive(Debug)] pub(crate) struct FixedPositionGroundConstraint { position2: usize, - anchor1: Isometry, - local_anchor2: Isometry, - local_com2: Point, - im2: f32, - ii2: AngularInertia, - impulse: f32, + anchor1: Isometry, + local_anchor2: Isometry, + local_com2: Point, + im2: Real, + ii2: AngularInertia, + impulse: Real, } impl FixedPositionGroundConstraint { @@ -118,7 +118,7 @@ impl FixedPositionGroundConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position2 = positions[self.position2 as usize]; // Angular correction. diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs index e4187c8..715b7dd 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs @@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, }; -use crate::math::{AngularInertia, Dim, SpacialVector, Vector}; +use crate::math::{AngularInertia, Dim, Real, SpacialVector, Vector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[cfg(feature = "dim2")] use na::{Matrix3, Vector3}; @@ -16,29 +16,29 @@ pub(crate) struct FixedVelocityConstraint { joint_id: JointIndex, - impulse: SpacialVector, + impulse: SpacialVector, #[cfg(feature = "dim3")] - inv_lhs: Matrix6, // FIXME: replace by Cholesky. + inv_lhs: Matrix6, // FIXME: replace by Cholesky. #[cfg(feature = "dim3")] - rhs: Vector6, + rhs: Vector6, #[cfg(feature = "dim2")] - inv_lhs: Matrix3, // FIXME: replace by Cholesky. + inv_lhs: Matrix3, // FIXME: replace by Cholesky. #[cfg(feature = "dim2")] - rhs: Vector3, + rhs: Vector3, - im1: f32, - im2: f32, + im1: Real, + im2: Real, - ii1: AngularInertia, - ii2: AngularInertia, + ii1: AngularInertia, + ii2: AngularInertia, - ii1_sqrt: AngularInertia, - ii2_sqrt: AngularInertia, + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, - r1: Vector, - r2: Vector, + r1: Vector, + r2: Vector, } impl FixedVelocityConstraint { @@ -128,7 +128,7 @@ impl FixedVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; @@ -152,7 +152,7 @@ impl FixedVelocityConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; @@ -207,22 +207,22 @@ pub(crate) struct FixedVelocityGroundConstraint { joint_id: JointIndex, - impulse: SpacialVector, + impulse: SpacialVector, #[cfg(feature = "dim3")] - inv_lhs: Matrix6, // FIXME: replace by Cholesky. + inv_lhs: Matrix6, // FIXME: replace by Cholesky. #[cfg(feature = "dim3")] - rhs: Vector6, + rhs: Vector6, #[cfg(feature = "dim2")] - inv_lhs: Matrix3, // FIXME: replace by Cholesky. + inv_lhs: Matrix3, // FIXME: replace by Cholesky. #[cfg(feature = "dim2")] - rhs: Vector3, + rhs: Vector3, - im2: f32, - ii2: AngularInertia, - ii2_sqrt: AngularInertia, - r2: Vector, + im2: Real, + ii2: AngularInertia, + ii2_sqrt: AngularInertia, + r2: Vector, } impl FixedVelocityGroundConstraint { @@ -312,7 +312,7 @@ impl FixedVelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); @@ -329,7 +329,7 @@ impl FixedVelocityGroundConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs index 6584ea2..48a6cba 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs @@ -5,8 +5,8 @@ use crate::dynamics::{ FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, }; use crate::math::{ - AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, SimdReal, SpacialVector, Vector, - SIMD_WIDTH, + AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, Real, SimdReal, SpacialVector, + Vector, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[cfg(feature = "dim3")] @@ -158,7 +158,7 @@ impl WFixedVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -202,7 +202,7 @@ impl WFixedVelocityConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1: DeltaVel = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -393,7 +393,7 @@ impl WFixedVelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], @@ -420,7 +420,7 @@ impl WFixedVelocityGroundConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2: DeltaVel = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs index 8b1f8bf..ed6d758 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs @@ -17,6 +17,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet, }; +use crate::math::Real; #[cfg(feature = "simd-is-enabled")] use crate::math::SIMD_WIDTH; @@ -220,7 +221,7 @@ impl AnyJointVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { match self { AnyJointVelocityConstraint::BallConstraint(c) => c.warmstart(mj_lambdas), AnyJointVelocityConstraint::BallGroundConstraint(c) => c.warmstart(mj_lambdas), @@ -254,7 +255,7 @@ impl AnyJointVelocityConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { match self { AnyJointVelocityConstraint::BallConstraint(c) => c.solve(mj_lambdas), AnyJointVelocityConstraint::BallGroundConstraint(c) => c.solve(mj_lambdas), diff --git a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs index 4773687..0ebe88e 100644 --- a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs @@ -7,9 +7,9 @@ use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint}; #[cfg(feature = "simd-is-enabled")] use super::{WBallPositionConstraint, WBallPositionGroundConstraint}; use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet}; -use crate::math::Isometry; #[cfg(feature = "simd-is-enabled")] use crate::math::SIMD_WIDTH; +use crate::math::{Isometry, Real}; pub(crate) enum AnyJointPositionConstraint { BallJoint(BallPositionConstraint), @@ -147,7 +147,7 @@ impl AnyJointPositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, position