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/position_solver.rs | 60 ++-------------------------------- 1 file changed, 3 insertions(+), 57 deletions(-) (limited to 'src/dynamics/solver/position_solver.rs') 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, -- 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_solver.rs | 85 +--------------------------------- 1 file changed, 1 insertion(+), 84 deletions(-) (limited to 'src/dynamics/solver/position_solver.rs') diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs index d610f3a..a0223a0 100644 --- a/src/dynamics/solver/position_solver.rs +++ b/src/dynamics/solver/position_solver.rs @@ -3,7 +3,7 @@ use super::{ }; #[cfg(feature = "simd-is-enabled")] use super::{WPositionConstraint, WPositionGroundConstraint}; -use crate::dynamics::solver::categorization::{categorize_joints, categorize_position_contacts}; +use crate::dynamics::solver::categorization::categorize_joints; use crate::dynamics::{ solver::AnyPositionConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, }; @@ -77,8 +77,6 @@ impl PositionSolver { joints: &[JointGraphEdge], joint_constraint_indices: &[JointIndex], ) { - // self.part - // .init_constraints(island_id, params, bodies, manifolds, manifold_indices); self.joint_part.init_constraints( island_id, params, @@ -117,87 +115,6 @@ impl PositionSolver { } } -impl PositionSolverPart { - pub fn init_constraints( - &mut self, - island_id: usize, - params: &IntegrationParameters, - bodies: &RigidBodySet, - manifolds_all: &[&mut ContactManifold], - manifold_indices: &[ContactManifoldIndex], - ) { - self.plane_point_ground_manifolds.clear(); - self.plane_point_manifolds.clear(); - categorize_position_contacts( - bodies, - manifolds_all, - manifold_indices, - &mut self.plane_point_ground_manifolds, - &mut self.plane_point_manifolds, - ); - - self.plane_point_groups.clear_groups(); - self.plane_point_groups.group_manifolds( - island_id, - bodies, - manifolds_all, - &self.plane_point_manifolds, - ); - - self.plane_point_ground_groups.clear_groups(); - self.plane_point_ground_groups.group_manifolds( - island_id, - bodies, - manifolds_all, - &self.plane_point_ground_manifolds, - ); - - self.constraints.clear(); - - /* - * Init non-ground contact constraints. - */ - #[cfg(feature = "simd-is-enabled")] - { - compute_grouped_constraints( - params, - bodies, - manifolds_all, - &self.plane_point_groups.grouped_interactions, - &mut self.constraints, - ); - } - compute_nongrouped_constraints( - params, - bodies, - manifolds_all, - &self.plane_point_groups.nongrouped_interactions, - &mut self.constraints, - ); - - /* - * Init ground contact constraints. - */ - #[cfg(feature = "simd-is-enabled")] - { - compute_grouped_ground_constraints( - params, - bodies, - manifolds_all, - &self.plane_point_ground_groups.grouped_interactions, - &mut self.constraints, - ); - } - compute_nongrouped_ground_constraints( - params, - bodies, - manifolds_all, - &self.plane_point_ground_groups.nongrouped_interactions, - &mut self.constraints, - ); - } -} - impl PositionSolverJointPart { pub fn init_constraints( &mut self, -- 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/position_solver.rs | 269 +-------------------------------- 1 file changed, 5 insertions(+), 264 deletions(-) (limited to 'src/dynamics/solver/position_solver.rs') diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs index a0223a0..b5a953f 100644 --- a/src/dynamics/solver/position_solver.rs +++ b/src/dynamics/solver/position_solver.rs @@ -12,85 +12,24 @@ use crate::math::Isometry; #[cfg(feature = "simd-is-enabled")] use crate::math::SIMD_WIDTH; -pub(crate) struct PositionSolverJointPart { - pub nonground_joints: Vec, - pub ground_joints: Vec, - pub nonground_joint_groups: InteractionGroups, - pub ground_joint_groups: InteractionGroups, - pub constraints: Vec, -} - -impl PositionSolverJointPart { - pub fn new() -> Self { - Self { - nonground_joints: Vec::new(), - ground_joints: Vec::new(), - nonground_joint_groups: InteractionGroups::new(), - ground_joint_groups: InteractionGroups::new(), - constraints: Vec::new(), - } - } -} - -pub(crate) struct PositionSolverPart { - pub plane_point_manifolds: Vec, - pub plane_point_ground_manifolds: Vec, - pub plane_point_groups: InteractionGroups, - pub plane_point_ground_groups: InteractionGroups, - pub constraints: Vec, -} - -impl PositionSolverPart { - pub fn new() -> Self { - Self { - plane_point_manifolds: Vec::new(), - plane_point_ground_manifolds: Vec::new(), - plane_point_groups: InteractionGroups::new(), - plane_point_ground_groups: InteractionGroups::new(), - constraints: Vec::new(), - } - } -} - pub(crate) struct PositionSolver { positions: Vec>, - pub part: PositionSolverPart, - joint_part: PositionSolverJointPart, } impl PositionSolver { pub fn new() -> Self { Self { positions: Vec::new(), - part: PositionSolverPart::new(), - joint_part: PositionSolverJointPart::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], - ) { - 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, + contact_constraints: &[AnyPositionConstraint], + joint_constraints: &[AnyJointPositionConstraint], ) { self.positions.clear(); self.positions.extend( @@ -100,11 +39,11 @@ impl PositionSolver { ); for _ in 0..params.max_position_iterations { - for constraint in &self.joint_part.constraints { + for constraint in joint_constraints { constraint.solve(params, &mut self.positions) } - for constraint in &self.part.constraints { + for constraint in contact_constraints { constraint.solve(params, &mut self.positions) } } @@ -114,201 +53,3 @@ impl PositionSolver { }); } } - -impl PositionSolverJointPart { - pub fn init_constraints( - &mut self, - island_id: usize, - params: &IntegrationParameters, - bodies: &RigidBodySet, - joints: &[JointGraphEdge], - joint_constraint_indices: &[JointIndex], - ) { - self.ground_joints.clear(); - self.nonground_joints.clear(); - categorize_joints( - bodies, - joints, - joint_constraint_indices, - &mut self.ground_joints, - &mut self.nonground_joints, - ); - - self.nonground_joint_groups.clear_groups(); - self.nonground_joint_groups - .group_joints(island_id, bodies, joints, &self.nonground_joints); - - self.ground_joint_groups.clear_groups(); - self.ground_joint_groups - .group_joints(island_id, bodies, joints, &self.ground_joints); - - /* - * Init ground joint constraints. - */ - self.constraints.clear(); - compute_nongrouped_joint_ground_constraints( - params, - bodies, - joints, - &self.ground_joint_groups.nongrouped_interactions, - &mut self.constraints, - ); - - #[cfg(feature = "simd-is-enabled")] - { - compute_grouped_joint_ground_constraints( - params, - bodies, - joints, - &self.ground_joint_groups.grouped_interactions, - &mut self.constraints, - ); - } - - /* - * Init non-ground joint constraints. - */ - compute_nongrouped_joint_constraints( - params, - bodies, - joints, - &self.nonground_joint_groups.nongrouped_interactions, - &mut self.constraints, - ); - - #[cfg(feature = "simd-is-enabled")] - { - compute_grouped_joint_constraints( - params, - bodies, - joints, - &self.nonground_joint_groups.grouped_interactions, - &mut self.constraints, - ); - } - } -} - -fn compute_nongrouped_constraints( - params: &IntegrationParameters, - bodies: &RigidBodySet, - manifolds_all: &[&mut ContactManifold], - manifold_indices: &[ContactManifoldIndex], - output: &mut Vec, -) { - for manifold in manifold_indices.iter().map(|i| &manifolds_all[*i]) { - PositionConstraint::generate(params, manifold, bodies, output, true) - } -} - -#[cfg(feature = "simd-is-enabled")] -fn compute_grouped_constraints( - params: &IntegrationParameters, - bodies: &RigidBodySet, - manifolds_all: &[&mut ContactManifold], - manifold_indices: &[ContactManifoldIndex], - output: &mut Vec, -) { - for manifolds_i in manifold_indices.chunks_exact(SIMD_WIDTH) { - let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH]; - WPositionConstraint::generate(params, manifolds, bodies, output, true) - } -} - -fn compute_nongrouped_ground_constraints( - params: &IntegrationParameters, - bodies: &RigidBodySet, - manifolds_all: &[&mut ContactManifold], - manifold_indices: &[ContactManifoldIndex], - output: &mut Vec, -) { - for manifold in manifold_indices.iter().map(|i| &manifolds_all[*i]) { - PositionGroundConstraint::generate(params, manifold, bodies, output, true) - } -} - -#[cfg(feature = "simd-is-enabled")] -fn compute_grouped_ground_constraints( - params: &IntegrationParameters, - bodies: &RigidBodySet, - manifolds_all: &[&mut ContactManifold], - manifold_indices: &[ContactManifoldIndex], - output: &mut Vec, -) { - for manifolds_i in manifold_indices.chunks_exact(SIMD_WIDTH) { - let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH]; - WPositionGroundConstraint::generate(params, manifolds, bodies, output, true); - } -} - -fn compute_nongrouped_joint_ground_constraints( - _params: &IntegrationParameters, - bodies: &RigidBodySet, - joints_all: &[JointGraphEdge], - joint_indices: &[JointIndex], - output: &mut Vec, -) { - for joint_i in joint_indices { - let joint = &joints_all[*joint_i].weight; - let pos_constraint = AnyJointPositionConstraint::from_joint_ground(joint, bodies); - output.push(pos_constraint); - } -} - -#[cfg(feature = "simd-is-enabled")] -fn compute_grouped_joint_ground_constraints( - _params: &IntegrationParameters, - bodies: &RigidBodySet, - joints_all: &[JointGraphEdge], - joint_indices: &[JointIndex], - output: &mut Vec, -) { - for joint_i in joint_indices.chunks_exact(SIMD_WIDTH) { - let joints = array![|ii| &joints_all[joint_i[ii]].weight; SIMD_WIDTH]; - if let Some(pos_constraint) = - AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies) - { - output.push(pos_constraint); - } else { - for joint in joints.iter() { - output.push(AnyJointPositionConstraint::from_joint_ground( - *joint, bodies, - )) - } - } - } -} - -fn compute_nongrouped_joint_constraints( - _params: &IntegrationParameters, - bodies: &RigidBodySet, - joints_all: &[JointGraphEdge], - joint_indices: &[JointIndex], - output: &mut Vec, -) { - for joint_i in joint_indices { - let joint = &joints_all[*joint_i]; - let pos_constraint = AnyJointPositionConstraint::from_joint(&joint.weight, bodies); - output.push(pos_constraint); - } -} - -#[cfg(feature = "simd-is-enabled")] -fn compute_grouped_joint_constraints( - _params: &IntegrationParameters, - bodies: &RigidBodySet, - joints_all: &[JointGraphEdge], - joint_indices: &[JointIndex], - output: &mut Vec, -) { - for joint_i in joint_indices.chunks_exact(SIMD_WIDTH) { - let joints = array![|ii| &joints_all[joint_i[ii]].weight; SIMD_WIDTH]; - if let Some(pos_constraint) = AnyJointPositionConstraint::from_wide_joint(joints, bodies) { - output.push(pos_constraint); - } else { - for joint in joints.iter() { - output.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/position_solver.rs | 14 ++------------ 1 file changed, 2 insertions(+), 12 deletions(-) (limited to 'src/dynamics/solver/position_solver.rs') diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs index b5a953f..cf23168 100644 --- a/src/dynamics/solver/position_solver.rs +++ b/src/dynamics/solver/position_solver.rs @@ -1,16 +1,6 @@ -use super::{ - AnyJointPositionConstraint, InteractionGroups, PositionConstraint, PositionGroundConstraint, -}; -#[cfg(feature = "simd-is-enabled")] -use super::{WPositionConstraint, WPositionGroundConstraint}; -use crate::dynamics::solver::categorization::categorize_joints; -use crate::dynamics::{ - solver::AnyPositionConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, -}; -use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use super::AnyJointPositionConstraint; +use crate::dynamics::{solver::AnyPositionConstraint, IntegrationParameters, RigidBodySet}; use crate::math::Isometry; -#[cfg(feature = "simd-is-enabled")] -use crate::math::SIMD_WIDTH; pub(crate) struct PositionSolver { positions: 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/position_solver.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/dynamics/solver/position_solver.rs') diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs index cf23168..df0e3fc 100644 --- a/src/dynamics/solver/position_solver.rs +++ b/src/dynamics/solver/position_solver.rs @@ -1,9 +1,9 @@ use super::AnyJointPositionConstraint; use crate::dynamics::{solver::AnyPositionConstraint, IntegrationParameters, RigidBodySet}; -use crate::math::Isometry; +use crate::math::{Isometry, Real}; pub(crate) struct PositionSolver { - positions: Vec>, + positions: Vec>, } impl PositionSolver { -- cgit