diff options
Diffstat (limited to 'src/dynamics/solver/velocity_constraint_wide.rs')
| -rw-r--r-- | src/dynamics/solver/velocity_constraint_wide.rs | 133 |
1 files changed, 63 insertions, 70 deletions
diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 5d8078a..a8384de 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -2,8 +2,7 @@ use super::{AnyVelocityConstraint, DeltaVel}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ - AngVector, AngularInertia, Isometry, Point, SimdFloat, Vector, DIM, MAX_MANIFOLD_POINTS, - SIMD_WIDTH, + AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; use num::Zero; @@ -11,11 +10,11 @@ use simba::simd::{SimdPartialOrd, SimdValue}; #[derive(Copy, Clone, Debug)] pub(crate) struct WVelocityConstraintElementPart { - pub gcross1: AngVector<SimdFloat>, - pub gcross2: AngVector<SimdFloat>, - pub rhs: SimdFloat, - pub impulse: SimdFloat, - pub r: SimdFloat, + pub gcross1: AngVector<SimdReal>, + pub gcross2: AngVector<SimdReal>, + pub rhs: SimdReal, + pub impulse: SimdReal, + pub r: SimdReal, } impl WVelocityConstraintElementPart { @@ -23,9 +22,9 @@ impl WVelocityConstraintElementPart { Self { gcross1: AngVector::zero(), gcross2: AngVector::zero(), - rhs: SimdFloat::zero(), - impulse: SimdFloat::zero(), - r: SimdFloat::zero(), + rhs: SimdReal::zero(), + impulse: SimdReal::zero(), + r: SimdReal::zero(), } } } @@ -47,12 +46,12 @@ impl WVelocityConstraintElement { #[derive(Copy, Clone, Debug)] pub(crate) struct WVelocityConstraint { - pub dir1: Vector<SimdFloat>, // Non-penetration force direction for the first body. + pub dir1: Vector<SimdReal>, // Non-penetration force direction for the first body. pub elements: [WVelocityConstraintElement; MAX_MANIFOLD_POINTS], pub num_contacts: u8, - pub im1: SimdFloat, - pub im2: SimdFloat, - pub limit: SimdFloat, + pub im1: SimdReal, + pub im2: SimdReal, + pub limit: SimdReal, pub mj_lambda1: [usize; SIMD_WIDTH], pub mj_lambda2: [usize; SIMD_WIDTH], pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], @@ -68,52 +67,46 @@ impl WVelocityConstraint { out_constraints: &mut Vec<AnyVelocityConstraint>, push: bool, ) { - let inv_dt = SimdFloat::splat(params.inv_dt()); - let rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH]; - let rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH]; + let inv_dt = SimdReal::splat(params.inv_dt()); + let rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH]; + let rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH]; - let delta1 = Isometry::from(array![|ii| manifolds[ii].delta1; SIMD_WIDTH]); - let delta2 = Isometry::from(array![|ii| manifolds[ii].delta2; SIMD_WIDTH]); - - let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii1: AngularInertia<SimdFloat> = - AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); + let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); + let ii1: AngularInertia<SimdReal> = AngularInertia::from( + array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], + ); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); - let pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2: AngularInertia<SimdFloat> = - AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); + let ii2: AngularInertia<SimdReal> = AngularInertia::from( + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], + ); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); - let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let coll_pos1 = pos1 * delta1; - let coll_pos2 = pos2 * delta2; - - let force_dir1 = coll_pos1 * -Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]); + let force_dir1 = -Vector::from(array![|ii| manifolds[ii].data.normal; SIMD_WIDTH]); let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let friction = SimdFloat::from(array![|ii| manifolds[ii].friction; SIMD_WIDTH]); - let restitution = SimdFloat::from(array![|ii| manifolds[ii].restitution; SIMD_WIDTH]); - let restitution_velocity_threshold = - SimdFloat::splat(params.restitution_velocity_threshold); + let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold); let warmstart_multiplier = - SimdFloat::from(array![|ii| manifolds[ii].warmstart_multiplier; SIMD_WIDTH]); - let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff); - - for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { - let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH]; + SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); + let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff); + let num_active_contacts = manifolds[0].data.num_active_contacts(); + + for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) { + let manifold_points = array![|ii| + &manifolds[ii].data.solver_contacts[l..num_active_contacts]; SIMD_WIDTH + ]; let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); let mut constraint = WVelocityConstraint { @@ -121,7 +114,7 @@ impl WVelocityConstraint { elements: [WVelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], im1, im2, - limit: friction, + limit: SimdReal::splat(0.0), mj_lambda1, mj_lambda2, manifold_id, @@ -130,36 +123,36 @@ impl WVelocityConstraint { }; for k in 0..num_points { - // FIXME: can we avoid the multiplications by coll_pos1/coll_pos2 here? - // By working as much as possible in local-space. - let p1 = coll_pos1 - * Point::from(array![|ii| manifold_points[ii][k].local_p1; SIMD_WIDTH]); - let p2 = coll_pos2 - * Point::from(array![|ii| manifold_points[ii][k].local_p2; SIMD_WIDTH]); - - let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); + let friction = + SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]); + let restitution = + SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]); + let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]); + let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); let impulse = - SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]); + SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); - let dp1 = p1 - world_com1; - let dp2 = p2 - world_com2; + let dp1 = point - world_com1; + let dp2 = point - world_com2; let vel1 = linvel1 + angvel1.gcross(dp1); let vel2 = linvel2 + angvel2.gcross(dp2); + constraint.limit = friction; + // Normal part. { let gcross1 = ii1.transform_vector(dp1.gcross(force_dir1)); let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); - let r = SimdFloat::splat(1.0) + let r = SimdReal::splat(1.0) / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); let mut rhs = (vel1 - vel2).dot(&force_dir1); let use_restitution = rhs.simd_le(-restitution_velocity_threshold); let rhs_with_restitution = rhs + rhs * restitution; rhs = rhs_with_restitution.select(use_restitution, rhs); - rhs += dist.simd_max(SimdFloat::zero()) * inv_dt; + rhs += dist.simd_max(SimdReal::zero()) * inv_dt; constraint.elements[k].normal_part = WVelocityConstraintElementPart { gcross1, @@ -175,17 +168,17 @@ impl WVelocityConstraint { for j in 0..DIM - 1 { #[cfg(feature = "dim2")] - let impulse = SimdFloat::from( - array![|ii| manifold_points[ii][k].tangent_impulse; SIMD_WIDTH], + let impulse = SimdReal::from( + array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH], ); #[cfg(feature = "dim3")] - let impulse = SimdFloat::from( - array![|ii| manifold_points[ii][k].tangent_impulse[j]; SIMD_WIDTH], + let impulse = SimdReal::from( + array![|ii| manifold_points[ii][k].data.tangent_impulse[j]; SIMD_WIDTH], ); let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j])); let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); - let r = SimdFloat::splat(1.0) + let r = SimdReal::splat(1.0) / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); let rhs = (vel1 - vel2).dot(&tangents1[j]); @@ -202,13 +195,13 @@ impl WVelocityConstraint { if push { out_constraints.push(AnyVelocityConstraint::Grouped(constraint)); } else { - out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = AnyVelocityConstraint::Grouped(constraint); } } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) { + 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], @@ -258,7 +251,7 @@ impl WVelocityConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) { + pub fn solve(&mut 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], @@ -309,7 +302,7 @@ impl WVelocityConstraint { - self.dir1.dot(&mj_lambda2.linear) + elt.gcross2.gdot(mj_lambda2.angular) + elt.rhs; - let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdFloat::zero()); + let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdReal::zero()); let dlambda = new_impulse - elt.impulse; elt.impulse = new_impulse; @@ -341,16 +334,16 @@ impl WVelocityConstraint { for ii in 0..SIMD_WIDTH { let manifold = &mut manifolds_all[self.manifold_id[ii]]; let k_base = self.manifold_contact_id; - let active_contacts = manifold.active_contacts_mut(); - active_contacts[k_base + k].impulse = impulses[ii]; + let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()]; + active_contacts[k_base + k].data.impulse = impulses[ii]; #[cfg(feature = "dim2")] { - active_contacts[k_base + k].tangent_impulse = tangent_impulses[ii]; + active_contacts[k_base + k].data.tangent_impulse = tangent_impulses[ii]; } #[cfg(feature = "dim3")] { - active_contacts[k_base + k].tangent_impulse = + active_contacts[k_base + k].data.tangent_impulse = [tangent_impulses[ii], bitangent_impulses[ii]]; } } |