diff options
Diffstat (limited to 'src/dynamics/solver/velocity_ground_constraint_wide.rs')
| -rw-r--r-- | src/dynamics/solver/velocity_ground_constraint_wide.rs | 130 |
1 files changed, 56 insertions, 74 deletions
diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index be01944..c2d2c4f 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_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,19 +10,19 @@ use simba::simd::{SimdPartialOrd, SimdValue}; #[derive(Copy, Clone, Debug)] pub(crate) struct WVelocityGroundConstraintElementPart { - pub gcross2: AngVector<SimdFloat>, - pub rhs: SimdFloat, - pub impulse: SimdFloat, - pub r: SimdFloat, + pub gcross2: AngVector<SimdReal>, + pub rhs: SimdReal, + pub impulse: SimdReal, + pub r: SimdReal, } impl WVelocityGroundConstraintElementPart { pub fn zero() -> Self { Self { gcross2: AngVector::zero(), - rhs: SimdFloat::zero(), - impulse: SimdFloat::zero(), - r: SimdFloat::zero(), + rhs: SimdReal::zero(), + impulse: SimdReal::zero(), + r: SimdReal::zero(), } } } @@ -45,11 +44,11 @@ impl WVelocityGroundConstraintElement { #[derive(Copy, Clone, Debug)] pub(crate) struct WVelocityGroundConstraint { - 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: [WVelocityGroundConstraintElement; MAX_MANIFOLD_POINTS], pub num_contacts: u8, - pub im2: SimdFloat, - pub limit: SimdFloat, + pub im2: SimdReal, + pub limit: SimdReal, pub mj_lambda2: [usize; SIMD_WIDTH], pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], pub manifold_contact_id: usize, @@ -64,9 +63,9 @@ impl WVelocityGroundConstraint { out_constraints: &mut Vec<AnyVelocityConstraint>, push: bool, ) { - let inv_dt = SimdFloat::splat(params.inv_dt()); - let mut rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH]; - let mut rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH]; + let inv_dt = SimdReal::splat(params.inv_dt()); + let mut rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH]; + let mut rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH]; let mut flipped = [false; SIMD_WIDTH]; for ii in 0..SIMD_WIDTH { @@ -76,57 +75,42 @@ impl WVelocityGroundConstraint { } } - 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 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 linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); - - let pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); - let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); - - let delta1 = Isometry::from( - array![|ii| if flipped[ii] { manifolds[ii].delta2 } else { manifolds[ii].delta1 }; SIMD_WIDTH], - ); - let delta2 = Isometry::from( - array![|ii| if flipped[ii] { manifolds[ii].delta1 } else { manifolds[ii].delta2 }; SIMD_WIDTH], - ); - - let coll_pos1 = pos1 * delta1; - let coll_pos2 = pos2 * delta2; + let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let force_dir1 = coll_pos1 - * -Vector::from( - array![|ii| if flipped[ii] { manifolds[ii].local_n2 } else { manifolds[ii].local_n1 }; SIMD_WIDTH], - ); + let force_dir1 = Vector::from( + array![|ii| if flipped[ii] { manifolds[ii].data.normal } else { -manifolds[ii].data.normal }; 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); + 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..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { - let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH]; + for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) { + let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..]; SIMD_WIDTH]; let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); let mut constraint = WVelocityGroundConstraint { dir1: force_dir1, elements: [WVelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], im2, - limit: friction, + limit: SimdReal::splat(0.0), mj_lambda2, manifold_id, manifold_contact_id: l, @@ -134,35 +118,33 @@ impl WVelocityGroundConstraint { }; for k in 0..num_points { - let p1 = coll_pos1 - * Point::from( - array![|ii| if flipped[ii] { manifold_points[ii][k].local_p2 } else { manifold_points[ii][k].local_p1 }; SIMD_WIDTH], - ); - let p2 = coll_pos2 - * Point::from( - array![|ii| if flipped[ii] { manifold_points[ii][k].local_p1 } else { 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]); - let dp1 = p1 - world_com1; - let dp2 = p2 - world_com2; + SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); + 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 gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); - let r = SimdFloat::splat(1.0) / (im2 + gcross2.gdot(gcross2)); + let r = SimdReal::splat(1.0) / (im2 + 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 = WVelocityGroundConstraintElementPart { gcross2, @@ -177,16 +159,16 @@ impl WVelocityGroundConstraint { 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 gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); - let r = SimdFloat::splat(1.0) / (im2 + gcross2.gdot(gcross2)); + let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2)); let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]); constraint.elements[k].tangent_parts[j] = @@ -202,13 +184,13 @@ impl WVelocityGroundConstraint { if push { out_constraints.push(AnyVelocityConstraint::GroupedGround(constraint)); } else { - out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = AnyVelocityConstraint::GroupedGround(constraint); } } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) { + 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], @@ -238,7 +220,7 @@ impl WVelocityGroundConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) { + pub fn solve(&mut 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], @@ -274,7 +256,7 @@ impl WVelocityGroundConstraint { let elt = &mut self.elements[i].normal_part; let dimpulse = -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; @@ -301,16 +283,16 @@ impl WVelocityGroundConstraint { 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]]; } } |