From 754a48b7ff6d8c58b1ee08651e60112900b60455 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Tue, 25 Aug 2020 22:10:25 +0200 Subject: First public release of Rapier. --- src/dynamics/solver/position_solver.rs | 451 +++++++++++++++++++++++++++++++++ 1 file changed, 451 insertions(+) create mode 100644 src/dynamics/solver/position_solver.rs (limited to 'src/dynamics/solver/position_solver.rs') diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs new file mode 100644 index 0000000..2cfd629 --- /dev/null +++ b/src/dynamics/solver/position_solver.rs @@ -0,0 +1,451 @@ +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, + 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 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, +} + +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(), + } + } +} + +pub(crate) struct PositionSolver { + positions: Vec>, + 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.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( + &mut self, + island_id: usize, + params: &IntegrationParameters, + bodies: &mut RigidBodySet, + ) { + self.positions.clear(); + self.positions.extend( + bodies + .iter_active_island(island_id) + .map(|(_, b)| b.position), + ); + + for _ in 0..params.max_position_iterations { + for constraint in &self.joint_part.constraints { + constraint.solve(params, &mut self.positions) + } + + for constraint in &self.part.constraints { + constraint.solve(params, &mut self.positions) + } + } + + bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { + rb.set_position(self.positions[rb.active_set_offset]) + }); + } +} + +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, +) { + 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