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

Refactor the parallel solver code the same way we did with the non-parallel solver.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-01-04 17:59:51 +01:00
parent aa61fe65e3
commit c28b14d31c
14 changed files with 691 additions and 873 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/dynamics/solver/interaction_groups.rs
View File

@@ -77,7 +77,7 @@ impl ParallelInteractionGroups {
.iter()
.zip(self.interaction_colors.iter_mut())
{
let body_pair = interactions[*interaction_id].data.body_pair();
let body_pair = interactions[*interaction_id].body_pair();
let rb1 = &bodies[body_pair.body1];
let rb2 = &bodies[body_pair.body2];

53
src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs Normal file
View File

@@ -0,0 +1,53 @@
use super::{FixedPositionConstraint, FixedPositionGroundConstraint};
use crate::dynamics::{FixedJoint, IntegrationParameters, JointIndex, RigidBody};
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector, SIMD_WIDTH};
use crate::utils::WAngularInertia;
use na::Unit;
// TODO: this does not uses SIMD optimizations yet.
#[derive(Debug)]
pub(crate) struct WFixedPositionConstraint {
constraints: [FixedPositionConstraint; SIMD_WIDTH],
}
impl WFixedPositionConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
cparams: [&FixedJoint; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| FixedPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
for constraint in &self.constraints {
constraint.solve(params, positions);
}
}
}
#[derive(Debug)]
pub(crate) struct WFixedPositionGroundConstraint {
constraints: [FixedPositionGroundConstraint; SIMD_WIDTH],
}
impl WFixedPositionGroundConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
cparams: [&FixedJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| FixedPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
for constraint in &self.constraints {
constraint.solve(params, positions);
}
}
}

94
src/dynamics/solver/joint_constraint/joint_position_constraint.rs
View File

@@ -4,8 +4,15 @@ use super::{
};
#[cfg(feature = "dim3")]
use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
use super::{WRevolutePositionConstraint, WRevolutePositionGroundConstraint};
#[cfg(feature = "simd-is-enabled")]
use super::{WBallPositionConstraint, WBallPositionGroundConstraint};
use super::{
WBallPositionConstraint, WBallPositionGroundConstraint, WFixedPositionConstraint,
WFixedPositionGroundConstraint, WPrismaticPositionConstraint,
WPrismaticPositionGroundConstraint,
};
use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet};
#[cfg(feature = "simd-is-enabled")]
use crate::math::SIMD_WIDTH;
@@ -20,12 +27,24 @@ pub(crate) enum AnyJointPositionConstraint {
WBallGroundConstraint(WBallPositionGroundConstraint),
FixedJoint(FixedPositionConstraint),
FixedGroundConstraint(FixedPositionGroundConstraint),
#[cfg(feature = "simd-is-enabled")]
WFixedJoint(WFixedPositionConstraint),
#[cfg(feature = "simd-is-enabled")]
WFixedGroundConstraint(WFixedPositionGroundConstraint),
PrismaticJoint(PrismaticPositionConstraint),
PrismaticGroundConstraint(PrismaticPositionGroundConstraint),
#[cfg(feature = "simd-is-enabled")]
WPrismaticJoint(WPrismaticPositionConstraint),
#[cfg(feature = "simd-is-enabled")]
WPrismaticGroundConstraint(WPrismaticPositionGroundConstraint),
#[cfg(feature = "dim3")]
RevoluteJoint(RevolutePositionConstraint),
#[cfg(feature = "dim3")]
RevoluteGroundConstraint(RevolutePositionGroundConstraint),
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
WRevoluteJoint(WRevolutePositionConstraint),
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
WRevoluteGroundConstraint(WRevolutePositionGroundConstraint),
#[allow(dead_code)] // The Empty variant is only used with parallel code.
Empty,
}
@@ -71,21 +90,38 @@ impl AnyJointPositionConstraint {
}
#[cfg(feature = "simd-is-enabled")]
pub fn from_wide_joint(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Option<Self> {
pub fn from_wide_joint(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Self {
let rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
let rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
match &joints[0].params {
JointParams::BallJoint(_) => {
let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
Some(AnyJointPositionConstraint::WBallJoint(
WBallPositionConstraint::from_params(rbs1, rbs2, joints),
AnyJointPositionConstraint::WBallJoint(WBallPositionConstraint::from_params(
rbs1, rbs2, joints,
))
}
JointParams::FixedJoint(_) => None,
JointParams::PrismaticJoint(_) => None,
JointParams::FixedJoint(_) => {
let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
AnyJointPositionConstraint::WFixedJoint(WFixedPositionConstraint::from_params(
rbs1, rbs2, joints,
))
}
JointParams::PrismaticJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
AnyJointPositionConstraint::WPrismaticJoint(
WPrismaticPositionConstraint::from_params(rbs1, rbs2, joints),
)
}
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(_) => None,
JointParams::RevoluteJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
AnyJointPositionConstraint::WRevoluteJoint(
WRevolutePositionConstraint::from_params(rbs1, rbs2, joints),
)
}
}
}
@@ -118,10 +154,7 @@ impl AnyJointPositionConstraint {
}
#[cfg(feature = "simd-is-enabled")]
pub fn from_wide_joint_ground(
joints: [&Joint; SIMD_WIDTH],
bodies: &RigidBodySet,
) -> Option<Self> {
pub fn from_wide_joint_ground(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Self {
let mut rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
let mut rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
let mut flipped = [false; SIMD_WIDTH];
@@ -136,14 +169,31 @@ impl AnyJointPositionConstraint {
match &joints[0].params {
JointParams::BallJoint(_) => {
let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
Some(AnyJointPositionConstraint::WBallGroundConstraint(
AnyJointPositionConstraint::WBallGroundConstraint(
WBallPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
))
)
}
JointParams::FixedJoint(_) => {
let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
AnyJointPositionConstraint::WFixedGroundConstraint(
WFixedPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
)
}
JointParams::PrismaticJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
AnyJointPositionConstraint::WPrismaticGroundConstraint(
WPrismaticPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
)
}
JointParams::FixedJoint(_) => None,
JointParams::PrismaticJoint(_) => None,
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(_) => None,
JointParams::RevoluteJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
AnyJointPositionConstraint::WRevoluteGroundConstraint(
WRevolutePositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
)
}
}
}
@@ -157,12 +207,24 @@ impl AnyJointPositionConstraint {
AnyJointPositionConstraint::WBallGroundConstraint(c) => c.solve(params, positions),
AnyJointPositionConstraint::FixedJoint(c) => c.solve(params, positions),
AnyJointPositionConstraint::FixedGroundConstraint(c) => c.solve(params, positions),
#[cfg(feature = "simd-is-enabled")]
AnyJointPositionConstraint::WFixedJoint(c) => c.solve(params, positions),
#[cfg(feature = "simd-is-enabled")]
AnyJointPositionConstraint::WFixedGroundConstraint(c) => c.solve(params, positions),
AnyJointPositionConstraint::PrismaticJoint(c) => c.solve(params, positions),
AnyJointPositionConstraint::PrismaticGroundConstraint(c) => c.solve(params, positions),
#[cfg(feature = "simd-is-enabled")]
AnyJointPositionConstraint::WPrismaticJoint(c) => c.solve(params, positions),
#[cfg(feature = "simd-is-enabled")]
AnyJointPositionConstraint::WPrismaticGroundConstraint(c) => c.solve(params, positions),
#[cfg(feature = "dim3")]
AnyJointPositionConstraint::RevoluteJoint(c) => c.solve(params, positions),
#[cfg(feature = "dim3")]
AnyJointPositionConstraint::RevoluteGroundConstraint(c) => c.solve(params, positions),
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
AnyJointPositionConstraint::WRevoluteJoint(c) => c.solve(params, positions),
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
AnyJointPositionConstraint::WRevoluteGroundConstraint(c) => c.solve(params, positions),
AnyJointPositionConstraint::Empty => unreachable!(),
}
}

24
src/dynamics/solver/joint_constraint/mod.rs
View File

@@ -9,6 +9,10 @@ pub(self) use ball_velocity_constraint_wide::{
WBallVelocityConstraint, WBallVelocityGroundConstraint,
};
pub(self) use fixed_position_constraint::{FixedPositionConstraint, FixedPositionGroundConstraint};
#[cfg(feature = "simd-is-enabled")]
pub(self) use fixed_position_constraint_wide::{
WFixedPositionConstraint, WFixedPositionGroundConstraint,
};
pub(self) use fixed_velocity_constraint::{FixedVelocityConstraint, FixedVelocityGroundConstraint};
#[cfg(feature = "simd-is-enabled")]
pub(self) use fixed_velocity_constraint_wide::{
@@ -19,6 +23,10 @@ pub(crate) use joint_position_constraint::AnyJointPositionConstraint;
pub(self) use prismatic_position_constraint::{
PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
};
#[cfg(feature = "simd-is-enabled")]
pub(self) use prismatic_position_constraint_wide::{
WPrismaticPositionConstraint, WPrismaticPositionGroundConstraint,
};
pub(self) use prismatic_velocity_constraint::{
PrismaticVelocityConstraint, PrismaticVelocityGroundConstraint,
};
@@ -30,12 +38,15 @@ pub(self) use prismatic_velocity_constraint_wide::{
pub(self) use revolute_position_constraint::{
RevolutePositionConstraint, RevolutePositionGroundConstraint,
};
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
pub(self) use revolute_position_constraint_wide::{
WRevolutePositionConstraint, WRevolutePositionGroundConstraint,
};
#[cfg(feature = "dim3")]
pub(self) use revolute_velocity_constraint::{
RevoluteVelocityConstraint, RevoluteVelocityGroundConstraint,
};
#[cfg(feature = "dim3")]
#[cfg(feature = "simd-is-enabled")]
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
pub(self) use revolute_velocity_constraint_wide::{
WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint,
};
@@ -47,19 +58,24 @@ mod ball_velocity_constraint;
#[cfg(feature = "simd-is-enabled")]
mod ball_velocity_constraint_wide;
mod fixed_position_constraint;
#[cfg(feature = "simd-is-enabled")]
mod fixed_position_constraint_wide;
mod fixed_velocity_constraint;
#[cfg(feature = "simd-is-enabled")]
mod fixed_velocity_constraint_wide;
mod joint_constraint;
mod joint_position_constraint;
mod prismatic_position_constraint;
#[cfg(feature = "simd-is-enabled")]
mod prismatic_position_constraint_wide;
mod prismatic_velocity_constraint;
#[cfg(feature = "simd-is-enabled")]
mod prismatic_velocity_constraint_wide;
#[cfg(feature = "dim3")]
mod revolute_position_constraint;
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
mod revolute_position_constraint_wide;
#[cfg(feature = "dim3")]
mod revolute_velocity_constraint;
#[cfg(feature = "dim3")]
#[cfg(feature = "simd-is-enabled")]
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
mod revolute_velocity_constraint_wide;

53
src/dynamics/solver/joint_constraint/prismatic_position_constraint_wide.rs Normal file
View File

@@ -0,0 +1,53 @@
use super::{PrismaticPositionConstraint, PrismaticPositionGroundConstraint};
use crate::dynamics::{IntegrationParameters, JointIndex, PrismaticJoint, RigidBody};
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector, SIMD_WIDTH};
use crate::utils::WAngularInertia;
use na::Unit;
// TODO: this does not uses SIMD optimizations yet.
#[derive(Debug)]
pub(crate) struct WPrismaticPositionConstraint {
constraints: [PrismaticPositionConstraint; SIMD_WIDTH],
}
impl WPrismaticPositionConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
cparams: [&PrismaticJoint; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| PrismaticPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
for constraint in &self.constraints {
constraint.solve(params, positions);
}
}
}
#[derive(Debug)]
pub(crate) struct WPrismaticPositionGroundConstraint {
constraints: [PrismaticPositionGroundConstraint; SIMD_WIDTH],
}
impl WPrismaticPositionGroundConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
cparams: [&PrismaticJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| PrismaticPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
for constraint in &self.constraints {
constraint.solve(params, positions);
}
}
}

53
src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs Normal file
View File

@@ -0,0 +1,53 @@
use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
use crate::dynamics::{IntegrationParameters, JointIndex, RevoluteJoint, RigidBody};
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector, SIMD_WIDTH};
use crate::utils::WAngularInertia;
use na::Unit;
// TODO: this does not uses SIMD optimizations yet.
#[derive(Debug)]
pub(crate) struct WRevolutePositionConstraint {
constraints: [RevolutePositionConstraint; SIMD_WIDTH],
}
impl WRevolutePositionConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
cparams: [&RevoluteJoint; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| RevolutePositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
for constraint in &self.constraints {
constraint.solve(params, positions);
}
}
}
#[derive(Debug)]
pub(crate) struct WRevolutePositionGroundConstraint {
constraints: [RevolutePositionGroundConstraint; SIMD_WIDTH],
}
impl WRevolutePositionGroundConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
cparams: [&RevoluteJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| RevolutePositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
for constraint in &self.constraints {
constraint.solve(params, positions);
}
}
}

