nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Refactor the constraints solver code.

Browse Source
This commit is contained in:
Crozet Sébastien
2020-12-31 10:32:34 +01:00
parent 348a339fe3
commit 64507a68e1
6 changed files with 435 additions and 651 deletions
Download Patch File Download Diff File Expand all files Collapse all files

269
src/dynamics/solver/position_solver.rs
View File

@@ -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))
}
}
}
}