From 7545e06cb15d6e851e5dee7d3761901e5d40f271 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Wed, 30 Dec 2020 12:03:25 +0100 Subject: Attempt to combine the position constraints initialization with the velocity constraints initialization. --- src/dynamics/solver/categorization.rs | 12 ++----- src/dynamics/solver/island_solver.rs | 2 ++ src/dynamics/solver/position_solver.rs | 60 ++-------------------------------- src/dynamics/solver/velocity_solver.rs | 39 ++++++++++++++++++---- 4 files changed, 39 insertions(+), 74 deletions(-) diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index d0a2d0f..69fe7df 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -5,9 +5,7 @@ pub(crate) fn categorize_position_contacts( bodies: &RigidBodySet, manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], - out_point_point_ground: &mut Vec, out_plane_point_ground: &mut Vec, - out_point_point: &mut Vec, out_plane_point: &mut Vec, ) { for manifold_i in manifold_indices { @@ -16,15 +14,9 @@ pub(crate) fn categorize_position_contacts( let rb2 = &bodies[manifold.data.body_pair.body2]; if !rb1.is_dynamic() || !rb2.is_dynamic() { - match manifold.kinematics.category { - KinematicsCategory::PointPoint => out_point_point_ground.push(*manifold_i), - KinematicsCategory::PlanePoint => out_plane_point_ground.push(*manifold_i), - } + out_plane_point_ground.push(*manifold_i) } else { - match manifold.kinematics.category { - KinematicsCategory::PointPoint => out_point_point.push(*manifold_i), - KinematicsCategory::PlanePoint => out_plane_point.push(*manifold_i), - } + out_plane_point.push(*manifold_i) } } } diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index 7ce142a..1b40634 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -29,6 +29,7 @@ impl IslandSolver { ) { if manifold_indices.len() != 0 || joint_indices.len() != 0 { counters.solver.velocity_assembly_time.resume(); + self.position_solver.part.constraints.clear(); self.velocity_solver.init_constraints( island_id, params, @@ -37,6 +38,7 @@ impl IslandSolver { &manifold_indices, joints, &joint_indices, + &mut self.position_solver.part.constraints, ); counters.solver.velocity_assembly_time.pause(); diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs index be70d74..d610f3a 100644 --- a/src/dynamics/solver/position_solver.rs +++ b/src/dynamics/solver/position_solver.rs @@ -33,13 +33,9 @@ impl PositionSolverJointPart { } pub(crate) struct PositionSolverPart { - pub point_point_manifolds: Vec, pub plane_point_manifolds: Vec, - pub point_point_ground_manifolds: Vec, pub plane_point_ground_manifolds: Vec, - pub point_point_groups: InteractionGroups, pub plane_point_groups: InteractionGroups, - pub point_point_ground_groups: InteractionGroups, pub plane_point_ground_groups: InteractionGroups, pub constraints: Vec, } @@ -47,13 +43,9 @@ pub(crate) struct PositionSolverPart { impl PositionSolverPart { pub fn new() -> Self { Self { - point_point_manifolds: Vec::new(), plane_point_manifolds: Vec::new(), - point_point_ground_manifolds: Vec::new(), plane_point_ground_manifolds: Vec::new(), - point_point_groups: InteractionGroups::new(), plane_point_groups: InteractionGroups::new(), - point_point_ground_groups: InteractionGroups::new(), plane_point_ground_groups: InteractionGroups::new(), constraints: Vec::new(), } @@ -62,7 +54,7 @@ impl PositionSolverPart { pub(crate) struct PositionSolver { positions: Vec>, - part: PositionSolverPart, + pub part: PositionSolverPart, joint_part: PositionSolverJointPart, } @@ -85,8 +77,8 @@ impl PositionSolver { joints: &[JointGraphEdge], joint_constraint_indices: &[JointIndex], ) { - self.part - .init_constraints(island_id, params, bodies, manifolds, manifold_indices); + // self.part + // .init_constraints(island_id, params, bodies, manifolds, manifold_indices); self.joint_part.init_constraints( island_id, params, @@ -134,27 +126,16 @@ impl PositionSolverPart { manifolds_all: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], ) { - self.point_point_ground_manifolds.clear(); self.plane_point_ground_manifolds.clear(); - self.point_point_manifolds.clear(); self.plane_point_manifolds.clear(); categorize_position_contacts( bodies, manifolds_all, manifold_indices, - &mut self.point_point_ground_manifolds, &mut self.plane_point_ground_manifolds, - &mut self.point_point_manifolds, &mut self.plane_point_manifolds, ); - self.point_point_groups.clear_groups(); - self.point_point_groups.group_manifolds( - island_id, - bodies, - manifolds_all, - &self.point_point_manifolds, - ); self.plane_point_groups.clear_groups(); self.plane_point_groups.group_manifolds( island_id, @@ -163,13 +144,6 @@ impl PositionSolverPart { &self.plane_point_manifolds, ); - self.point_point_ground_groups.clear_groups(); - self.point_point_ground_groups.group_manifolds( - island_id, - bodies, - manifolds_all, - &self.point_point_ground_manifolds, - ); self.plane_point_ground_groups.clear_groups(); self.plane_point_ground_groups.group_manifolds( island_id, @@ -185,13 +159,6 @@ impl PositionSolverPart { */ #[cfg(feature = "simd-is-enabled")] { - compute_grouped_constraints( - params, - bodies, - manifolds_all, - &self.point_point_groups.grouped_interactions, - &mut self.constraints, - ); compute_grouped_constraints( params, bodies, @@ -200,13 +167,6 @@ impl PositionSolverPart { &mut self.constraints, ); } - compute_nongrouped_constraints( - params, - bodies, - manifolds_all, - &self.point_point_groups.nongrouped_interactions, - &mut self.constraints, - ); compute_nongrouped_constraints( params, bodies, @@ -220,13 +180,6 @@ impl PositionSolverPart { */ #[cfg(feature = "simd-is-enabled")] { - compute_grouped_ground_constraints( - params, - bodies, - manifolds_all, - &self.point_point_ground_groups.grouped_interactions, - &mut self.constraints, - ); compute_grouped_ground_constraints( params, bodies, @@ -235,13 +188,6 @@ impl PositionSolverPart { &mut self.constraints, ); } - compute_nongrouped_ground_constraints( - params, - bodies, - manifolds_all, - &self.point_point_ground_groups.nongrouped_interactions, - &mut self.constraints, - ); compute_nongrouped_ground_constraints( params, bodies, diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index 593eb0f..f091fc2 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -4,6 +4,10 @@ use super::{ #[cfg(feature = "simd-is-enabled")] use super::{WVelocityConstraint, WVelocityGroundConstraint}; use crate::dynamics::solver::categorization::{categorize_joints, categorize_velocity_contacts}; +use crate::dynamics::solver::{ + AnyPositionConstraint, PositionConstraint, PositionGroundConstraint, WPositionConstraint, + WPositionGroundConstraint, +}; use crate::dynamics::{ solver::{AnyVelocityConstraint, DeltaVel}, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, @@ -37,9 +41,16 @@ impl VelocitySolver { manifold_indices: &[ContactManifoldIndex], joints: &[JointGraphEdge], joint_constraint_indices: &[JointIndex], + position_constraints: &mut Vec, ) { - self.contact_part - .init_constraints(island_id, params, bodies, manifolds, manifold_indices); + self.contact_part.init_constraints( + island_id, + params, + bodies, + manifolds, + manifold_indices, + position_constraints, + ); self.joint_part.init_constraints( island_id, params, @@ -173,19 +184,25 @@ impl VelocitySolverPart { bodies: &RigidBodySet, manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], + position_constraints: &mut Vec, ) { self.init_constraint_groups(island_id, bodies, manifolds, manifold_indices); self.constraints.clear(); #[cfg(feature = "simd-is-enabled")] { - self.compute_grouped_constraints(params, bodies, manifolds); + self.compute_grouped_constraints(params, bodies, manifolds, position_constraints); } - self.compute_nongrouped_constraints(params, bodies, manifolds); + self.compute_nongrouped_constraints(params, bodies, manifolds, position_constraints); #[cfg(feature = "simd-is-enabled")] { - self.compute_grouped_ground_constraints(params, bodies, manifolds); + self.compute_grouped_ground_constraints( + params, + bodies, + manifolds, + position_constraints, + ); } - self.compute_nongrouped_ground_constraints(params, bodies, manifolds); + self.compute_nongrouped_ground_constraints(params, bodies, manifolds, position_constraints); } #[cfg(feature = "simd-is-enabled")] @@ -194,6 +211,7 @@ impl VelocitySolverPart { params: &IntegrationParameters, bodies: &RigidBodySet, manifolds_all: &[&mut ContactManifold], + constraints_all: &mut Vec, ) { for manifolds_i in self .interaction_groups @@ -210,6 +228,7 @@ impl VelocitySolverPart { &mut self.constraints, true, ); + WPositionConstraint::generate(params, manifolds, bodies, constraints_all, true); } } @@ -218,6 +237,7 @@ impl VelocitySolverPart { params: &IntegrationParameters, bodies: &RigidBodySet, manifolds_all: &[&mut ContactManifold], + constraints_all: &mut Vec, ) { for manifold_i in &self.interaction_groups.nongrouped_interactions { let manifold = &manifolds_all[*manifold_i]; @@ -229,6 +249,7 @@ impl VelocitySolverPart { &mut self.constraints, true, ); + PositionConstraint::generate(params, manifold, bodies, constraints_all, true); } } @@ -238,6 +259,7 @@ impl VelocitySolverPart { params: &IntegrationParameters, bodies: &RigidBodySet, manifolds_all: &[&mut ContactManifold], + constraints_all: &mut Vec, ) { for manifolds_i in self .ground_interaction_groups @@ -254,6 +276,7 @@ impl VelocitySolverPart { &mut self.constraints, true, ); + WPositionGroundConstraint::generate(params, manifolds, bodies, constraints_all, true); } } @@ -262,6 +285,7 @@ impl VelocitySolverPart { params: &IntegrationParameters, bodies: &RigidBodySet, manifolds_all: &[&mut ContactManifold], + constraints_all: &mut Vec, ) { for manifold_i in &self.ground_interaction_groups.nongrouped_interactions { let manifold = &manifolds_all[*manifold_i]; @@ -272,7 +296,8 @@ impl VelocitySolverPart { bodies, &mut self.constraints, true, - ) + ); + PositionGroundConstraint::generate(params, manifold, bodies, constraints_all, true) } } } -- cgit