4
src/dynamics/solver/mod.rs
View File

@@ -5,6 +5,8 @@ pub(crate) use self::parallel_island_solver::{ParallelIslandSolver, ThreadContex
#[cfg(feature = "parallel")]
pub(self) use self::parallel_position_solver::ParallelPositionSolver;
#[cfg(feature = "parallel")]
pub(self) use self::parallel_solver_constraints::ParallelSolverConstraints;
#[cfg(feature = "parallel")]
pub(self) use self::parallel_velocity_solver::ParallelVelocitySolver;
#[cfg(not(feature = "parallel"))]
pub(self) use self::position_solver::PositionSolver;
@@ -39,6 +41,8 @@ mod parallel_island_solver;
#[cfg(feature = "parallel")]
mod parallel_position_solver;
#[cfg(feature = "parallel")]
mod parallel_solver_constraints;
#[cfg(feature = "parallel")]
mod parallel_velocity_solver;
mod position_constraint;
#[cfg(feature = "simd-is-enabled")]

73
src/dynamics/solver/parallel_island_solver.rs
View File

@@ -1,5 +1,8 @@
use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver};
use crate::dynamics::solver::ParallelPositionSolver;
use crate::dynamics::solver::{
AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint,
AnyVelocityConstraint, ParallelPositionSolver, ParallelSolverConstraints,
};
use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Isometry, Real};
@@ -123,8 +126,10 @@ pub struct ParallelIslandSolver {
positions: Vec<Isometry<Real>>,
parallel_groups: ParallelInteractionGroups,
parallel_joint_groups: ParallelInteractionGroups,
parallel_velocity_solver: ParallelVelocitySolver,
parallel_position_solver: ParallelPositionSolver,
parallel_contact_constraints:
ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint>,
parallel_joint_constraints:
ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint>,
thread: ThreadContext,
}
@@ -135,8 +140,8 @@ impl ParallelIslandSolver {
positions: Vec::new(),
parallel_groups: ParallelInteractionGroups::new(),
parallel_joint_groups: ParallelInteractionGroups::new(),
parallel_velocity_solver: ParallelVelocitySolver::new(),
parallel_position_solver: ParallelPositionSolver::new(),
parallel_contact_constraints: ParallelSolverConstraints::new(),
parallel_joint_constraints: ParallelSolverConstraints::new(),
thread: ThreadContext::new(8),
}
}
@@ -153,25 +158,21 @@ impl ParallelIslandSolver {
joint_indices: &[JointIndex],
) {
let num_threads = rayon::current_num_threads();
let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island?
let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island?
self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here?
self.parallel_groups
.group_interactions(island_id, bodies, manifolds, manifold_indices);
self.parallel_joint_groups
.group_interactions(island_id, bodies, joints, joint_indices);
self.parallel_velocity_solver.init_constraint_groups(
self.parallel_contact_constraints.init_constraint_groups(
island_id,
bodies,
manifolds,
&self.parallel_groups,
joints,
&self.parallel_joint_groups,
);
self.parallel_position_solver.init_constraint_groups(
self.parallel_joint_constraints.init_constraint_groups(
island_id,
bodies,
manifolds,
&self.parallel_groups,
joints,
&self.parallel_joint_groups,
);
@@ -192,10 +193,10 @@ impl ParallelIslandSolver {
let bodies = std::sync::atomic::AtomicPtr::new(bodies as *mut _);
let manifolds = std::sync::atomic::AtomicPtr::new(manifolds as *mut _);
let joints = std::sync::atomic::AtomicPtr::new(joints as *mut _);
let parallel_velocity_solver =
std::sync::atomic::AtomicPtr::new(&mut self.parallel_velocity_solver as *mut _);
let parallel_position_solver =
std::sync::atomic::AtomicPtr::new(&mut self.parallel_position_solver as *mut _);
let parallel_contact_constraints =
std::sync::atomic::AtomicPtr::new(&mut self.parallel_contact_constraints as *mut _);
let parallel_joint_constraints =
std::sync::atomic::AtomicPtr::new(&mut self.parallel_joint_constraints as *mut _);
scope.spawn(move |_| {
// Transmute *mut -> &mut
@@ -209,18 +210,34 @@ impl ParallelIslandSolver {
unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) };
let joints: &mut Vec<JointGraphEdge> =
unsafe { std::mem::transmute(joints.load(Ordering::Relaxed)) };
let parallel_velocity_solver: &mut ParallelVelocitySolver = unsafe {
std::mem::transmute(parallel_velocity_solver.load(Ordering::Relaxed))
let parallel_contact_constraints: &mut ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> = unsafe {
std::mem::transmute(parallel_contact_constraints.load(Ordering::Relaxed))
};
let parallel_position_solver: &mut ParallelPositionSolver = unsafe {
std::mem::transmute(parallel_position_solver.load(Ordering::Relaxed))
let parallel_joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> = unsafe {
std::mem::transmute(parallel_joint_constraints.load(Ordering::Relaxed))
};
enable_flush_to_zero!(); // Ensure this is enabled on each thread.
parallel_velocity_solver.fill_constraints(&thread, params, bodies, manifolds, joints);
parallel_position_solver.fill_constraints(&thread, params, bodies, manifolds, joints);
parallel_velocity_solver
.solve_constraints(&thread, params, manifolds, joints, mj_lambdas);
parallel_contact_constraints.fill_constraints(&thread, params, bodies, manifolds);
parallel_joint_constraints.fill_constraints(&thread, params, bodies, joints);
ThreadContext::lock_until_ge(
&thread.num_initialized_constraints,
parallel_contact_constraints.constraint_descs.len(),
);
ThreadContext::lock_until_ge(
&thread.num_initialized_joint_constraints,
parallel_joint_constraints.constraint_descs.len(),
);
ParallelVelocitySolver::solve(
&thread,
params,
manifolds,
joints,
mj_lambdas,
parallel_contact_constraints,
parallel_joint_constraints
);
// Write results back to rigid bodies and integrate velocities.
let island_range = bodies.active_island_range(island_id);
@@ -243,7 +260,13 @@ impl ParallelIslandSolver {
// initialized `positions` to the updated values.
ThreadContext::lock_until_ge(&thread.num_integrated_bodies, active_bodies.len());
parallel_position_solver.solve_constraints(&thread, params, positions);
ParallelPositionSolver::solve(
&thread,
params,
positions,
parallel_contact_constraints,
parallel_joint_constraints
);
// Write results back to rigid bodies.
concurrent_loop! {

513
src/dynamics/solver/parallel_position_solver.rs
View File

@@ -1,14 +1,17 @@
use super::ParallelInteractionGroups;
use super::{AnyJointPositionConstraint, AnyPositionConstraint, ThreadContext};
use crate::dynamics::solver::categorization::{categorize_joints, categorize_position_contacts};
use crate::dynamics::solver::{InteractionGroups, PositionConstraint, PositionGroundConstraint};
use crate::dynamics::solver::categorization::categorize_joints;
use crate::dynamics::solver::{
AnyJointVelocityConstraint, AnyVelocityConstraint, InteractionGroups,
ParallelSolverConstraints, PositionConstraint, PositionGroundConstraint,
};
use crate::dynamics::{IntegrationParameters, JointGraphEdge, RigidBodySet};
use crate::geometry::ContactManifold;
use crate::math::{Isometry, Real};
#[cfg(feature = "simd-is-enabled")]
use crate::{
dynamics::solver::{WPositionConstraint, WPositionGroundConstraint},
simd::SIMD_WIDTH,
math::SIMD_WIDTH,
};
use std::sync::atomic::Ordering;
@@ -21,488 +24,25 @@ pub(crate) enum PositionConstraintDesc {
GroundGrouped([usize; SIMD_WIDTH]),
}
pub(crate) struct ParallelPositionSolverContactPart {
pub point_point: Vec<usize>,
pub plane_point: Vec<usize>,
pub ground_point_point: Vec<usize>,
pub ground_plane_point: Vec<usize>,
pub interaction_groups: InteractionGroups,
pub ground_interaction_groups: InteractionGroups,
pub constraints: Vec<AnyPositionConstraint>,
pub constraint_descs: Vec<(usize, PositionConstraintDesc)>,
pub parallel_desc_groups: Vec<usize>,
}
pub(crate) struct ParallelPositionSolverJointPart {
pub not_ground_interactions: Vec<usize>,
pub ground_interactions: Vec<usize>,
pub interaction_groups: InteractionGroups,
pub ground_interaction_groups: InteractionGroups,
pub constraints: Vec<AnyJointPositionConstraint>,
pub constraint_descs: Vec<(usize, PositionConstraintDesc)>,
pub parallel_desc_groups: Vec<usize>,
}
impl ParallelPositionSolverContactPart {
pub fn new() -> Self {
Self {
point_point: Vec::new(),
plane_point: Vec::new(),
ground_point_point: Vec::new(),
ground_plane_point: Vec::new(),
interaction_groups: InteractionGroups::new(),
ground_interaction_groups: InteractionGroups::new(),
constraints: Vec::new(),
constraint_descs: Vec::new(),
parallel_desc_groups: Vec::new(),
}
}
}
impl ParallelPositionSolverJointPart {
pub fn new() -> Self {
Self {
not_ground_interactions: Vec::new(),
ground_interactions: Vec::new(),
interaction_groups: InteractionGroups::new(),
ground_interaction_groups: InteractionGroups::new(),
constraints: Vec::new(),
constraint_descs: Vec::new(),
parallel_desc_groups: Vec::new(),
}
}
}
impl ParallelPositionSolverJointPart {
pub fn init_constraints_groups(
&mut self,
island_id: usize,
bodies: &RigidBodySet,
joints: &mut [JointGraphEdge],
joint_groups: &ParallelInteractionGroups,
) {
let mut total_num_constraints = 0;
let num_groups = joint_groups.num_groups();
self.interaction_groups.clear_groups();
self.ground_interaction_groups.clear_groups();
self.parallel_desc_groups.clear();
self.constraint_descs.clear();
self.parallel_desc_groups.push(0);
for i in 0..num_groups {
let group = joint_groups.group(i);
self.not_ground_interactions.clear();
self.ground_interactions.clear();
categorize_joints(
bodies,
joints,
group,
&mut self.ground_interactions,
&mut self.not_ground_interactions,
);
#[cfg(feature = "simd-is-enabled")]
let start_grouped = self.interaction_groups.grouped_interactions.len();
let start_nongrouped = self.interaction_groups.nongrouped_interactions.len();
#[cfg(feature = "simd-is-enabled")]
let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len();
let start_nongrouped_ground =
self.ground_interaction_groups.nongrouped_interactions.len();
self.interaction_groups.group_joints(
island_id,
bodies,
joints,
&self.not_ground_interactions,
);
self.ground_interaction_groups.group_joints(
island_id,
bodies,
joints,
&self.ground_interactions,
);
// Compute constraint indices.
for interaction_i in
&self.interaction_groups.nongrouped_interactions[start_nongrouped..]
{
let joint = &mut joints[*interaction_i].weight;
joint.position_constraint_index = total_num_constraints;
self.constraint_descs.push((
total_num_constraints,
PositionConstraintDesc::NongroundNongrouped(*interaction_i),
));
total_num_constraints +=
AnyJointPositionConstraint::num_active_constraints(joint, false);
}
#[cfg(feature = "simd-is-enabled")]
for interaction_i in
self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH)
{
let joint = &mut joints[interaction_i[0]].weight;
joint.position_constraint_index = total_num_constraints;
self.constraint_descs.push((
total_num_constraints,
PositionConstraintDesc::NongroundGrouped(
array![|ii| interaction_i[ii]; SIMD_WIDTH],
),
));
total_num_constraints +=
AnyJointPositionConstraint::num_active_constraints(joint, true);
}
for interaction_i in
&self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..]
{
let joint = &mut joints[*interaction_i].weight;
joint.position_constraint_index = total_num_constraints;
self.constraint_descs.push((
total_num_constraints,
PositionConstraintDesc::GroundNongrouped(*interaction_i),
));
total_num_constraints +=
AnyJointPositionConstraint::num_active_constraints(joint, false);
}
#[cfg(feature = "simd-is-enabled")]
for interaction_i in self.ground_interaction_groups.grouped_interactions
[start_grouped_ground..]
.chunks(SIMD_WIDTH)
{
let joint = &mut joints[interaction_i[0]].weight;
joint.position_constraint_index = total_num_constraints;
self.constraint_descs.push((
total_num_constraints,
PositionConstraintDesc::GroundGrouped(
array![|ii| interaction_i[ii]; SIMD_WIDTH],
),
));
total_num_constraints +=
AnyJointPositionConstraint::num_active_constraints(joint, true);
}
self.parallel_desc_groups.push(self.constraint_descs.len());
}
// Resize the constraints set.
self.constraints.clear();
self.constraints
.resize_with(total_num_constraints, || AnyJointPositionConstraint::Empty)
}
fn fill_constraints(
&mut self,
thread: &ThreadContext,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
) {
let descs = &self.constraint_descs;
crate::concurrent_loop! {
let batch_size = thread.batch_size;
for desc in descs[thread.position_joint_constraint_initialization_index, thread.num_initialized_position_joint_constraints] {
match &desc.1 {
PositionConstraintDesc::NongroundNongrouped(joint_id) => {
let joint = &joints_all[*joint_id].weight;
let constraint = AnyJointPositionConstraint::from_joint(
joint,
bodies,
);
self.constraints[joint.position_constraint_index] = constraint;
}
PositionConstraintDesc::GroundNongrouped(joint_id) => {
let joint = &joints_all[*joint_id].weight;
let constraint = AnyJointPositionConstraint::from_joint_ground(
joint,
bodies,
);
self.constraints[joint.position_constraint_index] = constraint;
}
#[cfg(feature = "simd-is-enabled")]
PositionConstraintDesc::NongroundGrouped(joint_id) => {
let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
if let Some(constraint) = AnyJointPositionConstraint::from_wide_joint(
joints, bodies,
) {
self.constraints[joints[0].position_constraint_index] = constraint
} else {
for ii in 0..SIMD_WIDTH {
let constraint = AnyJointPositionConstraint::from_joint(joints[ii], bodies);
self.constraints[joints[0].position_constraint_index + ii] = constraint;
}
}
}
#[cfg(feature = "simd-is-enabled")]
PositionConstraintDesc::GroundGrouped(joint_id) => {
let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
if let Some(constraint) = AnyJointPositionConstraint::from_wide_joint_ground(
joints, bodies,
) {
self.constraints[joints[0].position_constraint_index] = constraint
} else {
for ii in 0..SIMD_WIDTH {
let constraint = AnyJointPositionConstraint::from_joint_ground(joints[ii], bodies);
self.constraints[joints[0].position_constraint_index + ii] = constraint;
}
}
}
}
}
}
}
}
impl ParallelPositionSolverContactPart {
pub fn init_constraints_groups(
&mut self,
island_id: usize,
bodies: &RigidBodySet,
manifolds: &mut [&mut ContactManifold],
manifold_groups: &ParallelInteractionGroups,
) {
let mut total_num_constraints = 0;
let num_groups = manifold_groups.num_groups();
self.interaction_groups.clear_groups();
self.ground_interaction_groups.clear_groups();
self.parallel_desc_groups.clear();
self.constraint_descs.clear();
self.parallel_desc_groups.push(0);
for i in 0..num_groups {
let group = manifold_groups.group(i);
self.ground_point_point.clear();
self.ground_plane_point.clear();
self.point_point.clear();
self.plane_point.clear();
categorize_position_contacts(
bodies,
manifolds,
group,
&mut self.ground_point_point,
&mut self.ground_plane_point,
&mut self.point_point,
&mut self.plane_point,
);
#[cfg(feature = "simd-is-enabled")]
let start_grouped = self.interaction_groups.grouped_interactions.len();
let start_nongrouped = self.interaction_groups.nongrouped_interactions.len();
#[cfg(feature = "simd-is-enabled")]
let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len();
let start_nongrouped_ground =
self.ground_interaction_groups.nongrouped_interactions.len();
self.interaction_groups.group_manifolds(
island_id,
bodies,
manifolds,
&self.point_point,
);
self.interaction_groups.group_manifolds(
island_id,
bodies,
manifolds,
&self.plane_point,
);
self.ground_interaction_groups.group_manifolds(
island_id,
bodies,
manifolds,
&self.ground_point_point,
);
self.ground_interaction_groups.group_manifolds(
island_id,
bodies,
manifolds,
&self.ground_plane_point,
);
// Compute constraint indices.
for interaction_i in
&self.interaction_groups.nongrouped_interactions[start_nongrouped..]
{
let manifold = &mut *manifolds[*interaction_i];
manifold.position_constraint_index = total_num_constraints;
self.constraint_descs.push((
total_num_constraints,
PositionConstraintDesc::NongroundNongrouped(*interaction_i),
));
total_num_constraints += PositionConstraint::num_active_constraints(manifold);
}
#[cfg(feature = "simd-is-enabled")]
for interaction_i in
self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH)
{
let manifold = &mut *manifolds[interaction_i[0]];
manifold.position_constraint_index = total_num_constraints;
self.constraint_descs.push((
total_num_constraints,
PositionConstraintDesc::NongroundGrouped(
array![|ii| interaction_i[ii]; SIMD_WIDTH],
),
));
total_num_constraints += PositionConstraint::num_active_constraints(manifold);
}
for interaction_i in
&self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..]
{
let manifold = &mut *manifolds[*interaction_i];
manifold.position_constraint_index = total_num_constraints;
self.constraint_descs.push((
total_num_constraints,
PositionConstraintDesc::GroundNongrouped(*interaction_i),
));
total_num_constraints += PositionConstraint::num_active_constraints(manifold);
}
#[cfg(feature = "simd-is-enabled")]
for interaction_i in self.ground_interaction_groups.grouped_interactions
[start_grouped_ground..]
.chunks(SIMD_WIDTH)
{
let manifold = &mut *manifolds[interaction_i[0]];
manifold.position_constraint_index = total_num_constraints;
self.constraint_descs.push((
total_num_constraints,
PositionConstraintDesc::GroundGrouped(
array![|ii| interaction_i[ii]; SIMD_WIDTH],
),
));
total_num_constraints += PositionConstraint::num_active_constraints(manifold);
}
self.parallel_desc_groups.push(self.constraint_descs.len());
}
// Resize the constraints set.
self.constraints.clear();
self.constraints
.resize_with(total_num_constraints, || AnyPositionConstraint::Empty)
}
fn fill_constraints(
&mut self,
thread: &ThreadContext,
params: &IntegrationParameters,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
) {
let descs = &self.constraint_descs;
crate::concurrent_loop! {
let batch_size = thread.batch_size;
for desc in descs[thread.position_constraint_initialization_index, thread.num_initialized_position_constraints] {
match &desc.1 {
PositionConstraintDesc::NongroundNongrouped(manifold_id) => {
let manifold = &*manifolds_all[*manifold_id];
PositionConstraint::generate(
params,
manifold,
bodies,
&mut self.constraints,
false,
);
}
PositionConstraintDesc::GroundNongrouped(manifold_id) => {
let manifold = &*manifolds_all[*manifold_id];
PositionGroundConstraint::generate(
params,
manifold,
bodies,
&mut self.constraints,
false,
);
}
#[cfg(feature = "simd-is-enabled")]
PositionConstraintDesc::NongroundGrouped(manifold_id) => {
let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
WPositionConstraint::generate(
params,
manifolds,
bodies,
&mut self.constraints,
false,
);
}
#[cfg(feature = "simd-is-enabled")]
PositionConstraintDesc::GroundGrouped(manifold_id) => {
let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
WPositionGroundConstraint::generate(
params,
manifolds,
bodies,
&mut self.constraints,
false,
);
}
}
}
}
}
}
pub(crate) struct ParallelPositionSolver {
part: ParallelPositionSolverContactPart,
joint_part: ParallelPositionSolverJointPart,
}
pub(crate) struct ParallelPositionSolver;
impl ParallelPositionSolver {
pub fn new() -> Self {
Self {
part: ParallelPositionSolverContactPart::new(),
joint_part: ParallelPositionSolverJointPart::new(),
}
}
pub fn init_constraint_groups(
&mut self,
island_id: usize,
bodies: &RigidBodySet,
manifolds: &mut [&mut ContactManifold],
manifold_groups: &ParallelInteractionGroups,
joints: &mut [JointGraphEdge],
joint_groups: &ParallelInteractionGroups,
) {
self.part
.init_constraints_groups(island_id, bodies, manifolds, manifold_groups);
self.joint_part
.init_constraints_groups(island_id, bodies, joints, joint_groups);
}
pub fn fill_constraints(
&mut self,
thread: &ThreadContext,
params: &IntegrationParameters,
bodies: &RigidBodySet,
manifolds: &[&mut ContactManifold],
joints: &[JointGraphEdge],
) {
self.part
.fill_constraints(thread, params, bodies, manifolds);
self.joint_part.fill_constraints(thread, bodies, joints);
ThreadContext::lock_until_ge(
&thread.num_initialized_position_constraints,
self.part.constraint_descs.len(),
);
ThreadContext::lock_until_ge(
&thread.num_initialized_position_joint_constraints,
self.joint_part.constraint_descs.len(),
);
}
pub fn solve_constraints(
&mut self,
pub fn solve(
thread: &ThreadContext,
params: &IntegrationParameters,
positions: &mut [Isometry<Real>],
contact_constraints: &mut ParallelSolverConstraints<
AnyVelocityConstraint,
AnyPositionConstraint,
>,
joint_constraints: &mut ParallelSolverConstraints<
AnyJointVelocityConstraint,
AnyJointPositionConstraint,
>,
) {
if self.part.constraint_descs.len() == 0 {
if contact_constraints.constraint_descs.is_empty()
&& joint_constraints.constraint_descs.is_empty()
{
return;
}
@@ -518,8 +58,8 @@ impl ParallelPositionSolver {
.solve_position_interaction_index
.fetch_add(thread.batch_size, Ordering::SeqCst);
let mut batch_size = thread.batch_size;
let contact_descs = &self.part.constraint_descs[..];
let joint_descs = &self.joint_part.constraint_descs[..];
let contact_descs = &contact_constraints.constraint_descs[..];
let joint_descs = &joint_constraints.constraint_descs[..];
let mut target_num_desc = 0;
let mut shift = 0;
@@ -535,9 +75,12 @@ impl ParallelPositionSolver {
let end_index = (start_index + batch_size).min(group[1]);
let constraints = if end_index == $part.constraint_descs.len() {
&mut $part.constraints[$part.constraint_descs[start_index].0..]
&mut $part.position_constraints
[$part.constraint_descs[start_index].0..]
} else {
&mut $part.constraints[$part.constraint_descs[start_index].0
&mut $part.position_constraints[$part.constraint_descs
[start_index]
.0
..$part.constraint_descs[end_index].0]
};
@@ -570,10 +113,10 @@ impl ParallelPositionSolver {
};
}
solve!(self.joint_part);
solve!(joint_constraints);
shift += joint_descs.len();
start_index -= joint_descs.len();
solve!(self.part);
solve!(contact_constraints);
shift += contact_descs.len();
start_index -= contact_descs.len();
}

309
src/dynamics/solver/parallel_solver_constraints.rs Normal file
View File

@@ -0,0 +1,309 @@
use super::ParallelInteractionGroups;
use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel, ThreadContext};
use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
use crate::dynamics::solver::{
AnyJointPositionConstraint, AnyPositionConstraint, InteractionGroups, PositionConstraint,
PositionGroundConstraint, VelocityConstraint, VelocityGroundConstraint, WPositionConstraint,
WPositionGroundConstraint,
};
use crate::dynamics::{IntegrationParameters, JointGraphEdge, RigidBodySet};
use crate::geometry::ContactManifold;
#[cfg(feature = "simd-is-enabled")]
use crate::{
dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint},
math::{Real, SIMD_WIDTH},
};
use std::sync::atomic::Ordering;
// pub fn init_constraint_groups(
// &mut self,
// island_id: usize,
// bodies: &RigidBodySet,
// manifolds: &mut [&mut ContactManifold],
// manifold_groups: &ParallelInteractionGroups,
// joints: &mut [JointGraphEdge],
// joint_groups: &ParallelInteractionGroups,
// ) {
// self.part
// .init_constraints_groups(island_id, bodies, manifolds, manifold_groups);
// self.joint_part
// .init_constraints_groups(island_id, bodies, joints, joint_groups);
// }
pub(crate) enum ConstraintDesc {
NongroundNongrouped(usize),
GroundNongrouped(usize),
#[cfg(feature = "simd-is-enabled")]
NongroundGrouped([usize; SIMD_WIDTH]),
#[cfg(feature = "simd-is-enabled")]
GroundGrouped([usize; SIMD_WIDTH]),
}
pub(crate) struct ParallelSolverConstraints<VelocityConstraint, PositionConstraint> {
pub not_ground_interactions: Vec<usize>,
pub ground_interactions: Vec<usize>,
pub interaction_groups: InteractionGroups,
pub ground_interaction_groups: InteractionGroups,
pub velocity_constraints: Vec<VelocityConstraint>,
pub position_constraints: Vec<PositionConstraint>,
pub constraint_descs: Vec<(usize, ConstraintDesc)>,
pub parallel_desc_groups: Vec<usize>,
}
impl<VelocityConstraint, PositionConstraint>
ParallelSolverConstraints<VelocityConstraint, PositionConstraint>
{
pub fn new() -> Self {
Self {
not_ground_interactions: Vec::new(),
ground_interactions: Vec::new(),
interaction_groups: InteractionGroups::new(),
ground_interaction_groups: InteractionGroups::new(),
velocity_constraints: Vec::new(),
position_constraints: Vec::new(),
constraint_descs: Vec::new(),
parallel_desc_groups: Vec::new(),
}
}
}
macro_rules! impl_init_constraints_group {
($VelocityConstraint: ty, $PositionConstraint: ty, $Interaction: ty,
$categorize: ident, $group: ident,
$data: ident$(.$constraint_index: ident)*,
$num_active_constraints: path, $empty_velocity_constraint: expr, $empty_position_constraint: expr $(, $weight: ident)*) => {
impl ParallelSolverConstraints<$VelocityConstraint, $PositionConstraint> {
pub fn init_constraint_groups(
&mut self,
island_id: usize,
bodies: &RigidBodySet,
interactions: &mut [$Interaction],
interaction_groups: &ParallelInteractionGroups,
) {
let mut total_num_constraints = 0;
let num_groups = interaction_groups.num_groups();
self.interaction_groups.clear_groups();
self.ground_interaction_groups.clear_groups();
self.parallel_desc_groups.clear();
self.constraint_descs.clear();
self.parallel_desc_groups.push(0);
for i in 0..num_groups {
let group = interaction_groups.group(i);
self.not_ground_interactions.clear();
self.ground_interactions.clear();
$categorize(
bodies,
interactions,
group,
&mut self.ground_interactions,
&mut self.not_ground_interactions,
);
#[cfg(feature = "simd-is-enabled")]
let start_grouped = self.interaction_groups.grouped_interactions.len();
let start_nongrouped = self.interaction_groups.nongrouped_interactions.len();
#[cfg(feature = "simd-is-enabled")]
let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len();
let start_nongrouped_ground = self.ground_interaction_groups.nongrouped_interactions.len();
self.interaction_groups.$group(
island_id,
bodies,
interactions,
&self.not_ground_interactions,
);
self.ground_interaction_groups.$group(
island_id,
bodies,
interactions,
&self.ground_interactions,
);
// Compute constraint indices.
for interaction_i in &self.interaction_groups.nongrouped_interactions[start_nongrouped..] {
let interaction = &mut interactions[*interaction_i]$(.$weight)*;
interaction.$data$(.$constraint_index)* = total_num_constraints;
self.constraint_descs.push((
total_num_constraints,
ConstraintDesc::NongroundNongrouped(*interaction_i),
));
total_num_constraints += $num_active_constraints(interaction);
}
#[cfg(feature = "simd-is-enabled")]
for interaction_i in
self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH)
{
let interaction = &mut interactions[interaction_i[0]]$(.$weight)*;
interaction.$data$(.$constraint_index)* = total_num_constraints;
self.constraint_descs.push((
total_num_constraints,
ConstraintDesc::NongroundGrouped(
array![|ii| interaction_i[ii]; SIMD_WIDTH],
),
));
total_num_constraints += $num_active_constraints(interaction);
}
for interaction_i in
&self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..]
{
let interaction = &mut interactions[*interaction_i]$(.$weight)*;
interaction.$data$(.$constraint_index)* = total_num_constraints;
self.constraint_descs.push((
total_num_constraints,
ConstraintDesc::GroundNongrouped(*interaction_i),
));
total_num_constraints += $num_active_constraints(interaction);
}
#[cfg(feature = "simd-is-enabled")]
for interaction_i in self.ground_interaction_groups.grouped_interactions
[start_grouped_ground..]
.chunks(SIMD_WIDTH)
{
let interaction = &mut interactions[interaction_i[0]]$(.$weight)*;
interaction.$data$(.$constraint_index)* = total_num_constraints;
self.constraint_descs.push((
total_num_constraints,
ConstraintDesc::GroundGrouped(
array![|ii| interaction_i[ii]; SIMD_WIDTH],
),
));
total_num_constraints += $num_active_constraints(interaction);
}
self.parallel_desc_groups.push(self.constraint_descs.len());
}
// Resize the constraint sets.
self.velocity_constraints.clear();
self.velocity_constraints
.resize_with(total_num_constraints, || $empty_velocity_constraint);
self.position_constraints.clear();
self.position_constraints
.resize_with(total_num_constraints, || $empty_position_constraint);
}
}
}
}
impl_init_constraints_group!(
AnyVelocityConstraint,
AnyPositionConstraint,
&mut ContactManifold,
categorize_contacts,
group_manifolds,
data.constraint_index,
VelocityConstraint::num_active_constraints,
AnyVelocityConstraint::Empty,
AnyPositionConstraint::Empty
);
impl_init_constraints_group!(
AnyJointVelocityConstraint,
AnyJointPositionConstraint,
JointGraphEdge,
categorize_joints,
group_joints,
constraint_index,
AnyJointVelocityConstraint::num_active_constraints,
AnyJointVelocityConstraint::Empty,
AnyJointPositionConstraint::Empty,
weight
);
impl ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
pub fn fill_constraints(
&mut self,
thread: &ThreadContext,
params: &IntegrationParameters,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
) {
let descs = &self.constraint_descs;
crate::concurrent_loop! {
let batch_size = thread.batch_size;
for desc in descs[thread.constraint_initialization_index, thread.num_initialized_constraints] {
match &desc.1 {
ConstraintDesc::NongroundNongrouped(manifold_id) => {
let manifold = &*manifolds_all[*manifold_id];
VelocityConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, false);
PositionConstraint::generate(params, manifold, bodies, &mut self.position_constraints, false);
}
ConstraintDesc::GroundNongrouped(manifold_id) => {
let manifold = &*manifolds_all[*manifold_id];
VelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, false);
PositionGroundConstraint::generate(params, manifold, bodies, &mut self.position_constraints, false);
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::NongroundGrouped(manifold_id) => {
let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false);
WPositionConstraint::generate(params, manifolds, bodies, &mut self.position_constraints, false);
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::GroundGrouped(manifold_id) => {
let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false);
WPositionGroundConstraint::generate(params, manifolds, bodies, &mut self.position_constraints, false);
}
}
}
}
}
}
impl ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
pub fn fill_constraints(
&mut self,
thread: &ThreadContext,
params: &IntegrationParameters,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
) {
let descs = &self.constraint_descs;
crate::concurrent_loop! {
let batch_size = thread.batch_size;
for desc in descs[thread.joint_constraint_initialization_index, thread.num_initialized_joint_constraints] {
match &desc.1 {
ConstraintDesc::NongroundNongrouped(joint_id) => {
let joint = &joints_all[*joint_id].weight;
let velocity_constraint = AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies);
let position_constraint = AnyJointPositionConstraint::from_joint(joint, bodies);
self.velocity_constraints[joint.constraint_index] = velocity_constraint;
self.position_constraints[joint.constraint_index] = position_constraint;
}
ConstraintDesc::GroundNongrouped(joint_id) => {
let joint = &joints_all[*joint_id].weight;
let velocity_constraint = AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies);
let position_constraint = AnyJointPositionConstraint::from_joint_ground(joint, bodies);
self.velocity_constraints[joint.constraint_index] = velocity_constraint;
self.position_constraints[joint.constraint_index] = position_constraint;
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::NongroundGrouped(joint_id) => {
let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
let velocity_constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, joints, bodies);
let position_constraint = AnyJointPositionConstraint::from_wide_joint(joints, bodies);
self.velocity_constraints[joints[0].constraint_index] = velocity_constraint;
self.position_constraints[joints[0].constraint_index] = position_constraint;
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::GroundGrouped(joint_id) => {
let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
let velocity_constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, joints, bodies);
let position_constraint = AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies);
self.velocity_constraints[joints[0].constraint_index] = velocity_constraint;
self.position_constraints[joints[0].constraint_index] = position_constraint;
}
}
}
}
}
}

