From c32da78f2a6014c491aa3e975fb83ddb7c80610e Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 26 Apr 2021 17:59:25 +0200 Subject: Split rigid-bodies and colliders into multiple components --- .../joint_constraint/ball_position_constraint.rs | 56 ++-- .../ball_position_constraint_wide.rs | 83 +++--- .../joint_constraint/ball_velocity_constraint.rs | 114 ++++---- .../ball_velocity_constraint_wide.rs | 190 +++++++------ .../joint_constraint/fixed_position_constraint.rs | 48 ++-- .../fixed_position_constraint_wide.rs | 34 ++- .../joint_constraint/fixed_velocity_constraint.rs | 86 +++--- .../fixed_velocity_constraint_wide.rs | 196 +++++++------- .../generic_position_constraint.rs | 2 +- .../generic_position_constraint_wide.rs | 13 +- .../generic_velocity_constraint.rs | 16 +- .../generic_velocity_constraint_wide.rs | 154 +++++------ .../solver/joint_constraint/joint_constraint.rs | 139 +++++++--- .../joint_constraint/joint_position_constraint.rs | 99 ++++--- .../prismatic_position_constraint.rs | 42 +-- .../prismatic_position_constraint_wide.rs | 34 ++- .../prismatic_velocity_constraint.rs | 138 ++++++---- .../prismatic_velocity_constraint_wide.rs | 296 +++++++++++---------- .../revolute_position_constraint.rs | 60 +++-- .../revolute_position_constraint_wide.rs | 34 ++- .../revolute_velocity_constraint.rs | 140 ++++++---- .../revolute_velocity_constraint_wide.rs | 258 ++++++++++-------- 22 files changed, 1318 insertions(+), 914 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 93996f4..159cc77 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs @@ -1,4 +1,6 @@ -use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; +use crate::dynamics::{ + BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, +}; #[cfg(feature = "dim2")] use crate::math::SdpMatrix; use crate::math::{AngularInertia, Isometry, Point, Real, Rotation}; @@ -23,18 +25,25 @@ pub(crate) struct BallPositionConstraint { } impl BallPositionConstraint { - pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &BallJoint) -> Self { + pub fn from_params( + rb1: (&RigidBodyMassProps, &RigidBodyIds), + rb2: (&RigidBodyMassProps, &RigidBodyIds), + cparams: &BallJoint, + ) -> Self { + let (mprops1, ids1) = rb1; + let (mprops2, ids2) = rb2; + Self { - local_com1: rb1.mass_properties.local_com, - local_com2: rb2.mass_properties.local_com, - im1: rb1.effective_inv_mass, - im2: rb2.effective_inv_mass, - ii1: rb1.effective_world_inv_inertia_sqrt.squared(), - ii2: rb2.effective_world_inv_inertia_sqrt.squared(), + local_com1: mprops1.mass_properties.local_com, + local_com2: mprops2.mass_properties.local_com, + im1: mprops1.effective_inv_mass, + im2: mprops2.effective_inv_mass, + ii1: mprops1.effective_world_inv_inertia_sqrt.squared(), + ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), local_anchor1: cparams.local_anchor1, local_anchor2: cparams.local_anchor2, - position1: rb1.active_set_offset, - position2: rb2.active_set_offset, + position1: ids1.active_set_offset, + position2: ids2.active_set_offset, } } @@ -104,31 +113,34 @@ pub(crate) struct BallPositionGroundConstraint { impl BallPositionGroundConstraint { pub fn from_params( - rb1: &RigidBody, - rb2: &RigidBody, + rb1: &RigidBodyPosition, + rb2: (&RigidBodyMassProps, &RigidBodyIds), cparams: &BallJoint, flipped: bool, ) -> Self { + let poss1 = rb1; + let (mprops2, ids2) = rb2; + if flipped { // Note the only thing that is flipped here // are the local_anchors. The rb1 and rb2 have // already been flipped by the caller. Self { - anchor1: rb1.next_position * cparams.local_anchor2, - im2: rb2.effective_inv_mass, - ii2: rb2.effective_world_inv_inertia_sqrt.squared(), + anchor1: poss1.next_position * cparams.local_anchor2, + im2: mprops2.effective_inv_mass, + ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor1, - position2: rb2.active_set_offset, - local_com2: rb2.mass_properties.local_com, + position2: ids2.active_set_offset, + local_com2: mprops2.mass_properties.local_com, } } else { Self { - anchor1: rb1.next_position * cparams.local_anchor1, - im2: rb2.effective_inv_mass, - ii2: rb2.effective_world_inv_inertia_sqrt.squared(), + anchor1: poss1.next_position * cparams.local_anchor1, + im2: mprops2.effective_inv_mass, + ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor2, - position2: rb2.active_set_offset, - local_com2: rb2.mass_properties.local_com, + position2: ids2.active_set_offset, + local_com2: mprops2.mass_properties.local_com, } } } 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 e9162a4..6b00639 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs @@ -1,4 +1,6 @@ -use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; +use crate::dynamics::{ + BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, +}; #[cfg(feature = "dim2")] use crate::math::SdpMatrix; use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, SimdReal, SIMD_WIDTH}; @@ -25,26 +27,35 @@ pub(crate) struct WBallPositionConstraint { impl WBallPositionConstraint { pub fn from_params( - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&BallJoint; SIMD_WIDTH], ) -> 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 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii1 = AngularInertia::::from( - array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ) + let (mprops1, ids1) = rbs1; + let (mprops2, ids2) = rbs2; + + let local_com1 = Point::from(gather![|ii| mprops1[ii].mass_properties.local_com]); + let local_com2 = Point::from(gather![|ii| mprops2[ii].mass_properties.local_com]); + let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let ii1 = AngularInertia::::from(gather![ + |ii| mprops1[ii].effective_world_inv_inertia_sqrt + ]) .squared(); - let ii2 = AngularInertia::::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ) + let ii2 = AngularInertia::::from(gather![ + |ii| mprops2[ii].effective_world_inv_inertia_sqrt + ]) .squared(); - let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); - let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); - let position1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; - let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]); + let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]); + let position1 = gather![|ii| ids1[ii].active_set_offset]; + let position2 = gather![|ii| ids2[ii].active_set_offset]; Self { local_com1, @@ -61,8 +72,8 @@ impl WBallPositionConstraint { } 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]); + let mut position1 = Isometry::from(gather![|ii| positions[self.position1[ii]]]); + let mut position2 = Isometry::from(gather![|ii| positions[self.position2[ii]]]); let anchor1 = position1 * self.local_anchor1; let anchor2 = position2 * self.local_anchor2; @@ -129,30 +140,36 @@ pub(crate) struct WBallPositionGroundConstraint { impl WBallPositionGroundConstraint { pub fn from_params( - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: [&RigidBodyPosition; SIMD_WIDTH], + rbs2: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&BallJoint; SIMD_WIDTH], flipped: [bool; SIMD_WIDTH], ) -> Self { - let position1 = Isometry::from(array![|ii| rbs1[ii].next_position; SIMD_WIDTH]); + let poss1 = rbs1; + let (mprops2, ids2) = rbs2; + + let position1 = Isometry::from(gather![|ii| poss1[ii].next_position]); let anchor1 = position1 - * Point::from(array![|ii| if flipped[ii] { + * Point::from(gather![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 - }; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii2 = AngularInertia::::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ) + }]); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let ii2 = AngularInertia::::from(gather![ + |ii| mprops2[ii].effective_world_inv_inertia_sqrt + ]) .squared(); - let local_anchor2 = Point::from(array![|ii| if flipped[ii] { + let local_anchor2 = Point::from(gather![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 - }; SIMD_WIDTH]); - let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]); + }]); + let position2 = gather![|ii| ids2[ii].active_set_offset]; + let local_com2 = Point::from(gather![|ii| mprops2[ii].mass_properties.local_com]); Self { anchor1, @@ -165,7 +182,7 @@ impl WBallPositionGroundConstraint { } pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { - let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]); + let mut position2 = Isometry::from(gather![|ii| positions[self.position2[ii]]]); let anchor2 = position2 * self.local_anchor2; let com2 = position2 * self.local_com2; diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs index 508ac65..9e5227e 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs @@ -1,6 +1,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ - BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, + BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds, + RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, }; use crate::math::{AngVector, AngularInertia, Real, SdpMatrix, Vector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; @@ -36,19 +37,32 @@ impl BallVelocityConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: JointIndex, - rb1: &RigidBody, - rb2: &RigidBody, + rb1: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), + rb2: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), joint: &BallJoint, ) -> Self { - let anchor_world1 = rb1.position * joint.local_anchor1; - let anchor_world2 = rb2.position * joint.local_anchor2; - let anchor1 = anchor_world1 - rb1.world_com; - let anchor2 = anchor_world2 - rb2.world_com; + let (poss1, vels1, mprops1, ids1) = rb1; + let (poss2, vels2, mprops2, ids2) = rb2; - let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1); - let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2); - let im1 = rb1.effective_inv_mass; - let im2 = rb2.effective_inv_mass; + let anchor_world1 = poss1.position * joint.local_anchor1; + let anchor_world2 = poss2.position * joint.local_anchor2; + let anchor1 = anchor_world1 - mprops1.world_com; + let anchor2 = anchor_world2 - mprops2.world_com; + + let vel1 = vels1.linvel + vels1.angvel.gcross(anchor1); + let vel2 = vels2.linvel + vels2.angvel.gcross(anchor2); + let im1 = mprops1.effective_inv_mass; + let im2 = mprops2.effective_inv_mass; let rhs = (vel2 - vel1) * params.velocity_solve_fraction + (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt(); @@ -59,12 +73,12 @@ impl BallVelocityConstraint { #[cfg(feature = "dim3")] { - lhs = rb2 + lhs = mprops2 .effective_world_inv_inertia_sqrt .squared() .quadform(&cmat2) .add_diagonal(im2) - + rb1 + + mprops1 .effective_world_inv_inertia_sqrt .squared() .quadform(&cmat1) @@ -75,8 +89,8 @@ impl BallVelocityConstraint { // it's just easier that way. #[cfg(feature = "dim2")] { - let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2; let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2; let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2; @@ -100,8 +114,8 @@ impl BallVelocityConstraint { ); if stiffness != 0.0 { - let dpos = rb2.position.rotation - * (rb1.position.rotation * joint.motor_target_pos).inverse(); + let dpos = poss2.position.rotation + * (poss1.position.rotation * joint.motor_target_pos).inverse(); #[cfg(feature = "dim2")] { motor_rhs += dpos.angle() * stiffness; @@ -113,15 +127,15 @@ impl BallVelocityConstraint { } if damping != 0.0 { - let curr_vel = rb2.angvel - rb1.angvel; + let curr_vel = vels2.angvel - vels1.angvel; motor_rhs += (curr_vel - joint.motor_target_vel) * damping; } #[cfg(feature = "dim2")] if stiffness != 0.0 || damping != 0.0 { motor_inv_lhs = if keep_lhs { - let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); Some(gamma / (ii1 + ii2)) } else { Some(gamma) @@ -132,8 +146,8 @@ impl BallVelocityConstraint { #[cfg(feature = "dim3")] if stiffness != 0.0 || damping != 0.0 { motor_inv_lhs = if keep_lhs { - let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); Some((ii1 + ii2).inverse_unchecked() * gamma) } else { Some(SdpMatrix::diagonal(gamma)) @@ -151,8 +165,8 @@ impl BallVelocityConstraint { BallVelocityConstraint { joint_id, - mj_lambda1: rb1.active_set_offset, - mj_lambda2: rb2.active_set_offset, + mj_lambda1: ids1.active_set_offset, + mj_lambda2: ids2.active_set_offset, im1, im2, impulse: joint.impulse * params.warmstart_coeff, @@ -164,8 +178,8 @@ impl BallVelocityConstraint { motor_impulse, motor_inv_lhs, motor_max_impulse: joint.motor_max_impulse, - ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, - ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, + ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt, + ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt, } } @@ -269,29 +283,37 @@ impl BallVelocityGroundConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: JointIndex, - rb1: &RigidBody, - rb2: &RigidBody, + rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps), + rb2: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), joint: &BallJoint, flipped: bool, ) -> Self { + let (poss1, vels1, mprops1) = rb1; + let (poss2, vels2, mprops2, ids2) = rb2; + let (anchor_world1, anchor_world2) = if flipped { ( - rb1.position * joint.local_anchor2, - rb2.position * joint.local_anchor1, + poss1.position * joint.local_anchor2, + poss2.position * joint.local_anchor1, ) } else { ( - rb1.position * joint.local_anchor1, - rb2.position * joint.local_anchor2, + poss1.position * joint.local_anchor1, + poss2.position * joint.local_anchor2, ) }; - let anchor1 = anchor_world1 - rb1.world_com; - let anchor2 = anchor_world2 - rb2.world_com; + let anchor1 = anchor_world1 - mprops1.world_com; + let anchor2 = anchor_world2 - mprops2.world_com; - let im2 = rb2.effective_inv_mass; - let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1); - let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2); + let im2 = mprops2.effective_inv_mass; + let vel1 = vels1.linvel + vels1.angvel.gcross(anchor1); + let vel2 = vels2.linvel + vels2.angvel.gcross(anchor2); let rhs = (vel2 - vel1) * params.velocity_solve_fraction + (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt(); @@ -302,7 +324,7 @@ impl BallVelocityGroundConstraint { #[cfg(feature = "dim3")] { - lhs = rb2 + lhs = mprops2 .effective_world_inv_inertia_sqrt .squared() .quadform(&cmat2) @@ -311,7 +333,7 @@ impl BallVelocityGroundConstraint { #[cfg(feature = "dim2")] { - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); let m11 = im2 + cmat2.x * cmat2.x * ii2; let m12 = cmat2.x * cmat2.y * ii2; let m22 = im2 + cmat2.y * cmat2.y * ii2; @@ -335,8 +357,8 @@ impl BallVelocityGroundConstraint { ); if stiffness != 0.0 { - let dpos = rb2.position.rotation - * (rb1.position.rotation * joint.motor_target_pos).inverse(); + let dpos = poss2.position.rotation + * (poss1.position.rotation * joint.motor_target_pos).inverse(); #[cfg(feature = "dim2")] { motor_rhs += dpos.angle() * stiffness; @@ -348,14 +370,14 @@ impl BallVelocityGroundConstraint { } if damping != 0.0 { - let curr_vel = rb2.angvel - rb1.angvel; + let curr_vel = vels2.angvel - vels1.angvel; motor_rhs += (curr_vel - joint.motor_target_vel) * damping; } #[cfg(feature = "dim2")] if stiffness != 0.0 || damping != 0.0 { motor_inv_lhs = if keep_lhs { - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); Some(gamma / ii2) } else { Some(gamma) @@ -366,7 +388,7 @@ impl BallVelocityGroundConstraint { #[cfg(feature = "dim3")] if stiffness != 0.0 || damping != 0.0 { motor_inv_lhs = if keep_lhs { - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); Some(ii2.inverse_unchecked() * gamma) } else { Some(SdpMatrix::diagonal(gamma)) @@ -384,7 +406,7 @@ impl BallVelocityGroundConstraint { BallVelocityGroundConstraint { joint_id, - mj_lambda2: rb2.active_set_offset, + mj_lambda2: ids2.active_set_offset, im2, impulse: joint.impulse * params.warmstart_coeff, r2: anchor2, @@ -394,7 +416,7 @@ impl BallVelocityGroundConstraint { motor_impulse, motor_inv_lhs, motor_max_impulse: joint.motor_max_impulse, - ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, + ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt, } } 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 ff5e001..ee465cd 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs @@ -1,6 +1,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ - BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, + BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds, + RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, }; use crate::math::{ AngVector, AngularInertia, Isometry, Point, Real, SdpMatrix, SimdReal, Vector, SIMD_WIDTH, @@ -34,33 +35,46 @@ impl WBallVelocityConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&BallJoint; SIMD_WIDTH], ) -> 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 world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); - let ii1_sqrt = AngularInertia::::from( - array![|ii| rbs1[ii].effective_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 world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - - let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); - let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); - let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + let (poss1, vels1, mprops1, ids1) = rbs1; + let (poss2, vels2, mprops2, ids2) = rbs2; + + let position1 = Isometry::from(gather![|ii| poss1[ii].position]); + let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]); + let angvel1 = AngVector::::from(gather![|ii| vels1[ii].angvel]); + let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); + let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]); + let ii1_sqrt = AngularInertia::::from(gather![ + |ii| mprops1[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset]; + + let position2 = Isometry::from(gather![|ii| poss2[ii].position]); + let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]); + let angvel2 = AngVector::::from(gather![|ii| vels2[ii].angvel]); + let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let ii2_sqrt = AngularInertia::::from(gather![ + |ii| mprops2[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; + + let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]); + let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]); + let impulse = Vector::from(gather![|ii| cparams[ii].impulse]); let anchor_world1 = position1 * local_anchor1; let anchor_world2 = position2 * local_anchor2; @@ -114,20 +128,16 @@ impl WBallVelocityConstraint { 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], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular + ]), }; let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; mj_lambda1.linear += self.impulse * self.im1; @@ -147,20 +157,16 @@ impl WBallVelocityConstraint { 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], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular + ]), }; let mut mj_lambda2: DeltaVel = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); @@ -214,33 +220,49 @@ impl WBallVelocityGroundConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&BallJoint; SIMD_WIDTH], flipped: [bool; SIMD_WIDTH], ) -> 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 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], - ); - - 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 world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - - let local_anchor2 = Point::from( - array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH], - ); - let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + let (poss1, vels1, mprops1) = rbs1; + let (poss2, vels2, mprops2, ids2) = rbs2; + + let position1 = Isometry::from(gather![|ii| poss1[ii].position]); + let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]); + let angvel1 = AngVector::::from(gather![|ii| vels1[ii].angvel]); + let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); + let local_anchor1 = Point::from(gather![|ii| if flipped[ii] { + cparams[ii].local_anchor2 + } else { + cparams[ii].local_anchor1 + }]); + + let position2 = Isometry::from(gather![|ii| poss2[ii].position]); + let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]); + let angvel2 = AngVector::::from(gather![|ii| vels2[ii].angvel]); + let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let ii2_sqrt = AngularInertia::::from(gather![ + |ii| mprops2[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; + + let local_anchor2 = Point::from(gather![|ii| if flipped[ii] { + cparams[ii].local_anchor1 + } else { + cparams[ii].local_anchor2 + }]); + let impulse = Vector::from(gather![|ii| cparams[ii].impulse]); let anchor_world1 = position1 * local_anchor1; let anchor_world2 = position2 * local_anchor2; @@ -287,12 +309,10 @@ impl WBallVelocityGroundConstraint { 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], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; mj_lambda2.linear -= self.impulse * self.im2; @@ -306,12 +326,10 @@ impl WBallVelocityGroundConstraint { 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], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular); diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs index c98660f..239138f 100644 --- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs @@ -1,4 +1,6 @@ -use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody}; +use crate::dynamics::{ + FixedJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, +}; use crate::math::{AngularInertia, Isometry, Point, Real, Rotation}; use crate::utils::WAngularInertia; @@ -20,25 +22,32 @@ pub(crate) struct FixedPositionConstraint { } impl FixedPositionConstraint { - pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &FixedJoint) -> Self { - let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); - let im1 = rb1.effective_inv_mass; - let im2 = rb2.effective_inv_mass; + pub fn from_params( + rb1: (&RigidBodyMassProps, &RigidBodyIds), + rb2: (&RigidBodyMassProps, &RigidBodyIds), + cparams: &FixedJoint, + ) -> Self { + let (mprops1, ids1) = rb1; + let (mprops2, ids2) = rb2; + + let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); + let im1 = mprops1.effective_inv_mass; + let im2 = mprops2.effective_inv_mass; let lin_inv_lhs = 1.0 / (im1 + im2); let ang_inv_lhs = (ii1 + ii2).inverse(); Self { local_anchor1: cparams.local_anchor1, local_anchor2: cparams.local_anchor2, - position1: rb1.active_set_offset, - position2: rb2.active_set_offset, + position1: ids1.active_set_offset, + position2: ids2.active_set_offset, im1, im2, ii1, ii2, - local_com1: rb1.mass_properties.local_com, - local_com2: rb2.mass_properties.local_com, + local_com1: mprops1.mass_properties.local_com, + local_com2: mprops2.mass_properties.local_com, lin_inv_lhs, ang_inv_lhs, } @@ -91,29 +100,32 @@ pub(crate) struct FixedPositionGroundConstraint { impl FixedPositionGroundConstraint { pub fn from_params( - rb1: &RigidBody, - rb2: &RigidBody, + rb1: &RigidBodyPosition, + rb2: (&RigidBodyMassProps, &RigidBodyIds), cparams: &FixedJoint, flipped: bool, ) -> Self { + let poss1 = rb1; + let (mprops2, ids2) = rb2; + let anchor1; let local_anchor2; if flipped { - anchor1 = rb1.next_position * cparams.local_anchor2; + anchor1 = poss1.next_position * cparams.local_anchor2; local_anchor2 = cparams.local_anchor1; } else { - anchor1 = rb1.next_position * cparams.local_anchor1; + anchor1 = poss1.next_position * cparams.local_anchor1; local_anchor2 = cparams.local_anchor2; }; Self { anchor1, local_anchor2, - position2: rb2.active_set_offset, - im2: rb2.effective_inv_mass, - ii2: rb2.effective_world_inv_inertia_sqrt.squared(), - local_com2: rb2.mass_properties.local_com, + position2: ids2.active_set_offset, + im2: mprops2.effective_inv_mass, + ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), + local_com2: mprops2.mass_properties.local_com, impulse: 0.0, } } diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs index 2648816..0c0a6fd 100644 --- a/src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs @@ -1,5 +1,7 @@ use super::{FixedPositionConstraint, FixedPositionGroundConstraint}; -use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody}; +use crate::dynamics::{ + FixedJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, +}; use crate::math::{Isometry, Real, SIMD_WIDTH}; // TODO: this does not uses SIMD optimizations yet. @@ -10,12 +12,22 @@ pub(crate) struct WFixedPositionConstraint { impl WFixedPositionConstraint { pub fn from_params( - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&FixedJoint; SIMD_WIDTH], ) -> Self { Self { - constraints: array![|ii| FixedPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH], + constraints: gather![|ii| FixedPositionConstraint::from_params( + (rbs1.0[ii], rbs1.1[ii]), + (rbs2.0[ii], rbs2.1[ii]), + cparams[ii] + )], } } @@ -33,13 +45,21 @@ pub(crate) struct WFixedPositionGroundConstraint { impl WFixedPositionGroundConstraint { pub fn from_params( - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: [&RigidBodyPosition; SIMD_WIDTH], + rbs2: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&FixedJoint; SIMD_WIDTH], flipped: [bool; SIMD_WIDTH], ) -> Self { Self { - constraints: array![|ii| FixedPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH], + constraints: gather![|ii| FixedPositionGroundConstraint::from_params( + rbs1[ii], + (rbs2.0[ii], rbs2.1[ii]), + cparams[ii], + flipped[ii] + )], } } diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs index 11ade23..a0c0739 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs @@ -1,6 +1,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ - FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, + FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds, + RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, }; use crate::math::{AngularInertia, Real, SpacialVector, Vector, DIM}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; @@ -45,18 +46,31 @@ impl FixedVelocityConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: JointIndex, - rb1: &RigidBody, - rb2: &RigidBody, + rb1: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), + rb2: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), cparams: &FixedJoint, ) -> Self { - let anchor1 = rb1.position * cparams.local_anchor1; - let anchor2 = rb2.position * cparams.local_anchor2; - let im1 = rb1.effective_inv_mass; - let im2 = rb2.effective_inv_mass; - let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); - let r1 = anchor1.translation.vector - rb1.world_com.coords; - let r2 = anchor2.translation.vector - rb2.world_com.coords; + let (poss1, vels1, mprops1, ids1) = rb1; + let (poss2, vels2, mprops2, ids2) = rb2; + + let anchor1 = poss1.position * cparams.local_anchor1; + let anchor2 = poss2.position * cparams.local_anchor2; + let im1 = mprops1.effective_inv_mass; + let im2 = mprops2.effective_inv_mass; + let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); + let r1 = anchor1.translation.vector - mprops1.world_com.coords; + let r2 = anchor2.translation.vector - mprops2.world_com.coords; let rmat1 = r1.gcross_matrix(); let rmat2 = r2.gcross_matrix(); @@ -99,8 +113,9 @@ impl FixedVelocityConstraint { #[cfg(feature = "dim3")] let inv_lhs = lhs.cholesky().expect("Singular system.").inverse(); - let lin_dvel = -rb1.linvel - rb1.angvel.gcross(r1) + rb2.linvel + rb2.angvel.gcross(r2); - let ang_dvel = -rb1.angvel + rb2.angvel; + let lin_dvel = + -vels1.linvel - vels1.angvel.gcross(r1) + vels2.linvel + vels2.angvel.gcross(r2); + let ang_dvel = -vels1.angvel + vels2.angvel; #[cfg(feature = "dim2")] let mut rhs = @@ -133,14 +148,14 @@ impl FixedVelocityConstraint { FixedVelocityConstraint { joint_id, - mj_lambda1: rb1.active_set_offset, - mj_lambda2: rb2.active_set_offset, + mj_lambda1: ids1.active_set_offset, + mj_lambda2: ids2.active_set_offset, im1, im2, ii1, ii2, - ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, - ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, + ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt, + ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt, impulse: cparams.impulse * params.warmstart_coeff, inv_lhs, r1, @@ -250,28 +265,36 @@ impl FixedVelocityGroundConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: JointIndex, - rb1: &RigidBody, - rb2: &RigidBody, + rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps), + rb2: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), cparams: &FixedJoint, flipped: bool, ) -> Self { + let (poss1, vels1, mprops1) = rb1; + let (poss2, vels2, mprops2, ids2) = rb2; + let (anchor1, anchor2) = if flipped { ( - rb1.position * cparams.local_anchor2, - rb2.position * cparams.local_anchor1, + poss1.position * cparams.local_anchor2, + poss2.position * cparams.local_anchor1, ) } else { ( - rb1.position * cparams.local_anchor1, - rb2.position * cparams.local_anchor2, + poss1.position * cparams.local_anchor1, + poss2.position * cparams.local_anchor2, ) }; - let r1 = anchor1.translation.vector - rb1.world_com.coords; + let r1 = anchor1.translation.vector - mprops1.world_com.coords; - let im2 = rb2.effective_inv_mass; - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); - let r2 = anchor2.translation.vector - rb2.world_com.coords; + let im2 = mprops2.effective_inv_mass; + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); + let r2 = anchor2.translation.vector - mprops2.world_com.coords; let rmat2 = r2.gcross_matrix(); #[allow(unused_mut)] // For 2D. @@ -310,8 +333,9 @@ impl FixedVelocityGroundConstraint { #[cfg(feature = "dim3")] let inv_lhs = lhs.cholesky().expect("Singular system.").inverse(); - let lin_dvel = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1); - let ang_dvel = rb2.angvel - rb1.angvel; + let lin_dvel = + vels2.linvel + vels2.angvel.gcross(r2) - vels1.linvel - vels1.angvel.gcross(r1); + let ang_dvel = vels2.angvel - vels1.angvel; #[cfg(feature = "dim2")] let mut rhs = @@ -343,10 +367,10 @@ impl FixedVelocityGroundConstraint { FixedVelocityGroundConstraint { joint_id, - mj_lambda2: rb2.active_set_offset, + mj_lambda2: ids2.active_set_offset, im2, ii2, - ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, + ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt, impulse: cparams.impulse * params.warmstart_coeff, inv_lhs, r2, 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 b93bd3d..84e1fca 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs @@ -2,7 +2,8 @@ use simba::simd::SimdValue; use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ - FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, + FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds, + RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, }; use crate::math::{ AngVector, AngularInertia, CrossMatrix, Isometry, Point, Real, SimdReal, SpacialVector, Vector, @@ -53,33 +54,46 @@ impl WFixedVelocityConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&FixedJoint; SIMD_WIDTH], ) -> 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 world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); - let ii1_sqrt = AngularInertia::::from( - array![|ii| rbs1[ii].effective_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 world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - - let local_anchor1 = Isometry::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); - let local_anchor2 = Isometry::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); - let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + let (poss1, vels1, mprops1, ids1) = rbs1; + let (poss2, vels2, mprops2, ids2) = rbs2; + + let position1 = Isometry::from(gather![|ii| poss1[ii].position]); + let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]); + let angvel1 = AngVector::::from(gather![|ii| vels1[ii].angvel]); + let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); + let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]); + let ii1_sqrt = AngularInertia::::from(gather![ + |ii| mprops1[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset]; + + let position2 = Isometry::from(gather![|ii| poss2[ii].position]); + let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]); + let angvel2 = AngVector::::from(gather![|ii| vels2[ii].angvel]); + let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let ii2_sqrt = AngularInertia::::from(gather![ + |ii| mprops2[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; + + let local_anchor1 = Isometry::from(gather![|ii| cparams[ii].local_anchor1]); + let local_anchor2 = Isometry::from(gather![|ii| cparams[ii].local_anchor2]); + let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]); let anchor1 = position1 * local_anchor1; let anchor2 = position2 * local_anchor2; @@ -157,8 +171,7 @@ impl WFixedVelocityConstraint { #[cfg(feature = "dim3")] { - let ang_err = - Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]); + let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]); rhs += Vector6::new( lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z, ) * velocity_based_erp_inv_dt; @@ -185,20 +198,16 @@ impl WFixedVelocityConstraint { 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], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular + ]), }; let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); @@ -229,20 +238,16 @@ impl WFixedVelocityConstraint { 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], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular + ]), }; let mut mj_lambda2: DeltaVel = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); @@ -326,33 +331,49 @@ impl WFixedVelocityGroundConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&FixedJoint; SIMD_WIDTH], flipped: [bool; SIMD_WIDTH], ) -> 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 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 world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - - let local_anchor1 = Isometry::from( - array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH], - ); - let local_anchor2 = Isometry::from( - array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH], - ); - let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + let (poss1, vels1, mprops1) = rbs1; + let (poss2, vels2, mprops2, ids2) = rbs2; + + let position1 = Isometry::from(gather![|ii| poss1[ii].position]); + let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]); + let angvel1 = AngVector::::from(gather![|ii| vels1[ii].angvel]); + let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); + + let position2 = Isometry::from(gather![|ii| poss2[ii].position]); + let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]); + let angvel2 = AngVector::::from(gather![|ii| vels2[ii].angvel]); + let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let ii2_sqrt = AngularInertia::::from(gather![ + |ii| mprops2[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; + + let local_anchor1 = Isometry::from(gather![|ii| if flipped[ii] { + cparams[ii].local_anchor2 + } else { + cparams[ii].local_anchor1 + }]); + let local_anchor2 = Isometry::from(gather![|ii| if flipped[ii] { + cparams[ii].local_anchor1 + } else { + cparams[ii].local_anchor2 + }]); + let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]); let anchor1 = position1 * local_anchor1; let anchor2 = position2 * local_anchor2; @@ -423,8 +444,7 @@ impl WFixedVelocityGroundConstraint { #[cfg(feature = "dim3")] { - let ang_err = - Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]); + let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]); rhs += Vector6::new( lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z, ) * velocity_based_erp_inv_dt; @@ -446,12 +466,10 @@ impl WFixedVelocityGroundConstraint { 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], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); @@ -473,12 +491,10 @@ impl WFixedVelocityGroundConstraint { 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], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs index be12258..76901b1 100644 --- a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs @@ -1,6 +1,6 @@ use super::{GenericVelocityConstraint, GenericVelocityGroundConstraint}; use crate::dynamics::solver::DeltaVel; -use crate::dynamics::{GenericJoint, IntegrationParameters, RigidBody}; +use crate::dynamics::{GenericJoint, IntegrationParameters}; use crate::math::{ AngDim, AngVector, AngularInertia, Dim, Isometry, Point, Real, Rotation, SpatialVector, Vector, DIM, diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs index 9ceea67..d44c761 100644 --- a/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs @@ -15,7 +15,11 @@ impl WGenericPositionConstraint { cparams: [&GenericJoint; SIMD_WIDTH], ) -> Self { Self { - constraints: array![|ii| GenericPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH], + constraints: gather![|ii| GenericPositionConstraint::from_params( + rbs1[ii], + rbs2[ii], + cparams[ii] + )], } } @@ -39,7 +43,12 @@ impl WGenericPositionGroundConstraint { flipped: [bool; SIMD_WIDTH], ) -> Self { Self { - constraints: array![|ii| GenericPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[