diff options
Diffstat (limited to 'src/dynamics/solver/position_solver.rs')
| -rw-r--r-- | src/dynamics/solver/position_solver.rs | 269 |
1 files changed, 5 insertions, 264 deletions
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<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 plane_point_manifolds: Vec<ContactManifoldIndex>, - pub plane_point_ground_manifolds: Vec<ContactManifoldIndex>, - pub plane_point_groups: InteractionGroups, - pub plane_point_ground_groups: InteractionGroups, - pub constraints: Vec<AnyPositionConstraint>, -} - -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<Isometry<f32>>, - 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<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)) - } - } - } -} |