diff options
Diffstat (limited to 'src/dynamics/solver/position_solver.rs')
| -rw-r--r-- | src/dynamics/solver/position_solver.rs | 424 |
1 files changed, 9 insertions, 415 deletions
diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs index be70d74..df0e3fc 100644 --- a/src/dynamics/solver/position_solver.rs +++ b/src/dynamics/solver/position_solver.rs @@ -1,106 +1,25 @@ -use super::{ - AnyJointPositionConstraint, InteractionGroups, PositionConstraint, PositionGroundConstraint, -}; -#[cfg(feature = "simd-is-enabled")] -use super::{WPositionConstraint, WPositionGroundConstraint}; -use crate::dynamics::solver::categorization::{categorize_joints, categorize_position_contacts}; -use crate::dynamics::{ - solver::AnyPositionConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, -}; -use crate::geometry::{ContactManifold, ContactManifoldIndex}; -use crate::math::Isometry; -#[cfg(feature = "simd-is-enabled")] -use crate::math::SIMD_WIDTH; - -pub(crate) struct PositionSolverJointPart { - pub nonground_joints: Vec<JointIndex>, - pub ground_joints: Vec<JointIndex>, - pub nonground_joint_groups: InteractionGroups, - pub ground_joint_groups: InteractionGroups, - pub constraints: Vec<AnyJointPositionConstraint>, -} - -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 point_point_manifolds: Vec<ContactManifoldIndex>, - pub plane_point_manifolds: Vec<ContactManifoldIndex>, - pub point_point_ground_manifolds: Vec<ContactManifoldIndex>, - pub plane_point_ground_manifolds: Vec<ContactManifoldIndex>, - 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<AnyPositionConstraint>, -} - -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(), - } - } -} +use super::AnyJointPositionConstraint; +use crate::dynamics::{solver::AnyPositionConstraint, IntegrationParameters, RigidBodySet}; +use crate::math::{Isometry, Real}; pub(crate) struct PositionSolver { - positions: Vec<Isometry<f32>>, - part: PositionSolverPart, - joint_part: PositionSolverJointPart, + positions: Vec<Isometry<Real>>, } 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.part - .init_constraints(island_id, params, bodies, manifolds, manifold_indices); - 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( @@ -110,11 +29,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) } } @@ -124,328 +43,3 @@ 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.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, - bodies, - manifolds_all, - &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, - 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.point_point_groups.grouped_interactions, - &mut self.constraints, - ); - compute_grouped_constraints( - params, - bodies, - manifolds_all, - &self.plane_point_groups.grouped_interactions, - &mut self.constraints, - ); - } - compute_nongrouped_constraints( - params, - bodies, - manifolds_all, - &self.point_point_groups.nongrouped_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.point_point_ground_groups.grouped_interactions, - &mut self.constraints, - ); - 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.point_point_ground_groups.nongrouped_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, - 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<AnyPositionConstraint>, -) { - 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<AnyPositionConstraint>, -) { - 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<AnyPositionConstraint>, -) { - 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<AnyPositionConstraint>, -) { - 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<AnyJointPositionConstraint>, -) { - 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<AnyJointPositionConstraint>, -) { - 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<AnyJointPositionConstraint>, -) { - 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<AnyJointPositionConstraint>, -) { - 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)) - } - } - } -} |