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/position_constraint.rs | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'src/dynamics/solver/position_constraint.rs') diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs index 69fcf57..b329e39 100644 --- a/src/dynamics/solver/position_constraint.rs +++ b/src/dynamics/solver/position_constraint.rs @@ -88,8 +88,8 @@ impl PositionConstraint { 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 shift1 = manifold.local_n1 * -manifold.kinematics.radius1; let shift2 = manifold.local_n2 * -manifold.kinematics.radius2; let radius = @@ -104,8 +104,8 @@ impl PositionConstraint { let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS]; for l in 0..manifold_points.len() { - local_p1[l] = manifold.delta1 * (manifold_points[l].local_p1 + shift1); - local_p2[l] = manifold.delta2 * (manifold_points[l].local_p2 + shift2); + local_p1[l] = manifold.data.delta1 * (manifold_points[l].local_p1 + shift1); + local_p2[l] = manifold.data.delta2 * (manifold_points[l].local_p2 + shift2); } let constraint = PositionConstraint { @@ -132,10 +132,10 @@ impl PositionConstraint { } } else { if manifold.kinematics.category == KinematicsCategory::PointPoint { - out_constraints[manifold.constraint_index + l] = + out_constraints[manifold.data.constraint_index + l] = AnyPositionConstraint::NongroupedPointPoint(constraint); } else { - out_constraints[manifold.constraint_index + l] = + out_constraints[manifold.data.constraint_index + l] = AnyPositionConstraint::NongroupedPlanePoint(constraint); } } -- 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/position_constraint.rs | 33 +++++++++++++++--------------- 1 file changed, 16 insertions(+), 17 deletions(-) (limited to 'src/dynamics/solver/position_constraint.rs') diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs index b329e39..72dd534 100644 --- a/src/dynamics/solver/position_constraint.rs +++ b/src/dynamics/solver/position_constraint.rs @@ -63,9 +63,9 @@ pub(crate) struct PositionConstraint { // NOTE: the points are relative to the center of masses. pub local_p1: [Point; MAX_MANIFOLD_POINTS], pub local_p2: [Point; MAX_MANIFOLD_POINTS], + pub dists: [f32; MAX_MANIFOLD_POINTS], pub local_n1: Vector, pub num_contacts: u8, - pub radius: f32, pub im1: f32, pub im2: f32, pub ii1: AngularInertia, @@ -90,22 +90,21 @@ impl PositionConstraint { ) { let rb1 = &bodies[manifold.data.body_pair.body1]; let rb2 = &bodies[manifold.data.body_pair.body2]; - let shift1 = manifold.local_n1 * -manifold.kinematics.radius1; - let shift2 = manifold.local_n2 * -manifold.kinematics.radius2; - let radius = - manifold.kinematics.radius1 + manifold.kinematics.radius2 /*- params.allowed_linear_error*/; - - for (l, manifold_points) in manifold - .active_contacts() - .chunks(MAX_MANIFOLD_POINTS) - .enumerate() - { + let active_contacts = &manifold.data.solver_contacts[..manifold.num_active_contacts]; + + for (l, manifold_points) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() { let mut local_p1 = [Point::origin(); MAX_MANIFOLD_POINTS]; let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS]; + let mut dists = [0.0; MAX_MANIFOLD_POINTS]; for l in 0..manifold_points.len() { - local_p1[l] = manifold.data.delta1 * (manifold_points[l].local_p1 + shift1); - local_p2[l] = manifold.data.delta2 * (manifold_points[l].local_p2 + shift2); + local_p1[l] = rb1 + .position + .inverse_transform_point(&manifold_points[l].point); + local_p2[l] = rb2 + .position + .inverse_transform_point(&manifold_points[l].point); + dists[l] = manifold_points[l].dist; } let constraint = PositionConstraint { @@ -113,8 +112,8 @@ impl PositionConstraint { rb2: rb2.active_set_offset, local_p1, local_p2, - local_n1: manifold.local_n1, - radius, + local_n1: rb1.position.inverse_transform_vector(&manifold.data.normal), + dists, im1: rb1.mass_properties.inv_mass, im2: rb2.mass_properties.inv_mass, ii1: rb1.world_inv_inertia_sqrt.squared(), @@ -152,9 +151,9 @@ impl PositionConstraint { let mut pos1 = positions[self.rb1]; let mut pos2 = positions[self.rb2]; let allowed_err = params.allowed_linear_error; - let target_dist = self.radius - allowed_err; for k in 0..self.num_contacts as usize { + let target_dist = -self.dists[k] - allowed_err; let p1 = pos1 * self.local_p1[k]; let p2 = pos2 * self.local_p2[k]; let dpos = p2 - p1; @@ -204,9 +203,9 @@ impl PositionConstraint { let mut pos1 = positions[self.rb1]; let mut pos2 = positions[self.rb2]; let allowed_err = params.allowed_linear_error; - let target_dist = self.radius - allowed_err; for k in 0..self.num_contacts as usize { + let target_dist = -self.dists[k] - allowed_err; let n1 = pos1 * self.local_n1; let p1 = pos1 * self.local_p1[k]; let p2 = pos2 * self.local_p2[k]; -- cgit From 348a339fe350aff6d885cb5a857a0bb6afbea990 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Thu, 31 Dec 2020 10:02:51 +0100 Subject: Remove code related to point-point kinematics. --- src/dynamics/solver/position_constraint.rs | 115 ++++------------------------- 1 file changed, 13 insertions(+), 102 deletions(-) (limited to 'src/dynamics/solver/position_constraint.rs') diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs index 72dd534..1ef0660 100644 --- a/src/dynamics/solver/position_constraint.rs +++ b/src/dynamics/solver/position_constraint.rs @@ -2,7 +2,7 @@ use crate::dynamics::solver::PositionGroundConstraint; #[cfg(feature = "simd-is-enabled")] use crate::dynamics::solver::{WPositionConstraint, WPositionGroundConstraint}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; -use crate::geometry::{ContactManifold, KinematicsCategory}; +use crate::geometry::ContactManifold; use crate::math::{ AngularInertia, Isometry, Point, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS, }; @@ -10,17 +10,11 @@ use crate::utils::{WAngularInertia, WCross, WDot}; pub(crate) enum AnyPositionConstraint { #[cfg(feature = "simd-is-enabled")] - GroupedPointPointGround(WPositionGroundConstraint), + GroupedGround(WPositionGroundConstraint), + NonGroupedGround(PositionGroundConstraint), #[cfg(feature = "simd-is-enabled")] - GroupedPlanePointGround(WPositionGroundConstraint), - NongroupedPointPointGround(PositionGroundConstraint), - NongroupedPlanePointGround(PositionGroundConstraint), - #[cfg(feature = "simd-is-enabled")] - GroupedPointPoint(WPositionConstraint), - #[cfg(feature = "simd-is-enabled")] - GroupedPlanePoint(WPositionConstraint), - NongroupedPointPoint(PositionConstraint), - NongroupedPlanePoint(PositionConstraint), + GroupedNonGround(WPositionConstraint), + NonGroupedNonGround(PositionConstraint), #[allow(dead_code)] // The Empty variant is only used with parallel code. Empty, } @@ -29,29 +23,11 @@ impl AnyPositionConstraint { pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { match self { #[cfg(feature = "simd-is-enabled")] - AnyPositionConstraint::GroupedPointPointGround(c) => { - c.solve_point_point(params, positions) - } - #[cfg(feature = "simd-is-enabled")] - AnyPositionConstraint::GroupedPlanePointGround(c) => { - c.solve_plane_point(params, positions) - } - AnyPositionConstraint::NongroupedPointPointGround(c) => { - c.solve_point_point(params, positions) - } - AnyPositionConstraint::NongroupedPlanePointGround(c) => { - c.solve_plane_point(params, positions) - } - #[cfg(feature = "simd-is-enabled")] - AnyPositionConstraint::GroupedPointPoint(c) => c.solve_point_point(params, positions), + AnyPositionConstraint::GroupedGround(c) => c.solve(params, positions), + AnyPositionConstraint::NonGroupedGround(c) => c.solve(params, positions), #[cfg(feature = "simd-is-enabled")] - AnyPositionConstraint::GroupedPlanePoint(c) => c.solve_plane_point(params, positions), - AnyPositionConstraint::NongroupedPointPoint(c) => { - c.solve_point_point(params, positions) - } - AnyPositionConstraint::NongroupedPlanePoint(c) => { - c.solve_plane_point(params, positions) - } + AnyPositionConstraint::GroupedNonGround(c) => c.solve(params, positions), + AnyPositionConstraint::NonGroupedNonGround(c) => c.solve(params, positions), AnyPositionConstraint::Empty => unreachable!(), } } @@ -124,80 +100,15 @@ impl PositionConstraint { }; if push { - if manifold.kinematics.category == KinematicsCategory::PointPoint { - out_constraints.push(AnyPositionConstraint::NongroupedPointPoint(constraint)); - } else { - out_constraints.push(AnyPositionConstraint::NongroupedPlanePoint(constraint)); - } + out_constraints.push(AnyPositionConstraint::NonGroupedNonGround(constraint)); } else { - if manifold.kinematics.category == KinematicsCategory::PointPoint { - out_constraints[manifold.data.constraint_index + l] = - AnyPositionConstraint::NongroupedPointPoint(constraint); - } else { - out_constraints[manifold.data.constraint_index + l] = - AnyPositionConstraint::NongroupedPlanePoint(constraint); - } + out_constraints[manifold.data.constraint_index + l] = + AnyPositionConstraint::NonGroupedNonGround(constraint); } } } - pub fn solve_point_point( - &self, - params: &IntegrationParameters, - positions: &mut [Isometry], - ) { - // FIXME: can we avoid most of the multiplications by pos1/pos2? - // Compute jacobians. - let mut pos1 = positions[self.rb1]; - let mut pos2 = positions[self.rb2]; - let allowed_err = params.allowed_linear_error; - - for k in 0..self.num_contacts as usize { - let target_dist = -self.dists[k] - allowed_err; - let p1 = pos1 * self.local_p1[k]; - let p2 = pos2 * self.local_p2[k]; - let dpos = p2 - p1; - - let sqdist = dpos.norm_squared(); - - // NOTE: only works for the point-point case. - if sqdist < target_dist * target_dist { - let dist = sqdist.sqrt(); - let n = dpos / dist; - let err = ((dist - target_dist) * self.erp).max(-self.max_linear_correction); - let dp1 = p1.coords - pos1.translation.vector; - let dp2 = p2.coords - pos2.translation.vector; - - let gcross1 = dp1.gcross(n); - let gcross2 = -dp2.gcross(n); - let ii_gcross1 = self.ii1.transform_vector(gcross1); - let ii_gcross2 = self.ii2.transform_vector(gcross2); - - // Compute impulse. - let inv_r = - self.im1 + self.im2 + gcross1.gdot(ii_gcross1) + gcross2.gdot(ii_gcross2); - let impulse = err / inv_r; - - // Apply impulse. - let tra1 = Translation::from(n * (impulse * self.im1)); - let tra2 = Translation::from(n * (-impulse * self.im2)); - let rot1 = Rotation::new(ii_gcross1 * impulse); - let rot2 = Rotation::new(ii_gcross2 * impulse); - - pos1 = Isometry::from_parts(tra1 * pos1.translation, rot1 * pos1.rotation); - pos2 = Isometry::from_parts(tra2 * pos2.translation, rot2 * pos2.rotation); - } - } - - positions[self.rb1] = pos1; - positions[self.rb2] = pos2; - } - - pub fn solve_plane_point( - &self, - params: &IntegrationParameters, - positions: &mut [Isometry], - ) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { // FIXME: can we avoid most of the multiplications by pos1/pos2? // Compute jacobians. let mut pos1 = positions[self.rb1]; -- 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/position_constraint.rs | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) (limited to 'src/dynamics/solver/position_constraint.rs') diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs index 1ef0660..843fd1a 100644 --- a/src/dynamics/solver/position_constraint.rs +++ b/src/dynamics/solver/position_constraint.rs @@ -66,9 +66,13 @@ impl PositionConstraint { ) { let rb1 = &bodies[manifold.data.body_pair.body1]; let rb2 = &bodies[manifold.data.body_pair.body2]; - 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() + { let mut local_p1 = [Point::origin(); MAX_MANIFOLD_POINTS]; let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS]; let mut dists = [0.0; MAX_MANIFOLD_POINTS]; -- 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/position_constraint.rs | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) (limited to 'src/dynamics/solver/position_constraint.rs') diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs index 843fd1a..e7188f8 100644 --- a/src/dynamics/solver/position_constraint.rs +++ b/src/dynamics/solver/position_constraint.rs @@ -4,7 +4,7 @@ use crate::dynamics::solver::{WPositionConstraint, WPositionGroundConstraint}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::ContactManifold; use crate::math::{ - AngularInertia, Isometry, Point, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS, + AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS, }; use crate::utils::{WAngularInertia, WCross, WDot}; @@ -20,7 +20,7 @@ pub(crate) enum AnyPositionConstraint { } impl AnyPositionConstraint { - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { match self { #[cfg(feature = "simd-is-enabled")] AnyPositionConstraint::GroupedGround(c) => c.solve(params, positions), @@ -37,17 +37,17 @@ pub(crate) struct PositionConstraint { pub rb1: usize, pub rb2: usize, // NOTE: the points are relative to the center of masses. - pub local_p1: [Point; MAX_MANIFOLD_POINTS], - pub local_p2: [Point; MAX_MANIFOLD_POINTS], - pub dists: [f32; MAX_MANIFOLD_POINTS], - pub local_n1: Vector, + pub local_p1: [Point; MAX_MANIFOLD_POINTS], + pub local_p2: [Point; MAX_MANIFOLD_POINTS], + pub dists: [Real; MAX_MANIFOLD_POINTS], + pub local_n1: Vector, pub num_contacts: u8, - pub im1: f32, - pub im2: f32, - pub ii1: AngularInertia, - pub ii2: AngularInertia, - pub erp: f32, - pub max_linear_correction: f32, + pub im1: Real, + pub im2: Real, + pub ii1: AngularInertia, + pub ii2: AngularInertia, + pub erp: Real, + pub max_linear_correction: Real, } impl PositionConstraint { @@ -112,7 +112,7 @@ impl PositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { // FIXME: can we avoid most of the multiplications by pos1/pos2? // Compute jacobians. let mut pos1 = positions[self.rb1]; -- 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/position_constraint.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/dynamics/solver/position_constraint.rs') diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs index e7188f8..9aa10bb 100644 --- a/src/dynamics/solver/position_constraint.rs +++ b/src/dynamics/solver/position_constraint.rs @@ -53,8 +53,8 @@ pub(crate) struct PositionConstraint { impl PositionConstraint { #[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/position_constraint.rs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src/dynamics/solver/position_constraint.rs') diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs index 9aa10bb..f846989 100644 --- a/src/dynamics/solver/position_constraint.rs +++ b/src/dynamics/solver/position_constraint.rs @@ -94,10 +94,10 @@ impl PositionConstraint { local_p2, local_n1: rb1.position.inverse_transform_vector(&manifold.data.normal), dists, - im1: rb1.mass_properties.inv_mass, - im2: rb2.mass_properties.inv_mass, - ii1: rb1.world_inv_inertia_sqrt.squared(), - ii2: rb2.world_inv_inertia_sqrt.squared(), + im1: rb1.effective_inv_mass, + im2: rb2.effective_inv_mass, + ii1: rb1.effective_world_inv_inertia_sqrt.squared(), + ii2: rb2.effective_world_inv_inertia_sqrt.squared(), num_contacts: manifold_points.len() as u8, erp: params.erp, max_linear_correction: params.max_linear_correction, -- cgit From 00caa7ce353c02cfc22f0cc27f6361e968157953 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Sat, 23 Jan 2021 12:58:45 +0100 Subject: Fix warnings and 2D compilation when parallelism is ON but SIMD is OFF. --- src/dynamics/solver/position_constraint.rs | 6 ------ 1 file changed, 6 deletions(-) (limited to 'src/dynamics/solver/position_constraint.rs') diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs index f846989..844b1cd 100644 --- a/src/dynamics/solver/position_constraint.rs +++ b/src/dynamics/solver/position_constraint.rs @@ -51,12 +51,6 @@ pub(crate) struct PositionConstraint { } impl PositionConstraint { - #[cfg(feature = "parallel")] - pub fn num_active_constraints(manifold: &ContactManifold) -> 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( params: &IntegrationParameters, manifold: &ContactManifold, -- cgit