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/velocity_solver.rs | 39 ++++++++++++++++++++++++++++------ 1 file changed, 32 insertions(+), 7 deletions(-) (limited to 'src/dynamics/solver/velocity_solver.rs') 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 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/velocity_solver.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/dynamics/solver/velocity_solver.rs') diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index f091fc2..5f68995 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -3,7 +3,7 @@ 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::categorization::{categorize_contacts, categorize_joints}; use crate::dynamics::solver::{ AnyPositionConstraint, PositionConstraint, PositionGroundConstraint, WPositionConstraint, WPositionGroundConstraint, @@ -144,7 +144,7 @@ impl VelocitySolverPart { ) { self.not_ground_interactions.clear(); self.ground_interactions.clear(); - categorize_velocity_contacts( + categorize_contacts( bodies, manifolds, manifold_indices, -- cgit 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/velocity_solver.rs | 367 +-------------------------------- 1 file changed, 11 insertions(+), 356 deletions(-) (limited to 'src/dynamics/solver/velocity_solver.rs') diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index 5f68995..89f2809 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -5,8 +5,8 @@ use super::{ use super::{WVelocityConstraint, WVelocityGroundConstraint}; use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints}; use crate::dynamics::solver::{ - AnyPositionConstraint, PositionConstraint, PositionGroundConstraint, WPositionConstraint, - WPositionGroundConstraint, + AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint, + PositionGroundConstraint, WPositionConstraint, WPositionGroundConstraint, }; use crate::dynamics::{ solver::{AnyVelocityConstraint, DeltaVel}, @@ -19,54 +19,24 @@ use crate::utils::WAngularInertia; pub(crate) struct VelocitySolver { pub mj_lambdas: Vec>, - pub contact_part: VelocitySolverPart, - pub joint_part: VelocitySolverPart, } impl VelocitySolver { pub fn new() -> Self { Self { mj_lambdas: Vec::new(), - contact_part: VelocitySolverPart::new(), - joint_part: VelocitySolverPart::new(), } } - pub fn init_constraints( - &mut self, - island_id: usize, - params: &IntegrationParameters, - bodies: &RigidBodySet, - manifolds: &[&mut ContactManifold], - 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, - position_constraints, - ); - self.joint_part.init_constraints( - island_id, - params, - bodies, - joints, - joint_constraint_indices, - ) - } - - pub fn solve_constraints( + pub fn solve( &mut self, island_id: usize, params: &IntegrationParameters, bodies: &mut RigidBodySet, manifolds_all: &mut [&mut ContactManifold], joints_all: &mut [JointGraphEdge], + contact_constraints: &mut [AnyVelocityConstraint], + joint_constraints: &mut [AnyJointVelocityConstraint], ) { self.mj_lambdas.clear(); self.mj_lambdas @@ -75,11 +45,11 @@ impl VelocitySolver { /* * Warmstart constraints. */ - for constraint in &self.joint_part.constraints { + for constraint in &*joint_constraints { constraint.warmstart(&mut self.mj_lambdas[..]); } - for constraint in &self.contact_part.constraints { + for constraint in &*contact_constraints { constraint.warmstart(&mut self.mj_lambdas[..]); } @@ -87,11 +57,11 @@ impl VelocitySolver { * Solve constraints. */ for _ in 0..params.max_velocity_iterations { - for constraint in &mut self.joint_part.constraints { + for constraint in &mut *joint_constraints { constraint.solve(&mut self.mj_lambdas[..]); } - for constraint in &mut self.contact_part.constraints { + for constraint in &mut *contact_constraints { constraint.solve(&mut self.mj_lambdas[..]); } } @@ -104,327 +74,12 @@ impl VelocitySolver { }); // Write impulses back into the manifold structures. - for constraint in &self.joint_part.constraints { + for constraint in &*joint_constraints { constraint.writeback_impulses(joints_all); } - for constraint in &self.contact_part.constraints { + for constraint in &*contact_constraints { constraint.writeback_impulses(manifolds_all); } } } - -pub(crate) struct VelocitySolverPart { - pub not_ground_interactions: Vec, - pub ground_interactions: Vec, - pub interaction_groups: InteractionGroups, - pub ground_interaction_groups: InteractionGroups, - pub constraints: Vec, -} - -impl VelocitySolverPart { - pub fn new() -> Self { - Self { - not_ground_interactions: Vec::new(), - ground_interactions: Vec::new(), - interaction_groups: InteractionGroups::new(), - ground_interaction_groups: InteractionGroups::new(), - constraints: Vec::new(), - } - } -} - -impl VelocitySolverPart { - 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_constraints( - &mut self, - island_id: usize, - params: &IntegrationParameters, - 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, position_constraints); - } - self.compute_nongrouped_constraints(params, bodies, manifolds, position_constraints); - #[cfg(feature = "simd-is-enabled")] - { - self.compute_grouped_ground_constraints( - params, - bodies, - manifolds, - position_constraints, - ); - } - self.compute_nongrouped_ground_constraints(params, bodies, manifolds, position_constraints); - } - - #[cfg(feature = "simd-is-enabled")] - fn compute_grouped_constraints( - &mut self, - params: &IntegrationParameters, - bodies: &RigidBodySet, - manifolds_all: &[&mut ContactManifold], - constraints_all: &mut Vec, - ) { - 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.constraints, - true, - ); - WPositionConstraint::generate(params, manifolds, bodies, constraints_all, true); - } - } - - fn compute_nongrouped_constraints( - &mut self, - 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]; - VelocityConstraint::generate( - params, - *manifold_i, - manifold, - bodies, - &mut self.constraints, - true, - ); - PositionConstraint::generate(params, manifold, bodies, constraints_all, true); - } - } - - #[cfg(feature = "simd-is-enabled")] - fn compute_grouped_ground_constraints( - &mut self, - params: &IntegrationParameters, - bodies: &RigidBodySet, - manifolds_all: &[&mut ContactManifold], - constraints_all: &mut Vec, - ) { - 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.constraints, - true, - ); - WPositionGroundConstraint::generate(params, manifolds, bodies, constraints_all, true); - } - } - - fn compute_nongrouped_ground_constraints( - &mut self, - 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]; - VelocityGroundConstraint::generate( - params, - *manifold_i, - manifold, - bodies, - &mut self.constraints, - true, - ); - PositionGroundConstraint::generate(params, manifold, bodies, constraints_all, true) - } - } -} - -impl VelocitySolverPart { - pub fn init_constraints( - &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.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.constraints.push(vel_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.constraints.push(vel_constraint); - } - } - - 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.constraints.push(vel_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.constraints.push(vel_constraint); - } - } -} -- 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_solver.rs | 17 +++-------------- 1 file changed, 3 insertions(+), 14 deletions(-) (limited to 'src/dynamics/solver/velocity_solver.rs') diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index 89f2809..332d809 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -1,20 +1,9 @@ -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 super::AnyJointVelocityConstraint; use crate::dynamics::{ solver::{AnyVelocityConstraint, DeltaVel}, - IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, + IntegrationParameters, JointGraphEdge, RigidBodySet, }; -use crate::geometry::{ContactManifold, ContactManifoldIndex}; -#[cfg(feature = "simd-is-enabled")] -use crate::math::SIMD_WIDTH; +use crate::geometry::ContactManifold; use crate::utils::WAngularInertia; pub(crate) struct VelocitySolver { -- 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_solver.rs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src/dynamics/solver/velocity_solver.rs') diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index 332d809..dfb97b0 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -4,10 +4,11 @@ use crate::dynamics::{ IntegrationParameters, JointGraphEdge, RigidBodySet, }; use crate::geometry::ContactManifold; +use crate::math::Real; use crate::utils::WAngularInertia; pub(crate) struct VelocitySolver { - pub mj_lambdas: Vec>, + pub mj_lambdas: Vec>, } impl VelocitySolver { -- 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_solver.rs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) (limited to 'src/dynamics/solver/velocity_solver.rs') diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index dfb97b0..89cf34e 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -60,7 +60,9 @@ impl VelocitySolver { bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { let dvel = self.mj_lambdas[rb.active_set_offset]; rb.linvel += dvel.linear; - rb.angvel += rb.world_inv_inertia_sqrt.transform_vector(dvel.angular); + rb.angvel += rb + .effective_world_inv_inertia_sqrt + .transform_vector(dvel.angular); }); // Write impulses back into the manifold structures. -- cgit