From 64507a68e179ebc652f177e727fac5ff1a82d931 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Thu, 31 Dec 2020 10:32:34 +0100 Subject: Refactor the constraints solver code. --- src/dynamics/solver/solver_constraints.rs | 383 ++++++++++++++++++++++++++++++ 1 file changed, 383 insertions(+) create mode 100644 src/dynamics/solver/solver_constraints.rs (limited to 'src/dynamics/solver/solver_constraints.rs') diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs new file mode 100644 index 0000000..88371ba --- /dev/null +++ b/src/dynamics/solver/solver_constraints.rs @@ -0,0 +1,383 @@ +use super::{ + AnyJointVelocityConstraint, InteractionGroups, VelocityConstraint, VelocityGroundConstraint, +}; +#[cfg(feature = "simd-is-enabled")] +use super::{WVelocityConstraint, WVelocityGroundConstraint}; +use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints}; +use crate::dynamics::solver::{ + AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint, + PositionGroundConstraint, WPositionConstraint, WPositionGroundConstraint, +}; +use crate::dynamics::{ + solver::{AnyVelocityConstraint, DeltaVel}, + IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, +}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; +#[cfg(feature = "simd-is-enabled")] +use crate::math::SIMD_WIDTH; +use crate::utils::WAngularInertia; + +pub(crate) struct SolverConstraints { + pub not_ground_interactions: Vec, + pub ground_interactions: Vec, + pub interaction_groups: InteractionGroups, + pub ground_interaction_groups: InteractionGroups, + pub velocity_constraints: Vec, + pub position_constraints: Vec, +} + +impl + SolverConstraints +{ + pub fn new() -> Self { + Self { + not_ground_interactions: Vec::new(), + ground_interactions: Vec::new(), + interaction_groups: InteractionGroups::new(), + ground_interaction_groups: InteractionGroups::new(), + velocity_constraints: Vec::new(), + position_constraints: Vec::new(), + } + } +} + +impl SolverConstraints { + pub fn init_constraint_groups( + &mut self, + island_id: usize, + bodies: &RigidBodySet, + manifolds: &[&mut ContactManifold], + manifold_indices: &[ContactManifoldIndex], + ) { + self.not_ground_interactions.clear(); + self.ground_interactions.clear(); + categorize_contacts( + bodies, + manifolds, + manifold_indices, + &mut self.ground_interactions, + &mut self.not_ground_interactions, + ); + + self.interaction_groups.clear_groups(); + self.interaction_groups.group_manifolds( + island_id, + bodies, + manifolds, + &self.not_ground_interactions, + ); + + self.ground_interaction_groups.clear_groups(); + self.ground_interaction_groups.group_manifolds( + island_id, + bodies, + manifolds, + &self.ground_interactions, + ); + + // NOTE: uncomment this do disable SIMD contact resolution. + // self.interaction_groups + // .nongrouped_interactions + // .append(&mut self.interaction_groups.grouped_interactions); + // self.ground_interaction_groups + // .nongrouped_interactions + // .append(&mut self.ground_interaction_groups.grouped_interactions); + } + + pub fn init( + &mut self, + island_id: usize, + params: &IntegrationParameters, + bodies: &RigidBodySet, + manifolds: &[&mut ContactManifold], + manifold_indices: &[ContactManifoldIndex], + ) { + self.velocity_constraints.clear(); + self.position_constraints.clear(); + + self.init_constraint_groups(island_id, bodies, manifolds, manifold_indices); + + #[cfg(feature = "simd-is-enabled")] + { + self.compute_grouped_constraints(params, bodies, manifolds); + } + self.compute_nongrouped_constraints(params, bodies, manifolds); + #[cfg(feature = "simd-is-enabled")] + { + self.compute_grouped_ground_constraints(params, bodies, manifolds); + } + self.compute_nongrouped_ground_constraints(params, bodies, manifolds); + } + + #[cfg(feature = "simd-is-enabled")] + fn compute_grouped_constraints( + &mut self, + params: &IntegrationParameters, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + ) { + for manifolds_i in self + .interaction_groups + .grouped_interactions + .chunks_exact(SIMD_WIDTH) + { + let manifold_id = array![|ii| manifolds_i[ii]; SIMD_WIDTH]; + let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH]; + WVelocityConstraint::generate( + params, + manifold_id, + manifolds, + bodies, + &mut self.velocity_constraints, + true, + ); + WPositionConstraint::generate( + params, + manifolds, + bodies, + &mut self.position_constraints, + true, + ); + } + } + + fn compute_nongrouped_constraints( + &mut self, + params: &IntegrationParameters, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + ) { + for manifold_i in &self.interaction_groups.nongrouped_interactions { + let manifold = &manifolds_all[*manifold_i]; + VelocityConstraint::generate( + params, + *manifold_i, + manifold, + bodies, + &mut self.velocity_constraints, + true, + ); + PositionConstraint::generate( + params, + manifold, + bodies, + &mut self.position_constraints, + true, + ); + } + } + + #[cfg(feature = "simd-is-enabled")] + fn compute_grouped_ground_constraints( + &mut self, + params: &IntegrationParameters, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + ) { + for manifolds_i in self + .ground_interaction_groups + .grouped_interactions + .chunks_exact(SIMD_WIDTH) + { + let manifold_id = array![|ii| manifolds_i[ii]; SIMD_WIDTH]; + let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH]; + WVelocityGroundConstraint::generate( + params, + manifold_id, + manifolds, + bodies, + &mut self.velocity_constraints, + true, + ); + WPositionGroundConstraint::generate( + params, + manifolds, + bodies, + &mut self.position_constraints, + true, + ); + } + } + + fn compute_nongrouped_ground_constraints( + &mut self, + params: &IntegrationParameters, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + ) { + for manifold_i in &self.ground_interaction_groups.nongrouped_interactions { + let manifold = &manifolds_all[*manifold_i]; + VelocityGroundConstraint::generate( + params, + *manifold_i, + manifold, + bodies, + &mut self.velocity_constraints, + true, + ); + PositionGroundConstraint::generate( + params, + manifold, + bodies, + &mut self.position_constraints, + true, + ) + } + } +} + +impl SolverConstraints { + pub fn init( + &mut self, + island_id: usize, + params: &IntegrationParameters, + bodies: &RigidBodySet, + joints: &[JointGraphEdge], + joint_constraint_indices: &[JointIndex], + ) { + // Generate constraints for joints. + self.not_ground_interactions.clear(); + self.ground_interactions.clear(); + categorize_joints( + bodies, + joints, + joint_constraint_indices, + &mut self.ground_interactions, + &mut self.not_ground_interactions, + ); + + self.velocity_constraints.clear(); + self.position_constraints.clear(); + + self.interaction_groups.clear_groups(); + self.interaction_groups.group_joints( + island_id, + bodies, + joints, + &self.not_ground_interactions, + ); + + self.ground_interaction_groups.clear_groups(); + self.ground_interaction_groups.group_joints( + island_id, + bodies, + joints, + &self.ground_interactions, + ); + // NOTE: uncomment this do disable SIMD joint resolution. + // self.interaction_groups + // .nongrouped_interactions + // .append(&mut self.interaction_groups.grouped_interactions); + // self.ground_interaction_groups + // .nongrouped_interactions + // .append(&mut self.ground_interaction_groups.grouped_interactions); + + self.compute_nongrouped_joint_ground_constraints(params, bodies, joints); + #[cfg(feature = "simd-is-enabled")] + { + self.compute_grouped_joint_ground_constraints(params, bodies, joints); + } + self.compute_nongrouped_joint_constraints(params, bodies, joints); + #[cfg(feature = "simd-is-enabled")] + { + self.compute_grouped_joint_constraints(params, bodies, joints); + } + } + + fn compute_nongrouped_joint_ground_constraints( + &mut self, + params: &IntegrationParameters, + bodies: &RigidBodySet, + joints_all: &[JointGraphEdge], + ) { + for joint_i in &self.ground_interaction_groups.nongrouped_interactions { + let joint = &joints_all[*joint_i].weight; + let vel_constraint = + AnyJointVelocityConstraint::from_joint_ground(params, *joint_i, joint, bodies); + self.velocity_constraints.push(vel_constraint); + let pos_constraint = AnyJointPositionConstraint::from_joint_ground(joint, bodies); + self.position_constraints.push(pos_constraint); + } + } + + #[cfg(feature = "simd-is-enabled")] + fn compute_grouped_joint_ground_constraints( + &mut self, + params: &IntegrationParameters, + bodies: &RigidBodySet, + joints_all: &[JointGraphEdge], + ) { + for joints_i in self + .ground_interaction_groups + .grouped_interactions + .chunks_exact(SIMD_WIDTH) + { + let joints_id = array![|ii| joints_i[ii]; SIMD_WIDTH]; + let joints = array![|ii| &joints_all[joints_i[ii]].weight; SIMD_WIDTH]; + let vel_constraint = AnyJointVelocityConstraint::from_wide_joint_ground( + params, joints_id, joints, bodies, + ); + self.velocity_constraints.push(vel_constraint); + + if let Some(pos_constraint) = + AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies) + { + self.position_constraints.push(pos_constraint); + } else { + for joint in joints.iter() { + self.position_constraints + .push(AnyJointPositionConstraint::from_joint_ground( + *joint, bodies, + )) + } + } + } + } + + fn compute_nongrouped_joint_constraints( + &mut self, + params: &IntegrationParameters, + bodies: &RigidBodySet, + joints_all: &[JointGraphEdge], + ) { + for joint_i in &self.interaction_groups.nongrouped_interactions { + let joint = &joints_all[*joint_i].weight; + let vel_constraint = + AnyJointVelocityConstraint::from_joint(params, *joint_i, joint, bodies); + self.velocity_constraints.push(vel_constraint); + let pos_constraint = AnyJointPositionConstraint::from_joint(joint, bodies); + self.position_constraints.push(pos_constraint); + } + } + + #[cfg(feature = "simd-is-enabled")] + fn compute_grouped_joint_constraints( + &mut self, + params: &IntegrationParameters, + bodies: &RigidBodySet, + joints_all: &[JointGraphEdge], + ) { + for joints_i in self + .interaction_groups + .grouped_interactions + .chunks_exact(SIMD_WIDTH) + { + let joints_id = array![|ii| joints_i[ii]; SIMD_WIDTH]; + let joints = array![|ii| &joints_all[joints_i[ii]].weight; SIMD_WIDTH]; + let vel_constraint = + AnyJointVelocityConstraint::from_wide_joint(params, joints_id, joints, bodies); + self.velocity_constraints.push(vel_constraint); + + if let Some(pos_constraint) = + AnyJointPositionConstraint::from_wide_joint(joints, bodies) + { + self.position_constraints.push(pos_constraint); + } else { + for joint in joints.iter() { + self.position_constraints + .push(AnyJointPositionConstraint::from_joint(*joint, bodies)) + } + } + } + } +} -- 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/solver_constraints.rs | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) (limited to 'src/dynamics/solver/solver_constraints.rs') diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs index 88371ba..92faa3b 100644 --- a/src/dynamics/solver/solver_constraints.rs +++ b/src/dynamics/solver/solver_constraints.rs @@ -9,13 +9,11 @@ use crate::dynamics::solver::{ PositionGroundConstraint, WPositionConstraint, WPositionGroundConstraint, }; use crate::dynamics::{ - solver::{AnyVelocityConstraint, DeltaVel}, - IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, + solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, }; use crate::geometry::{ContactManifold, ContactManifoldIndex}; #[cfg(feature = "simd-is-enabled")] use crate::math::SIMD_WIDTH; -use crate::utils::WAngularInertia; pub(crate) struct SolverConstraints { pub not_ground_interactions: Vec, -- 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/solver_constraints.rs | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) (limited to 'src/dynamics/solver/solver_constraints.rs') diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs index 92faa3b..1965d6d 100644 --- a/src/dynamics/solver/solver_constraints.rs +++ b/src/dynamics/solver/solver_constraints.rs @@ -2,11 +2,12 @@ use super::{ AnyJointVelocityConstraint, InteractionGroups, VelocityConstraint, VelocityGroundConstraint, }; #[cfg(feature = "simd-is-enabled")] -use super::{WVelocityConstraint, WVelocityGroundConstraint}; +use super::{ + WPositionConstraint, WPositionGroundConstraint, WVelocityConstraint, WVelocityGroundConstraint, +}; use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints}; use crate::dynamics::solver::{ - AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint, - PositionGroundConstraint, WPositionConstraint, WPositionGroundConstraint, + AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint, PositionGroundConstraint, }; use crate::dynamics::{ solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, -- 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/solver_constraints.rs | 26 ++++---------------------- 1 file changed, 4 insertions(+), 22 deletions(-) (limited to 'src/dynamics/solver/solver_constraints.rs') diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs index 1965d6d..b9dd497 100644 --- a/src/dynamics/solver/solver_constraints.rs +++ b/src/dynamics/solver/solver_constraints.rs @@ -318,18 +318,8 @@ impl SolverConstraints { ); self.velocity_constraints.push(vel_constraint); - if let Some(pos_constraint) = - AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies) - { - self.position_constraints.push(pos_constraint); - } else { - for joint in joints.iter() { - self.position_constraints - .push(AnyJointPositionConstraint::from_joint_ground( - *joint, bodies, - )) - } - } + let pos_constraint = AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies); + self.position_constraints.push(pos_constraint); } } @@ -367,16 +357,8 @@ impl SolverConstraints { AnyJointVelocityConstraint::from_wide_joint(params, joints_id, joints, bodies); self.velocity_constraints.push(vel_constraint); - if let Some(pos_constraint) = - AnyJointPositionConstraint::from_wide_joint(joints, bodies) - { - self.position_constraints.push(pos_constraint); - } else { - for joint in joints.iter() { - self.position_constraints - .push(AnyJointPositionConstraint::from_joint(*joint, bodies)) - } - } + let pos_constraint = AnyJointPositionConstraint::from_wide_joint(joints, bodies); + self.position_constraints.push(pos_constraint); } } } -- cgit