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_constraint.rs | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) (limited to 'src/dynamics/solver/velocity_constraint.rs') diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 948081d..1bc4c0e 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -144,14 +144,14 @@ impl VelocityConstraint { out_constraints: &mut Vec, push: bool, ) { - let rb1 = &bodies[manifold.body_pair.body1]; - let rb2 = &bodies[manifold.body_pair.body2]; + let rb1 = &bodies[manifold.data.body_pair.body1]; + let rb2 = &bodies[manifold.data.body_pair.body2]; let mj_lambda1 = rb1.active_set_offset; let mj_lambda2 = rb2.active_set_offset; - let pos_coll1 = rb1.position * manifold.delta1; - let pos_coll2 = rb2.position * manifold.delta2; + let pos_coll1 = rb1.position * manifold.data.delta1; + let pos_coll2 = rb2.position * manifold.data.delta2; let force_dir1 = pos_coll1 * (-manifold.local_n1); - 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() @@ -164,7 +164,7 @@ impl VelocityConstraint { elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], im1: rb1.mass_properties.inv_mass, im2: rb2.mass_properties.inv_mass, - limit: manifold.friction, + limit: manifold.data.friction, mj_lambda1, mj_lambda2, manifold_id, @@ -207,7 +207,7 @@ impl VelocityConstraint { constraint.dir1 = force_dir1; constraint.im1 = rb1.mass_properties.inv_mass; constraint.im2 = rb2.mass_properties.inv_mass; - constraint.limit = manifold.friction; + constraint.limit = manifold.data.friction; constraint.mj_lambda1 = mj_lambda1; constraint.mj_lambda2 = mj_lambda2; constraint.manifold_id = manifold_id; @@ -241,12 +241,12 @@ impl VelocityConstraint { 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 = VelocityConstraintElementPart { gcross1, @@ -275,9 +275,9 @@ impl VelocityConstraint { + gcross2.gdot(gcross2)); let rhs = (vel1 - vel2).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] = VelocityConstraintElementPart { gcross1, @@ -294,7 +294,7 @@ impl VelocityConstraint { if push { out_constraints.push(AnyVelocityConstraint::Nongrouped(constraint)); } else { - out_constraints[manifold.constraint_index + l] = + out_constraints[manifold.data.constraint_index + l] = AnyVelocityConstraint::Nongrouped(constraint); } } @@ -388,15 +388,15 @@ impl VelocityConstraint { 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_constraint.rs | 26 +++++++++++--------------- 1 file changed, 11 insertions(+), 15 deletions(-) (limited to 'src/dynamics/solver/velocity_constraint.rs') diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 1bc4c0e..4f94fba 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -148,23 +148,18 @@ impl VelocityConstraint { let rb2 = &bodies[manifold.data.body_pair.body2]; let mj_lambda1 = rb1.active_set_offset; let mj_lambda2 = rb2.active_set_offset; - let pos_coll1 = rb1.position * manifold.data.delta1; - let pos_coll2 = rb2.position * manifold.data.delta2; - let force_dir1 = pos_coll1 * (-manifold.local_n1); + let force_dir1 = -manifold.data.normal; 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 = VelocityConstraint { dir1: force_dir1, elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], im1: rb1.mass_properties.inv_mass, im2: rb2.mass_properties.inv_mass, - limit: manifold.data.friction, + limit: 0.0, mj_lambda1, mj_lambda2, manifold_id, @@ -217,12 +212,13 @@ impl VelocityConstraint { for k in 0..manifold_points.len() { let manifold_point = &manifold_points[k]; - let dp1 = (pos_coll1 * manifold_point.local_p1) - rb1.world_com; - let dp2 = (pos_coll2 * manifold_point.local_p2) - rb2.world_com; + let dp1 = manifold_point.point - rb1.world_com; + let dp2 = manifold_point.point - rb2.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 gcross1 = rb1 @@ -241,12 +237,12 @@ impl VelocityConstraint { 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(); - let impulse = manifold_points[k].data.impulse * warmstart_coeff; + let impulse = manifold_point.data.impulse * warmstart_coeff; constraint.elements[k].normal_part = VelocityConstraintElementPart { gcross1, @@ -275,9 +271,9 @@ impl VelocityConstraint { + gcross2.gdot(gcross2)); let rhs = (vel1 - vel2).dot(&tangents1[j]); #[cfg(feature = "dim2")] - let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff; + let impulse = manifold_point.data.tangent_impulse * warmstart_coeff; #[cfg(feature = "dim3")] - let impulse = manifold_points[k].data.tangent_impulse[j] * warmstart_coeff; + let impulse = manifold_point.data.tangent_impulse[j] * warmstart_coeff; constraint.elements[k].tangent_part[j] = VelocityConstraintElementPart { gcross1, -- 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_constraint.rs | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) (limited to 'src/dynamics/solver/velocity_constraint.rs') diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 4f94fba..6614b48 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -150,9 +150,13 @@ impl VelocityConstraint { let mj_lambda2 = rb2.active_set_offset; let force_dir1 = -manifold.data.normal; 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 = VelocityConstraint { dir1: force_dir1, @@ -383,7 +387,7 @@ impl VelocityConstraint { 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_constraint.rs | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) (limited to 'src/dynamics/solver/velocity_constraint.rs') diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 6614b48..2cb3975 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -4,7 +4,7 @@ use crate::dynamics::solver::VelocityGroundConstraint; use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; -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 simba::simd::SimdPartialOrd; @@ -40,7 +40,7 @@ impl AnyVelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { match self { AnyVelocityConstraint::NongroupedGround(c) => c.warmstart(mj_lambdas), AnyVelocityConstraint::Nongrouped(c) => c.warmstart(mj_lambdas), @@ -52,7 +52,7 @@ impl AnyVelocityConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { match self { AnyVelocityConstraint::NongroupedGround(c) => c.solve(mj_lambdas), AnyVelocityConstraint::Nongrouped(c) => c.solve(mj_lambdas), @@ -79,11 +79,11 @@ impl AnyVelocityConstraint { #[derive(Copy, Clone, Debug)] pub(crate) struct VelocityConstraintElementPart { - pub gcross1: AngVector, - pub gcross2: AngVector, - pub rhs: f32, - pub impulse: f32, - pub r: f32, + pub gcross1: AngVector, + pub gcross2: AngVector, + pub rhs: Real, + pub impulse: Real, + pub r: Real, } #[cfg(not(target_arch = "wasm32"))] @@ -117,10 +117,10 @@ impl VelocityConstraintElement { #[derive(Copy, Clone, Debug)] pub(crate) struct VelocityConstraint { - pub dir1: Vector, // Non-penetration force direction for the first body. - pub im1: f32, - pub im2: f32, - pub limit: f32, + pub dir1: Vector, // Non-penetration force direction for the first body. + pub im1: Real, + pub im2: Real, + pub limit: Real, pub mj_lambda1: usize, pub mj_lambda2: usize, pub manifold_id: ContactManifoldIndex, @@ -300,7 +300,7 @@ impl VelocityConstraint { } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel::zero(); let mut mj_lambda2 = DeltaVel::zero(); @@ -331,7 +331,7 @@ impl VelocityConstraint { 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_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; -- cgit From c28b14d31c43e1eb97a81df7673127d0c22d8deb Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 4 Jan 2021 17:59:51 +0100 Subject: Refactor the parallel solver code the same way we did with the non-parallel solver. --- src/dynamics/solver/velocity_constraint.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/dynamics/solver/velocity_constraint.rs') diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 2cb3975..2686c4d 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -132,8 +132,8 @@ pub(crate) struct VelocityConstraint { impl VelocityConstraint { #[cfg(feature = "parallel")] pub fn num_active_constraints(manifold: &ContactManifold) -> usize { - let rest = manifold.num_active_contacts() % MAX_MANIFOLD_POINTS != 0; - manifold.num_active_contacts() / MAX_MANIFOLD_POINTS + rest as usize + let rest = manifold.data.solver_contacts.len() % MAX_MANIFOLD_POINTS != 0; + manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS + rest as usize } pub fn generate( -- 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_constraint.rs | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) (limited to 'src/dynamics/solver/velocity_constraint.rs') diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 2686c4d..c2aa6f5 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -161,8 +161,8 @@ impl VelocityConstraint { let mut constraint = VelocityConstraint { dir1: force_dir1, elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], - im1: rb1.mass_properties.inv_mass, - im2: rb2.mass_properties.inv_mass, + im1: rb1.effective_inv_mass, + im2: rb2.effective_inv_mass, limit: 0.0, mj_lambda1, mj_lambda2, @@ -204,8 +204,8 @@ impl VelocityConstraint { #[cfg(target_arch = "wasm32")] { constraint.dir1 = force_dir1; - constraint.im1 = rb1.mass_properties.inv_mass; - constraint.im2 = rb2.mass_properties.inv_mass; + constraint.im1 = rb1.effective_inv_mass; + constraint.im2 = rb2.effective_inv_mass; constraint.limit = manifold.data.friction; constraint.mj_lambda1 = mj_lambda1; constraint.mj_lambda2 = mj_lambda2; @@ -226,15 +226,15 @@ impl VelocityConstraint { // Normal part. { let gcross1 = rb1 - .world_inv_inertia_sqrt + .effective_world_inv_inertia_sqrt .transform_vector(dp1.gcross(force_dir1)); let gcross2 = rb2 - .world_inv_inertia_sqrt + .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-force_dir1)); let r = 1.0 - / (rb1.mass_properties.inv_mass - + rb2.mass_properties.inv_mass + / (rb1.effective_inv_mass + + rb2.effective_inv_mass + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); @@ -263,14 +263,14 @@ impl VelocityConstraint { for j in 0..DIM - 1 { let gcross1 = rb1 - .world_inv_inertia_sqrt + .effective_world_inv_inertia_sqrt .transform_vector(dp1.gcross(tangents1[j])); let gcross2 = rb2 - .world_inv_inertia_sqrt + .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-tangents1[j])); let r = 1.0 - / (rb1.mass_properties.inv_mass - + rb2.mass_properties.inv_mass + / (rb1.effective_inv_mass + + rb2.effective_inv_mass + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); let rhs = (vel1 - vel2).dot(&tangents1[j]); -- 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_constraint.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/dynamics/solver/velocity_constraint.rs') diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 9d75830..514434b 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -207,7 +207,7 @@ impl VelocityConstraint { constraint.dir1 = force_dir1; constraint.im1 = rb1.effective_inv_mass; constraint.im2 = rb2.effective_inv_mass; - constraint.limit = manifold.data.friction; + constraint.limit = 0.0; constraint.mj_lambda1 = mj_lambda1; constraint.mj_lambda2 = mj_lambda2; constraint.manifold_id = manifold_id; -- cgit