From 9bf1321f8f1d2e116f44c2461a53f302c4ef4171 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 8 Dec 2020 17:31:49 +0100 Subject: Outsource the contact manifold, SAT, and some shapes. --- src/dynamics/solver/velocity_ground_constraint.rs | 34 +++++++++++------------ 1 file changed, 17 insertions(+), 17 deletions(-) (limited to 'src/dynamics/solver/velocity_ground_constraint.rs') diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index d8ef8be..c3a3356 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -63,26 +63,26 @@ impl VelocityGroundConstraint { out_constraints: &mut Vec, push: bool, ) { - let mut rb1 = &bodies[manifold.body_pair.body1]; - let mut rb2 = &bodies[manifold.body_pair.body2]; + let mut rb1 = &bodies[manifold.data.body_pair.body1]; + let mut rb2 = &bodies[manifold.data.body_pair.body2]; let flipped = !rb2.is_dynamic(); let force_dir1; let coll_pos1; let coll_pos2; if flipped { - coll_pos1 = rb2.position * manifold.delta2; - coll_pos2 = rb1.position * manifold.delta1; + coll_pos1 = rb2.position * manifold.data.delta2; + coll_pos2 = rb1.position * manifold.data.delta1; force_dir1 = coll_pos1 * (-manifold.local_n2); std::mem::swap(&mut rb1, &mut rb2); } else { - coll_pos1 = rb1.position * manifold.delta1; - coll_pos2 = rb2.position * manifold.delta2; + coll_pos1 = rb1.position * manifold.data.delta1; + coll_pos2 = rb2.position * manifold.data.delta2; force_dir1 = coll_pos1 * (-manifold.local_n1); } let mj_lambda2 = rb2.active_set_offset; - let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff; + let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; for (l, manifold_points) in manifold .active_contacts() @@ -94,7 +94,7 @@ impl VelocityGroundConstraint { dir1: force_dir1, elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], im2: rb2.mass_properties.inv_mass, - limit: manifold.friction, + limit: manifold.data.friction, mj_lambda2, manifold_id, manifold_contact_id: l * MAX_MANIFOLD_POINTS, @@ -135,7 +135,7 @@ impl VelocityGroundConstraint { { constraint.dir1 = force_dir1; constraint.im2 = rb2.mass_properties.inv_mass; - constraint.limit = manifold.friction; + constraint.limit = manifold.data.friction; constraint.mj_lambda2 = mj_lambda2; constraint.manifold_id = manifold_id; constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS; @@ -173,12 +173,12 @@ impl VelocityGroundConstraint { let mut rhs = (vel1 - vel2).dot(&force_dir1); if rhs <= -params.restitution_velocity_threshold { - rhs += manifold.restitution * rhs + rhs += manifold.data.restitution * rhs } rhs += manifold_point.dist.max(0.0) * params.inv_dt(); - let impulse = manifold_points[k].impulse * warmstart_coeff; + let impulse = manifold_points[k].data.impulse * warmstart_coeff; constraint.elements[k].normal_part = VelocityGroundConstraintElementPart { gcross2, @@ -199,9 +199,9 @@ impl VelocityGroundConstraint { let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2)); let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]); #[cfg(feature = "dim2")] - let impulse = manifold_points[k].tangent_impulse * warmstart_coeff; + let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff; #[cfg(feature = "dim3")] - let impulse = manifold_points[k].tangent_impulse[j] * warmstart_coeff; + let impulse = manifold_points[k].data.tangent_impulse[j] * warmstart_coeff; constraint.elements[k].tangent_part[j] = VelocityGroundConstraintElementPart { @@ -218,7 +218,7 @@ impl VelocityGroundConstraint { if push { out_constraints.push(AnyVelocityConstraint::NongroupedGround(constraint)); } else { - out_constraints[manifold.constraint_index + l] = + out_constraints[manifold.data.constraint_index + l] = AnyVelocityConstraint::NongroupedGround(constraint); } } @@ -290,15 +290,15 @@ impl VelocityGroundConstraint { for k in 0..self.num_contacts as usize { let active_contacts = manifold.active_contacts_mut(); - active_contacts[k_base + k].impulse = self.elements[k].normal_part.impulse; + active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse; #[cfg(feature = "dim2")] { - active_contacts[k_base + k].tangent_impulse = + active_contacts[k_base + k].data.tangent_impulse = self.elements[k].tangent_part[0].impulse; } #[cfg(feature = "dim3")] { - active_contacts[k_base + k].tangent_impulse = [ + active_contacts[k_base + k].data.tangent_impulse = [ self.elements[k].tangent_part[0].impulse, self.elements[k].tangent_part[1].impulse, ]; -- cgit From 43628c8846c8805d2f835dda4182b7240292900c Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Wed, 30 Dec 2020 17:30:07 +0100 Subject: Try using solver contacts again, but in a more cache-coherent way. --- src/dynamics/solver/velocity_ground_constraint.rs | 47 +++++++---------------- 1 file changed, 13 insertions(+), 34 deletions(-) (limited to 'src/dynamics/solver/velocity_ground_constraint.rs') diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index c3a3356..cbb6bb8 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -66,35 +66,25 @@ impl VelocityGroundConstraint { let mut rb1 = &bodies[manifold.data.body_pair.body1]; let mut rb2 = &bodies[manifold.data.body_pair.body2]; let flipped = !rb2.is_dynamic(); - let force_dir1; - let coll_pos1; - let coll_pos2; - - if flipped { - coll_pos1 = rb2.position * manifold.data.delta2; - coll_pos2 = rb1.position * manifold.data.delta1; - force_dir1 = coll_pos1 * (-manifold.local_n2); + + let force_dir1 = if flipped { std::mem::swap(&mut rb1, &mut rb2); + manifold.data.normal } else { - coll_pos1 = rb1.position * manifold.data.delta1; - coll_pos2 = rb2.position * manifold.data.delta2; - force_dir1 = coll_pos1 * (-manifold.local_n1); - } + -manifold.data.normal + }; let mj_lambda2 = rb2.active_set_offset; let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; + let active_contacts = &manifold.data.solver_contacts[..manifold.num_active_contacts]; - for (l, manifold_points) in manifold - .active_contacts() - .chunks(MAX_MANIFOLD_POINTS) - .enumerate() - { + for (l, manifold_points) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() { #[cfg(not(target_arch = "wasm32"))] let mut constraint = VelocityGroundConstraint { dir1: force_dir1, elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], im2: rb2.mass_properties.inv_mass, - limit: manifold.data.friction, + limit: 0.0, mj_lambda2, manifold_id, manifold_contact_id: l * MAX_MANIFOLD_POINTS, @@ -144,24 +134,13 @@ impl VelocityGroundConstraint { for k in 0..manifold_points.len() { let manifold_point = &manifold_points[k]; - let (p1, p2) = if flipped { - // NOTE: we already swapped rb1 and rb2 - // so we multiply by coll_pos1/coll_pos2. - ( - coll_pos1 * manifold_point.local_p2, - coll_pos2 * manifold_point.local_p1, - ) - } else { - ( - coll_pos1 * manifold_point.local_p1, - coll_pos2 * manifold_point.local_p2, - ) - }; - let dp2 = p2 - rb2.world_com; - let dp1 = p1 - rb1.world_com; + let dp2 = manifold_point.point - rb2.world_com; + let dp1 = manifold_point.point - rb1.world_com; let vel1 = rb1.linvel + rb1.angvel.gcross(dp1); let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); + constraint.limit = manifold_point.friction; + // Normal part. { let gcross2 = rb2 @@ -173,7 +152,7 @@ impl VelocityGroundConstraint { let mut rhs = (vel1 - vel2).dot(&force_dir1); if rhs <= -params.restitution_velocity_threshold { - rhs += manifold.data.restitution * rhs + rhs += manifold_point.restitution * rhs } rhs += manifold_point.dist.max(0.0) * params.inv_dt(); -- cgit From 967145a9492175be59e8db33299b1687d69d84e2 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Thu, 31 Dec 2020 11:16:03 +0100 Subject: Perform contact sorting in the narrow-phase directly. --- src/dynamics/solver/velocity_ground_constraint.rs | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) (limited to 'src/dynamics/solver/velocity_ground_constraint.rs') diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index cbb6bb8..fa2a23e 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -76,9 +76,13 @@ impl VelocityGroundConstraint { let mj_lambda2 = rb2.active_set_offset; let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; - let active_contacts = &manifold.data.solver_contacts[..manifold.num_active_contacts]; - for (l, manifold_points) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() { + for (l, manifold_points) in manifold + .data + .solver_contacts + .chunks(MAX_MANIFOLD_POINTS) + .enumerate() + { #[cfg(not(target_arch = "wasm32"))] let mut constraint = VelocityGroundConstraint { dir1: force_dir1, @@ -268,7 +272,7 @@ impl VelocityGroundConstraint { let k_base = self.manifold_contact_id; for k in 0..self.num_contacts as usize { - let active_contacts = manifold.active_contacts_mut(); + let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()]; active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse; #[cfg(feature = "dim2")] { -- 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. --- src/dynamics/solver/velocity_ground_constraint.rs | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) (limited to 'src/dynamics/solver/velocity_ground_constraint.rs') diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index fa2a23e..37d4e3a 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -1,5 +1,5 @@ use super::{AnyVelocityConstraint, DeltaVel}; -use crate::math::{AngVector, Vector, DIM, MAX_MANIFOLD_POINTS}; +use crate::math::{AngVector, Real, Vector, DIM, MAX_MANIFOLD_POINTS}; use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; @@ -8,10 +8,10 @@ use simba::simd::SimdPartialOrd; #[derive(Copy, Clone, Debug)] pub(crate) struct VelocityGroundConstraintElementPart { - pub gcross2: AngVector, - pub rhs: f32, - pub impulse: f32, - pub r: f32, + pub gcross2: AngVector, + pub rhs: Real, + pub impulse: Real, + pub r: Real, } #[cfg(not(target_arch = "wasm32"))] @@ -44,9 +44,9 @@ impl VelocityGroundConstraintElement { #[derive(Copy, Clone, Debug)] pub(crate) struct VelocityGroundConstraint { - pub dir1: Vector, // Non-penetration force direction for the first body. - pub im2: f32, - pub limit: f32, + pub dir1: Vector, // Non-penetration force direction for the first body. + pub im2: Real, + pub limit: Real, pub mj_lambda2: usize, pub manifold_id: ContactManifoldIndex, pub manifold_contact_id: usize, @@ -207,7 +207,7 @@ impl VelocityGroundConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel::zero(); let tangents1 = self.dir1.orthonormal_basis(); @@ -227,7 +227,7 @@ impl VelocityGroundConstraint { mj_lambdas[self.mj_lambda2 as usize].angular += mj_lambda2.angular; } - 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]; // Solve friction. -- cgit From 8f330b2a00610e5b68c1acd9208120e8f750c7aa Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Thu, 21 Jan 2021 14:58:40 +0100 Subject: Rotation locking: apply filter only to the world inertia properties to fix the multi-collider case. --- src/dynamics/solver/velocity_ground_constraint.rs | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'src/dynamics/solver/velocity_ground_constraint.rs') diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 37d4e3a..7ea4fbb 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -87,7 +87,7 @@ impl VelocityGroundConstraint { let mut constraint = VelocityGroundConstraint { dir1: force_dir1, elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], - im2: rb2.mass_properties.inv_mass, + im2: rb2.effective_inv_mass, limit: 0.0, mj_lambda2, manifold_id, @@ -128,7 +128,7 @@ impl VelocityGroundConstraint { #[cfg(target_arch = "wasm32")] { constraint.dir1 = force_dir1; - constraint.im2 = rb2.mass_properties.inv_mass; + constraint.im2 = rb2.effective_inv_mass; constraint.limit = manifold.data.friction; constraint.mj_lambda2 = mj_lambda2; constraint.manifold_id = manifold_id; @@ -148,10 +148,10 @@ impl VelocityGroundConstraint { // Normal part. { let gcross2 = rb2 - .world_inv_inertia_sqrt + .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-force_dir1)); - let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2)); + let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2)); let mut rhs = (vel1 - vel2).dot(&force_dir1); @@ -177,9 +177,9 @@ impl VelocityGroundConstraint { for j in 0..DIM - 1 { let gcross2 = rb2 - .world_inv_inertia_sqrt + .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-tangents1[j])); - let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2)); + let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2)); let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]); #[cfg(feature = "dim2")] let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff; -- cgit From e45e5f109c7527a49af2a3fe4404e0a5efd92f2f Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Sat, 23 Jan 2021 13:11:00 +0100 Subject: Fix WASM build. --- src/dynamics/solver/velocity_ground_constraint.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/dynamics/solver/velocity_ground_constraint.rs') diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 49bb465..20642ec 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -130,7 +130,7 @@ impl VelocityGroundConstraint { { constraint.dir1 = force_dir1; constraint.im2 = rb2.effective_inv_mass; - constraint.limit = manifold.data.friction; + constraint.limit = 0.0; constraint.mj_lambda2 = mj_lambda2; constraint.manifold_id = manifold_id; constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS; -- cgit