352
src/dynamics/solver/parallel_velocity_solver.rs
View File

@@ -1,325 +1,41 @@
use super::ParallelInteractionGroups;
use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel, ThreadContext};
use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
use crate::dynamics::solver::{InteractionGroups, VelocityConstraint, VelocityGroundConstraint};
use crate::dynamics::solver::parallel_solver_constraints::ConstraintDesc;
use crate::dynamics::solver::{
AnyJointPositionConstraint, AnyPositionConstraint, InteractionGroups,
ParallelSolverConstraints, VelocityConstraint, VelocityGroundConstraint,
};
use crate::dynamics::{IntegrationParameters, JointGraphEdge, RigidBodySet};
use crate::geometry::ContactManifold;
#[cfg(feature = "simd-is-enabled")]
use crate::{
dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint},
simd::SIMD_WIDTH,
math::{Real, SIMD_WIDTH},
};
use std::sync::atomic::Ordering;
pub(crate) enum VelocityConstraintDesc {
NongroundNongrouped(usize),
GroundNongrouped(usize),
#[cfg(feature = "simd-is-enabled")]
NongroundGrouped([usize; SIMD_WIDTH]),
#[cfg(feature = "simd-is-enabled")]
GroundGrouped([usize; SIMD_WIDTH]),
}
pub(crate) struct ParallelSolverConstraints<Constraint> {
pub not_ground_interactions: Vec<usize>,
pub ground_interactions: Vec<usize>,
pub interaction_groups: InteractionGroups,
pub ground_interaction_groups: InteractionGroups,
pub constraints: Vec<Constraint>,
pub constraint_descs: Vec<(usize, VelocityConstraintDesc)>,
pub parallel_desc_groups: Vec<usize>,
}
impl<Constraint> ParallelSolverConstraints<Constraint> {
pub fn new() -> Self {
Self {
not_ground_interactions: Vec::new(),
ground_interactions: Vec::new(),
interaction_groups: InteractionGroups::new(),
ground_interaction_groups: InteractionGroups::new(),
constraints: Vec::new(),
constraint_descs: Vec::new(),
parallel_desc_groups: Vec::new(),
}
}
}
macro_rules! impl_init_constraints_group {
($Constraint: ty, $Interaction: ty, $categorize: ident, $group: ident, $num_active_constraints: path, $empty_constraint: expr $(, $weight: ident)*) => {
impl ParallelSolverConstraints<$Constraint> {
pub fn init_constraints_groups(
&mut self,
island_id: usize,
bodies: &RigidBodySet,
interactions: &mut [$Interaction],
interaction_groups: &ParallelInteractionGroups,
) {
let mut total_num_constraints = 0;
let num_groups = interaction_groups.num_groups();
self.interaction_groups.clear_groups();
self.ground_interaction_groups.clear_groups();
self.parallel_desc_groups.clear();
self.constraint_descs.clear();
self.parallel_desc_groups.push(0);
for i in 0..num_groups {
let group = interaction_groups.group(i);
self.not_ground_interactions.clear();
self.ground_interactions.clear();
$categorize(
bodies,
interactions,
group,
&mut self.ground_interactions,
&mut self.not_ground_interactions,
);
#[cfg(feature = "simd-is-enabled")]
let start_grouped = self.interaction_groups.grouped_interactions.len();
let start_nongrouped = self.interaction_groups.nongrouped_interactions.len();
#[cfg(feature = "simd-is-enabled")]
let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len();
let start_nongrouped_ground = self.ground_interaction_groups.nongrouped_interactions.len();
self.interaction_groups.$group(
island_id,
bodies,
interactions,
&self.not_ground_interactions,
);
self.ground_interaction_groups.$group(
island_id,
bodies,
interactions,
&self.ground_interactions,
);
// Compute constraint indices.
for interaction_i in &self.interaction_groups.nongrouped_interactions[start_nongrouped..] {
let interaction = &mut interactions[*interaction_i]$(.$weight)*;
interaction.constraint_index = total_num_constraints;
self.constraint_descs.push((
total_num_constraints,
VelocityConstraintDesc::NongroundNongrouped(*interaction_i),
));
total_num_constraints += $num_active_constraints(interaction);
}
#[cfg(feature = "simd-is-enabled")]
for interaction_i in
self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH)
{
let interaction = &mut interactions[interaction_i[0]]$(.$weight)*;
interaction.constraint_index = total_num_constraints;
self.constraint_descs.push((
total_num_constraints,
VelocityConstraintDesc::NongroundGrouped(
array![|ii| interaction_i[ii]; SIMD_WIDTH],
),
));
total_num_constraints += $num_active_constraints(interaction);
}
for interaction_i in
&self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..]
{
let interaction = &mut interactions[*interaction_i]$(.$weight)*;
interaction.constraint_index = total_num_constraints;
self.constraint_descs.push((
total_num_constraints,
VelocityConstraintDesc::GroundNongrouped(*interaction_i),
));
total_num_constraints += $num_active_constraints(interaction);
}
#[cfg(feature = "simd-is-enabled")]
for interaction_i in self.ground_interaction_groups.grouped_interactions
[start_grouped_ground..]
.chunks(SIMD_WIDTH)
{
let interaction = &mut interactions[interaction_i[0]]$(.$weight)*;
interaction.constraint_index = total_num_constraints;
self.constraint_descs.push((
total_num_constraints,
VelocityConstraintDesc::GroundGrouped(
array![|ii| interaction_i[ii]; SIMD_WIDTH],
),
));
total_num_constraints += $num_active_constraints(interaction);
}
self.parallel_desc_groups.push(self.constraint_descs.len());
}
// Resize the constraints set.
self.constraints.clear();
self.constraints
.resize_with(total_num_constraints, || $empty_constraint)
}
}
}
}
impl_init_constraints_group!(
AnyVelocityConstraint,
&mut ContactManifold,
categorize_contacts,
group_manifolds,
VelocityConstraint::num_active_constraints,
AnyVelocityConstraint::Empty
);
impl_init_constraints_group!(
AnyJointVelocityConstraint,
JointGraphEdge,
categorize_joints,
group_joints,
AnyJointVelocityConstraint::num_active_constraints,
AnyJointVelocityConstraint::Empty,
weight
);
impl ParallelSolverConstraints<AnyVelocityConstraint> {
fn fill_constraints(
&mut self,
thread: &ThreadContext,
params: &IntegrationParameters,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
) {
let descs = &self.constraint_descs;
crate::concurrent_loop! {
let batch_size = thread.batch_size;
for desc in descs[thread.constraint_initialization_index, thread.num_initialized_constraints] {
match &desc.1 {
VelocityConstraintDesc::NongroundNongrouped(manifold_id) => {
let manifold = &*manifolds_all[*manifold_id];
VelocityConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.constraints, false);
}
VelocityConstraintDesc::GroundNongrouped(manifold_id) => {
let manifold = &*manifolds_all[*manifold_id];
VelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.constraints, false);
}
#[cfg(feature = "simd-is-enabled")]
VelocityConstraintDesc::NongroundGrouped(manifold_id) => {
let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.constraints, false);
}
#[cfg(feature = "simd-is-enabled")]
VelocityConstraintDesc::GroundGrouped(manifold_id) => {
let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.constraints, false);
}
}
}
}
}
}
impl ParallelSolverConstraints<AnyJointVelocityConstraint> {
fn fill_constraints(
&mut self,
thread: &ThreadContext,
params: &IntegrationParameters,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
) {
let descs = &self.constraint_descs;
crate::concurrent_loop! {
let batch_size = thread.batch_size;
for desc in descs[thread.joint_constraint_initialization_index, thread.num_initialized_joint_constraints] {
match &desc.1 {
VelocityConstraintDesc::NongroundNongrouped(joint_id) => {
let joint = &joints_all[*joint_id].weight;
let constraint = AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies);
self.constraints[joint.constraint_index] = constraint;
}
VelocityConstraintDesc::GroundNongrouped(joint_id) => {
let joint = &joints_all[*joint_id].weight;
let constraint = AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies);
self.constraints[joint.constraint_index] = constraint;
}
#[cfg(feature = "simd-is-enabled")]
VelocityConstraintDesc::NongroundGrouped(joint_id) => {
let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
let constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, joints, bodies);
self.constraints[joints[0].data.constraint_index] = constraint;
}
#[cfg(feature = "simd-is-enabled")]
VelocityConstraintDesc::GroundGrouped(joint_id) => {
let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
let constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, joints, bodies);
self.constraints[joints[0].data.constraint_index] = constraint;
}
}
}
}
}
}
pub(crate) struct ParallelVelocitySolver {
part: ParallelSolverConstraints<AnyVelocityConstraint>,
joint_part: ParallelSolverConstraints<AnyJointVelocityConstraint>,
}
pub(crate) struct ParallelVelocitySolver {}
impl ParallelVelocitySolver {
pub fn new() -> Self {
Self {
part: ParallelSolverConstraints::new(),
joint_part: ParallelSolverConstraints::new(),
}
}
pub fn init_constraint_groups(
&mut self,
island_id: usize,
bodies: &RigidBodySet,
manifolds: &mut [&mut ContactManifold],
manifold_groups: &ParallelInteractionGroups,
joints: &mut [JointGraphEdge],
joint_groups: &ParallelInteractionGroups,
) {
self.part
.init_constraints_groups(island_id, bodies, manifolds, manifold_groups);
self.joint_part
.init_constraints_groups(island_id, bodies, joints, joint_groups);
}
pub fn fill_constraints(
&mut self,
thread: &ThreadContext,
params: &IntegrationParameters,
bodies: &RigidBodySet,
manifolds: &[&mut ContactManifold],
joints: &[JointGraphEdge],
) {
self.part
.fill_constraints(thread, params, bodies, manifolds);
self.joint_part
.fill_constraints(thread, params, bodies, joints);
ThreadContext::lock_until_ge(
&thread.num_initialized_constraints,
self.part.constraint_descs.len(),
);
ThreadContext::lock_until_ge(
&thread.num_initialized_joint_constraints,
self.joint_part.constraint_descs.len(),
);
}
pub fn solve_constraints(
&mut self,
pub fn solve(
thread: &ThreadContext,
params: &IntegrationParameters,
manifolds_all: &mut [&mut ContactManifold],
joints_all: &mut [JointGraphEdge],
mj_lambdas: &mut [DeltaVel<Real>],
contact_constraints: &mut ParallelSolverConstraints<
AnyVelocityConstraint,
AnyPositionConstraint,
>,
joint_constraints: &mut ParallelSolverConstraints<
AnyJointVelocityConstraint,
AnyJointPositionConstraint,
>,
) {
if self.part.constraint_descs.len() == 0 && self.joint_part.constraint_descs.len() == 0 {
if contact_constraints.constraint_descs.is_empty()
&& joint_constraints.constraint_descs.is_empty()
{
return;
}
@@ -348,9 +64,9 @@ impl ParallelVelocitySolver {
let end_index = (start_index + batch_size).min(group[1]);
let constraints = if end_index == $part.constraint_descs.len() {
&mut $part.constraints[$part.constraint_descs[start_index].0..]
&mut $part.velocity_constraints[$part.constraint_descs[start_index].0..]
} else {
&mut $part.constraints[$part.constraint_descs[start_index].0..$part.constraint_descs[end_index].0]
&mut $part.velocity_constraints[$part.constraint_descs[start_index].0..$part.constraint_descs[end_index].0]
};
for constraint in constraints {
@@ -380,10 +96,10 @@ impl ParallelVelocitySolver {
}
);
warmstart!(self.joint_part);
shift = self.joint_part.constraint_descs.len();
warmstart!(joint_constraints);
shift = joint_constraints.constraint_descs.len();
start_index -= shift;
warmstart!(self.part);
warmstart!(contact_constraints);
}
/*
@@ -398,8 +114,8 @@ impl ParallelVelocitySolver {
.solve_interaction_index
.fetch_add(thread.batch_size, Ordering::SeqCst);
let mut batch_size = thread.batch_size;
let contact_descs = &self.part.constraint_descs[..];
let joint_descs = &self.joint_part.constraint_descs[..];
let contact_descs = &contact_constraints.constraint_descs[..];
let joint_descs = &joint_constraints.constraint_descs[..];
let mut target_num_desc = 0;
let mut shift = 0;
@@ -416,9 +132,12 @@ impl ParallelVelocitySolver {
let end_index = (start_index + batch_size).min(group[1]);
let constraints = if end_index == $part.constraint_descs.len() {
&mut $part.constraints[$part.constraint_descs[start_index].0..]
&mut $part.velocity_constraints
[$part.constraint_descs[start_index].0..]
} else {
&mut $part.constraints[$part.constraint_descs[start_index].0
&mut $part.velocity_constraints[$part.constraint_descs
[start_index]
.0
..$part.constraint_descs[end_index].0]
};
@@ -455,10 +174,10 @@ impl ParallelVelocitySolver {
};
}
solve!(self.joint_part);
solve!(joint_constraints);
shift += joint_descs.len();
start_index -= joint_descs.len();
solve!(self.part);
solve!(contact_constraints);
shift += contact_descs.len();
start_index -= contact_descs.len();
}
@@ -467,8 +186,9 @@ impl ParallelVelocitySolver {
/*
* Writeback impulses.
*/
let joint_constraints = &self.joint_part.constraints;
let contact_constraints = &self.part.constraints;
let joint_constraints = &joint_constraints.velocity_constraints;
let contact_constraints = &contact_constraints.velocity_constraints;
crate::concurrent_loop! {
let batch_size = thread.batch_size;
for constraint in joint_constraints[thread.joint_writeback_index] {

4
src/dynamics/solver/position_constraint.rs
View File

@@ -53,8 +53,8 @@ pub(crate) struct PositionConstraint {
impl PositionConstraint {
#[cfg(feature = "parallel")]
pub fn num_active_constraints(manifold: &ContactManifold) -> usize {
let rest = manifold.num_active_contacts() % MAX_MANIFOLD_POINTS != 0;
manifold.num_active_contacts() / MAX_MANIFOLD_POINTS + rest as usize
let rest = manifold.data.solver_contacts.len() % MAX_MANIFOLD_POINTS != 0;
manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS + rest as usize
}
pub fn generate(

26
src/dynamics/solver/solver_constraints.rs
View File

@@ -318,18 +318,8 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
);
self.velocity_constraints.push(vel_constraint);
if let Some(pos_constraint) =
AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies)
{
self.position_constraints.push(pos_constraint);
} else {
for joint in joints.iter() {
self.position_constraints
.push(AnyJointPositionConstraint::from_joint_ground(
*joint, bodies,
))
}
}
let pos_constraint = AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies);
self.position_constraints.push(pos_constraint);
}
}
@@ -367,16 +357,8 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
AnyJointVelocityConstraint::from_wide_joint(params, joints_id, joints, bodies);
self.velocity_constraints.push(vel_constraint);
if let Some(pos_constraint) =
AnyJointPositionConstraint::from_wide_joint(joints, bodies)
{
self.position_constraints.push(pos_constraint);
} else {
for joint in joints.iter() {
self.position_constraints
.push(AnyJointPositionConstraint::from_joint(*joint, bodies))
}
}
let pos_constraint = AnyJointPositionConstraint::from_wide_joint(joints, bodies);
self.position_constraints.push(pos_constraint);
}
}
}

4
src/dynamics/solver/velocity_constraint.rs
View File

@@ -132,8 +132,8 @@ pub(crate) struct VelocityConstraint {
impl VelocityConstraint {
#[cfg(feature = "parallel")]
pub fn num_active_constraints(manifold: &ContactManifold) -> usize {
let rest = manifold.num_active_contacts() % MAX_MANIFOLD_POINTS != 0;
manifold.num_active_contacts() / MAX_MANIFOLD_POINTS + rest as usize
let rest = manifold.data.solver_contacts.len() % MAX_MANIFOLD_POINTS != 0;
manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS + rest as usize
}
pub fn generate(