diff options
Diffstat (limited to 'src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs')
| -rw-r--r-- | src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs | 190 |
1 files changed, 104 insertions, 86 deletions
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::<SimdReal>::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::<SimdReal>::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::<SimdReal>::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::<SimdReal>::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::<SimdReal>::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::<SimdReal>::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::<SimdReal>::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::<SimdReal>::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<Real>]) { 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<Real>]) { let mut mj_lambda1: DeltaVel<SimdReal> = 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<SimdReal> = 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::<SimdReal>::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::<SimdReal>::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::<SimdReal>::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::<SimdReal>::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::<SimdReal>::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::<SimdReal>::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<Real>]) { 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<Real>]) { let mut mj_lambda2: DeltaVel<SimdReal> = 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); |