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

Start fixing the parallel version.

Browse Source
This commit is contained in:
Sébastien Crozet
2022-02-20 12:56:13 +01:00
committed by Sébastien Crozet
parent fb20d72ee2
commit 412fedf7e3
11 changed files with 454 additions and 170 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
examples3d/articulations3.rs
View File

@@ -1,6 +1,35 @@
use rapier3d::prelude::*;
use rapier_testbed3d::Testbed;
fn create_coupled_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
origin: Point<f32>,
use_articulations: bool,
) {
let ground = bodies.insert(RigidBodyBuilder::new_static().translation(origin.coords));
let body1 = bodies.insert(
RigidBodyBuilder::new_dynamic()
.translation(origin.coords)
.linvel(vector![5.0, 5.0, 5.0]),
);
colliders.insert_with_parent(ColliderBuilder::cuboid(1.0, 1.0, 1.0), body1, bodies);
let joint1 = GenericJointBuilder::new(JointAxesMask::empty())
.limits(JointAxis::X, [-3.0, 3.0])
.limits(JointAxis::Y, [0.0, 3.0])
.limits(JointAxis::Z, [0.0, 3.0])
.coupled_axes(JointAxesMask::Y | JointAxesMask::Z);
if use_articulations {
multibody_joints.insert(ground, body1, joint1);
} else {
impulse_joints.insert(ground, body1, joint1);
}
}
fn create_prismatic_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
@@ -450,7 +479,7 @@ fn create_actuated_revolute_joints(
if i == 1 {
joint = joint
.local_anchor2(point![0.0, 2.0, -shift])
.motor_velocity(-2.0, 100.0);
.motor_velocity(-2.0, 1000.0);
}
if use_articulations {
@@ -617,6 +646,14 @@ fn do_init_world(testbed: &mut Testbed, use_articulations: bool) {
point![-5.0, 0.0, 0.0],
use_articulations,
);
create_coupled_joints(
&mut bodies,
&mut colliders,
&mut impulse_joints,
&mut multibody_joints,
point![0.0, 20.0, 0.0],
use_articulations,
);
/*
* Set up the testbed.

20
src/dynamics/joint/generic_joint.rs
View File

@@ -26,6 +26,8 @@ bitflags::bitflags! {
const FREE_PRISMATIC_AXES = Self::X.bits;
const FREE_FIXED_AXES = 0;
const FREE_SPHERICAL_AXES = Self::ANG_X.bits | Self::ANG_Y.bits | Self::ANG_Z.bits;
const LIN_AXES = Self::X.bits() | Self::Y.bits() | Self::Z.bits();
const ANG_AXES = Self::ANG_X.bits() | Self::ANG_Y.bits() | Self::ANG_Z.bits();
}
}
@@ -42,6 +44,8 @@ bitflags::bitflags! {
const FREE_REVOLUTE_AXES = Self::ANG_X.bits;
const FREE_PRISMATIC_AXES = Self::X.bits;
const FREE_FIXED_AXES = 0;
const LIN_AXES = Self::X.bits() | Self::Y.bits();
const ANG_AXES = Self::ANG_X.bits();
}
}
@@ -104,7 +108,7 @@ impl Default for JointMotor {
damping: 0.0,
max_force: Real::MAX,
impulse: 0.0,
model: MotorModel::AccelerationBased, // VelocityBased,
model: MotorModel::AccelerationBased,
}
}
}
@@ -134,6 +138,7 @@ pub struct GenericJoint {
pub locked_axes: JointAxesMask,
pub limit_axes: JointAxesMask,
pub motor_axes: JointAxesMask,
pub coupled_axes: JointAxesMask,
pub limits: [JointLimits<Real>; SPATIAL_DIM],
pub motors: [JointMotor; SPATIAL_DIM],
}
@@ -146,6 +151,7 @@ impl Default for GenericJoint {
locked_axes: JointAxesMask::empty(),
limit_axes: JointAxesMask::empty(),
motor_axes: JointAxesMask::empty(),
coupled_axes: JointAxesMask::empty(),
limits: [JointLimits::default(); SPATIAL_DIM],
motors: [JointMotor::default(); SPATIAL_DIM],
}
@@ -445,6 +451,12 @@ impl GenericJointBuilder {
self
}
#[must_use]
pub fn coupled_axes(mut self, axes: JointAxesMask) -> Self {
self.0.coupled_axes = axes;
self
}
/// Set the spring-like model used by the motor to reach the desired target velocity and position.
#[must_use]
pub fn motor_model(mut self, axis: JointAxis, model: MotorModel) -> Self {
@@ -499,3 +511,9 @@ impl GenericJointBuilder {
self.0
}
}
impl Into<GenericJoint> for GenericJointBuilder {
fn into(self) -> GenericJoint {
self.0
}
}

1
src/dynamics/joint/impulse_joint/impulse_joint.rs
View File

@@ -15,6 +15,5 @@ pub struct ImpulseJoint {
// A joint needs to know its handle to simplify its removal.
pub(crate) handle: ImpulseJointHandle,
#[cfg(feature = "parallel")]
pub(crate) constraint_index: usize,
}

1
src/dynamics/joint/impulse_joint/impulse_joint_set.rs
View File

@@ -187,7 +187,6 @@ impl ImpulseJointSet {
data,
impulses: na::zero(),
handle: ImpulseJointHandle(handle),
#[cfg(feature = "parallel")]
constraint_index: 0,
};

66
src/dynamics/solver/joint_constraint/joint_constraint.rs
View File

@@ -7,12 +7,12 @@ use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
};
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyIds,
ImpulseJoint, IntegrationParameters, JointAxesMask, JointGraphEdge, JointIndex, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
};
#[cfg(feature = "simd-is-enabled")]
use crate::math::{Isometry, SimdReal, SIMD_WIDTH};
use crate::math::{Real, SPATIAL_DIM};
use crate::math::{Real, DIM, SPATIAL_DIM};
use crate::prelude::MultibodyJointSet;
use na::DVector;
@@ -30,8 +30,20 @@ pub enum AnyJointVelocityConstraint {
impl AnyJointVelocityConstraint {
#[cfg(feature = "parallel")]
pub fn num_active_constraints(_: &ImpulseJoint) -> usize {
1
pub fn num_active_constraints(joint: &ImpulseJoint) -> usize {
let joint = &joint.data;
let locked_axes = joint.locked_axes.bits();
let motor_axes = joint.motor_axes.bits() & !locked_axes;
let limit_axes = joint.limit_axes.bits() & !locked_axes;
let coupled_axes = joint.coupled_axes.bits();
(motor_axes & !coupled_axes).count_ones() as usize
+ ((motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0) as usize
+ ((motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0) as usize
+ locked_axes.count_ones() as usize
+ (limit_axes & !coupled_axes).count_ones() as usize
+ ((limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0) as usize
+ ((limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0) as usize
}
pub fn from_joint<Bodies>(
@@ -43,6 +55,7 @@ impl AnyJointVelocityConstraint {
j_id: &mut usize,
jacobians: &mut DVector<Real>,
out: &mut Vec<Self>,
push: bool,
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
@@ -130,9 +143,16 @@ impl AnyJointVelocityConstraint {
&mut out_tmp,
);
if push {
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointGenericConstraint(c));
}
} else {
for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
out[joint.constraint_index + i] =
AnyJointVelocityConstraint::JointGenericConstraint(c);
}
}
} else {
// TODO: find a way to avoid the temporary buffer.
let mut out_tmp = [JointVelocityConstraint::invalid(); 12];
@@ -147,9 +167,16 @@ impl AnyJointVelocityConstraint {
&mut out_tmp,
);
if push {
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointConstraint(c));
}
} else {
for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
out[joint.constraint_index + i] =
AnyJointVelocityConstraint::JointConstraint(c);
}
}
}
}
@@ -160,6 +187,7 @@ impl AnyJointVelocityConstraint {
impulse_joints: [&ImpulseJoint; SIMD_WIDTH],
bodies: &Bodies,
out: &mut Vec<Self>,
push: bool,
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
@@ -232,9 +260,16 @@ impl AnyJointVelocityConstraint {
&mut out_tmp,
);
if push {
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointConstraintSimd(c));
}
} else {
for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
out[impulse_joints[0].constraint_index + i] =
AnyJointVelocityConstraint::JointConstraintSimd(c);
}
}
}
pub fn from_joint_ground<Bodies>(
@@ -246,6 +281,7 @@ impl AnyJointVelocityConstraint {
j_id: &mut usize,
jacobians: &mut DVector<Real>,
out: &mut Vec<Self>,
push: bool,
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyType>
@@ -334,9 +370,16 @@ impl AnyJointVelocityConstraint {
&mut out_tmp,
);
if push {
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(c));
}
} else {
for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
out[joint.constraint_index + i] =
AnyJointVelocityConstraint::JointGenericGroundConstraint(c);
}
}
} else {
// TODO: find a way to avoid the temporary buffer.
let mut out_tmp = [JointVelocityGroundConstraint::invalid(); 12];
@@ -351,9 +394,16 @@ impl AnyJointVelocityConstraint {
&mut out_tmp,
);
if push {
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointGroundConstraint(c));
}
} else {
for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
out[joint.constraint_index + i] =
AnyJointVelocityConstraint::JointGroundConstraint(c);
}
}
}
}
@@ -364,6 +414,7 @@ impl AnyJointVelocityConstraint {
impulse_joints: [&ImpulseJoint; SIMD_WIDTH],
bodies: &Bodies,
out: &mut Vec<Self>,
push: bool,
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyType>
@@ -455,9 +506,16 @@ impl AnyJointVelocityConstraint {
&mut out_tmp,
);
if push {
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointGroundConstraintSimd(c));
}
} else {
for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
out[impulse_joints[0].constraint_index + i] =
AnyJointVelocityConstraint::JointGroundConstraintSimd(c);
}
}
}
pub fn remove_bias_from_rhs(&mut self) {

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

@@ -14,7 +14,7 @@ use crate::dynamics::{
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Isometry, Real};
use crate::utils::WAngularInertia;
use na::DVector;
use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver};
@@ -60,6 +60,25 @@ macro_rules! concurrent_loop {
}
}
};
(let batch_size = $batch_size: expr;
for $elt: ident in &mut $array: ident[$index_stream:expr] $f: expr) => {
let max_index = $array.len();
if max_index > 0 {
loop {
let start_index = $index_stream.fetch_add($batch_size, Ordering::SeqCst);
if start_index > max_index {
break;
}
let end_index = (start_index + $batch_size).min(max_index);
for $elt in &mut $array[start_index..end_index] {
$f
}
}
}
};
}
pub(crate) struct ThreadContext {
@@ -73,10 +92,14 @@ pub(crate) struct ThreadContext {
pub num_solved_interactions: AtomicUsize,
pub impulse_writeback_index: AtomicUsize,
pub joint_writeback_index: AtomicUsize,
pub body_integration_index: AtomicUsize,
pub impulse_rm_bias_index: AtomicUsize,
pub joint_rm_bias_index: AtomicUsize,
pub body_integration_pos_index: AtomicUsize,
pub body_integration_vel_index: AtomicUsize,
pub body_force_integration_index: AtomicUsize,
pub num_force_integrated_bodies: AtomicUsize,
pub num_integrated_bodies: AtomicUsize,
pub num_integrated_pos_bodies: AtomicUsize,
pub num_integrated_vel_bodies: AtomicUsize,
}
impl ThreadContext {
@@ -91,10 +114,14 @@ impl ThreadContext {
num_solved_interactions: AtomicUsize::new(0),
impulse_writeback_index: AtomicUsize::new(0),
joint_writeback_index: AtomicUsize::new(0),
impulse_rm_bias_index: AtomicUsize::new(0),
joint_rm_bias_index: AtomicUsize::new(0),
body_force_integration_index: AtomicUsize::new(0),
num_force_integrated_bodies: AtomicUsize::new(0),
body_integration_index: AtomicUsize::new(0),
num_integrated_bodies: AtomicUsize::new(0),
body_integration_pos_index: AtomicUsize::new(0),
body_integration_vel_index: AtomicUsize::new(0),
num_integrated_pos_bodies: AtomicUsize::new(0),
num_integrated_vel_bodies: AtomicUsize::new(0),
}
}
@@ -151,12 +178,12 @@ impl ParallelIslandSolver {
manifold_indices: &'s [ContactManifoldIndex],
impulse_joints: &'s mut Vec<JointGraphEdge>,
joint_indices: &[JointIndex],
multibody_joints: &mut MultibodyJointSet,
multibodies: &mut MultibodyJointSet,
) where
Bodies: ComponentSet<RigidBodyForces>
+ ComponentSetMut<RigidBodyPosition>
+ ComponentSetMut<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSetMut<RigidBodyMassProps>
+ ComponentSet<RigidBodyDamping>
+ ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyType>,
@@ -182,7 +209,7 @@ impl ParallelIslandSolver {
island_id,
islands,
bodies,
multibody_joints,
multibodies,
manifolds,
&self.parallel_groups,
);
@@ -190,7 +217,7 @@ impl ParallelIslandSolver {
island_id,
islands,
bodies,
multibody_joints,
multibodies,
impulse_joints,
&self.parallel_joint_groups,
);
@@ -207,6 +234,7 @@ impl ParallelIslandSolver {
let velocity_solver =
std::sync::atomic::AtomicPtr::new(&mut self.velocity_solver as *mut _);
let bodies = std::sync::atomic::AtomicPtr::new(bodies as *mut _);
let multibodies = std::sync::atomic::AtomicPtr::new(multibodies as *mut _);
let manifolds = std::sync::atomic::AtomicPtr::new(manifolds as *mut _);
let impulse_joints = std::sync::atomic::AtomicPtr::new(impulse_joints as *mut _);
let parallel_contact_constraints =
@@ -220,6 +248,8 @@ impl ParallelIslandSolver {
unsafe { std::mem::transmute(velocity_solver.load(Ordering::Relaxed)) };
let bodies: &mut Bodies =
unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
let multibodies: &mut MultibodyJointSet =
unsafe { std::mem::transmute(multibodies.load(Ordering::Relaxed)) };
let manifolds: &mut Vec<&mut ContactManifold> =
unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) };
let impulse_joints: &mut Vec<JointGraphEdge> =
@@ -257,8 +287,10 @@ impl ParallelIslandSolver {
}
let mut j_id = 0; // TODO
let mut jacobians = DVector::zeros(0); // TODO
parallel_contact_constraints.fill_constraints(&thread, params, bodies, manifolds);
parallel_joint_constraints.fill_constraints(&thread, params, bodies, impulse_joints);
parallel_joint_constraints.fill_constraints(&thread, params, bodies, multibodies, impulse_joints, &mut j_id, &mut jacobians);
ThreadContext::lock_until_ge(
&thread.num_initialized_constraints,
parallel_contact_constraints.constraint_descs.len(),
@@ -271,42 +303,14 @@ impl ParallelIslandSolver {
velocity_solver.solve(
&thread,
params,
island_id,
islands,
bodies,
manifolds,
impulse_joints,
parallel_contact_constraints,
parallel_joint_constraints,
);
// Write results back to rigid bodies and integrate velocities.
let island_range = islands.active_island_range(island_id);
let active_bodies = &islands.active_dynamic_set[island_range];
concurrent_loop! {
let batch_size = thread.batch_size;
for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] {
let (rb_ids, rb_pos, rb_vels, rb_damping, rb_mprops): (
&RigidBodyIds,
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyDamping,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let mut new_rb_pos = *rb_pos;
let mut new_rb_vels = *rb_vels;
let dvels = velocity_solver.mj_lambdas[rb_ids.active_set_offset];
new_rb_vels.linvel += dvels.linear;
new_rb_vels.angvel += rb_mprops.effective_world_inv_inertia_sqrt.transform_vector(dvels.angular);
let new_rb_vels = new_rb_vels.apply_damping(params.dt, rb_damping);
new_rb_pos.next_position =
new_rb_vels.integrate(params.dt, &rb_pos.position, &rb_mprops.local_mprops.local_com);
bodies.set_internal(handle.0, new_rb_vels);
bodies.set_internal(handle.0, new_rb_pos);
}
}
})
}
}

24
src/dynamics/solver/parallel_solver_constraints.rs
View File

@@ -87,7 +87,7 @@ macro_rules! impl_init_constraints_group {
island_id: usize,
islands: &IslandManager,
bodies: &Bodies,
multibody_joints: &MultibodyJointSet,
multibodies: &MultibodyJointSet,
interactions: &mut [$Interaction],
interaction_groups: &ParallelInteractionGroups,
) where Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds> {
@@ -107,7 +107,7 @@ macro_rules! impl_init_constraints_group {
self.ground_interactions.clear();
$categorize(
bodies,
multibody_joints,
multibodies,
interactions,
group,
&mut self.ground_interactions,
@@ -276,7 +276,10 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, ()> {
thread: &ThreadContext,
params: &IntegrationParameters,
bodies: &Bodies,
multibodies: &MultibodyJointSet,
joints_all: &[JointGraphEdge],
j_id: &mut usize,
jacobians: &mut DVector<Real>,
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
@@ -284,9 +287,6 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, ()> {
+ ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyType>,
{
return;
/*
let descs = &self.constraint_descs;
crate::concurrent_loop! {
@@ -295,30 +295,24 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, ()> {
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);
self.velocity_constraints[joint.constraint_index] = velocity_constraint;
AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies, multibodies, j_id, jacobians, &mut self.velocity_constraints, false);
}
ConstraintDesc::GroundNongrouped(joint_id) => {
let joint = &joints_all[*joint_id].weight;
let velocity_constraint = AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies);
self.velocity_constraints[joint.constraint_index] = velocity_constraint;
AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, j_id, jacobians, &mut self.velocity_constraints, false);
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::NongroundGrouped(joint_id) => {
let impulse_joints = gather![|ii| &joints_all[joint_id[ii]].weight];
let velocity_constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, impulse_joints, bodies);
self.velocity_constraints[impulse_joints[0].constraint_index] = velocity_constraint;
AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, false);
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::GroundGrouped(joint_id) => {
let impulse_joints = gather![|ii| &joints_all[joint_id[ii]].weight];
let velocity_constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, impulse_joints, bodies);
self.velocity_constraints[impulse_joints[0].constraint_index] = velocity_constraint;
AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, false);
}
}
}
}
*/
}
}

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

@@ -1,9 +1,15 @@
use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel, ThreadContext};
use crate::dynamics::solver::generic_velocity_constraint::GenericVelocityConstraint;
use crate::dynamics::solver::ParallelSolverConstraints;
use crate::dynamics::{IntegrationParameters, JointGraphEdge};
use crate::concurrent_loop;
use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
use crate::dynamics::{
solver::{GenericVelocityConstraint, ParallelSolverConstraints},
IntegrationParameters, IslandManager, JointGraphEdge, RigidBodyDamping, RigidBodyForces,
RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
};
use crate::geometry::ContactManifold;
use crate::math::Real;
use crate::utils::WAngularInertia;
use na::DVector;
use std::sync::atomic::Ordering;
@@ -20,10 +26,13 @@ impl ParallelVelocitySolver {
}
}
pub fn solve(
pub fn solve<Bodies>(
&mut self,
thread: &ThreadContext,
params: &IntegrationParameters,
island_id: usize,
islands: &IslandManager,
bodies: &mut Bodies,
manifolds_all: &mut [&mut ContactManifold],
joints_all: &mut [JointGraphEdge],
contact_constraints: &mut ParallelSolverConstraints<
@@ -31,21 +40,15 @@ impl ParallelVelocitySolver {
GenericVelocityConstraint,
>,
joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint, ()>,
) {
if contact_constraints.constraint_descs.is_empty()
&& joint_constraints.constraint_descs.is_empty()
) where
Bodies: ComponentSet<RigidBodyForces>
+ ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyVelocity>
+ ComponentSetMut<RigidBodyMassProps>
+ ComponentSetMut<RigidBodyPosition>
+ ComponentSet<RigidBodyDamping>,
{
return;
}
/*
* Solve constraints.
*/
{
// Each thread will concurrently grab thread.batch_size constraint desc to
// solve. If the batch size is large enough for to cross the boundary of
// a parallel_desc_group, we have to wait util the current group is finished
// before starting the next one.
let mut start_index = thread
.solve_interaction_index
.fetch_add(thread.batch_size, Ordering::SeqCst);
@@ -56,25 +59,26 @@ impl ParallelVelocitySolver {
let mut shift = 0;
let cfm_factor = params.cfm_factor();
for _ in 0..params.max_velocity_iterations {
// Each thread will concurrently grab thread.batch_size constraint desc to
// solve. If the batch size is large enough to cross the boundary of
// a parallel_desc_group, we have to wait util the current group is finished
// before starting the next one.
macro_rules! solve {
($part: expr, $($solve_args: expr),*) => {
// ImpulseJoint groups.
for group in $part.parallel_desc_groups.windows(2) {
let num_descs_in_group = group[1] - group[0];
target_num_desc += num_descs_in_group;
while start_index < group[1] {
let end_index = (start_index + batch_size).min(group[1]);
// TODO: remove the first branch case?
let constraints = if end_index == $part.constraint_descs.len() {
&mut $part.velocity_constraints
[$part.constraint_descs[start_index].0..]
} else {
&mut $part.velocity_constraints[$part.constraint_descs
[start_index]
.0
&mut $part.velocity_constraints
[$part.constraint_descs[start_index].0
..$part.constraint_descs[end_index].0]
};
@@ -109,6 +113,15 @@ impl ParallelVelocitySolver {
};
}
/*
* Solve constraints.
*/
{
for i in 0..params.max_velocity_iterations {
let solve_friction = params.interleave_restitution_and_friction_resolution
&& params.max_velocity_friction_iterations + i
>= params.max_velocity_iterations;
solve!(
joint_constraints,
&joint_constraints.generic_jacobians,
@@ -122,6 +135,17 @@ impl ParallelVelocitySolver {
cfm_factor,
&mut self.mj_lambdas,
true,
false
);
shift += contact_descs.len();
start_index -= contact_descs.len();
if solve_friction {
solve!(
contact_constraints,
cfm_factor,
&mut self.mj_lambdas,
false,
true
);
shift += contact_descs.len();
@@ -129,6 +153,150 @@ impl ParallelVelocitySolver {
}
}
// Solve the remaining friction iterations.
let remaining_friction_iterations =
if !params.interleave_restitution_and_friction_resolution {
params.max_velocity_friction_iterations
} else if params.max_velocity_friction_iterations > params.max_velocity_iterations {
params.max_velocity_friction_iterations - params.max_velocity_iterations
} else {
0
};
for _ in 0..remaining_friction_iterations {
solve!(
contact_constraints,
cfm_factor,
&mut self.mj_lambdas,
false,
true
);
shift += contact_descs.len();
start_index -= contact_descs.len();
}
}
// Integrate positions.
{
let island_range = islands.active_island_range(island_id);
let active_bodies = &islands.active_dynamic_set[island_range];
concurrent_loop! {
let batch_size = thread.batch_size;
for handle in active_bodies[thread.body_integration_pos_index, thread.num_integrated_pos_bodies] {
let (rb_ids, rb_mprops): (&RigidBodyIds, &RigidBodyMassProps) =
bodies.index_bundle(handle.0);
let dvel = self.mj_lambdas[rb_ids.active_set_offset];
let dangvel = rb_mprops
.effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular);
// Update positions.
let (rb_pos, rb_vels, rb_damping, rb_mprops): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyDamping,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let mut new_pos = *rb_pos;
let mut new_vels = *rb_vels;
new_vels.linvel += dvel.linear;
new_vels.angvel += dangvel;
new_vels = new_vels.apply_damping(params.dt, rb_damping);
new_pos.next_position = new_vels.integrate(
params.dt,
&rb_pos.position,
&rb_mprops.local_mprops.local_com,
);
bodies.set_internal(handle.0, new_pos);
}
}
}
// Remove bias from constraints.
{
let joint_constraints = &mut joint_constraints.velocity_constraints;
let contact_constraints = &mut contact_constraints.velocity_constraints;
crate::concurrent_loop! {
let batch_size = thread.batch_size;
for constraint in &mut joint_constraints[thread.joint_rm_bias_index] {
constraint.remove_bias_from_rhs();
}
}
crate::concurrent_loop! {
let batch_size = thread.batch_size;
for constraint in &mut contact_constraints[thread.impulse_rm_bias_index] {
constraint.remove_bias_from_rhs();
}
}
}
// Stabiliziton resolution.
{
for _ in 0..params.max_stabilization_iterations {
solve!(
joint_constraints,
&joint_constraints.generic_jacobians,
&mut self.mj_lambdas,
&mut self.generic_mj_lambdas
);
shift += joint_descs.len();
start_index -= joint_descs.len();
solve!(
contact_constraints,
cfm_factor,
&mut self.mj_lambdas,
true,
false
);
shift += contact_descs.len();
start_index -= contact_descs.len();
solve!(
contact_constraints,
cfm_factor,
&mut self.mj_lambdas,
false,
true
);
shift += contact_descs.len();
start_index -= contact_descs.len();
}
}
// Update velocities.
{
let island_range = islands.active_island_range(island_id);
let active_bodies = &islands.active_dynamic_set[island_range];
concurrent_loop! {
let batch_size = thread.batch_size;
for handle in active_bodies[thread.body_integration_vel_index, thread.num_integrated_vel_bodies] {
let (rb_ids, rb_damping, rb_mprops): (
&RigidBodyIds,
&RigidBodyDamping,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let dvel = self.mj_lambdas[rb_ids.active_set_offset];
let dangvel = rb_mprops
.effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular);
let damping = *rb_damping; // To avoid borrow issues.
bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| {
vels.linvel += dvel.linear;
vels.angvel += dangvel;
*vels = vels.apply_damping(params.dt, &damping);
});
}
}
}
/*
* Writeback impulses.
*/

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

@@ -496,6 +496,7 @@ impl SolverConstraints<AnyJointVelocityConstraint, ()> {
&mut j_id,
&mut self.generic_jacobians,
&mut self.velocity_constraints,
true,
);
}
}
@@ -526,6 +527,7 @@ impl SolverConstraints<AnyJointVelocityConstraint, ()> {
impulse_joints,
bodies,
&mut self.velocity_constraints,
true,
);
}
}
@@ -554,6 +556,7 @@ impl SolverConstraints<AnyJointVelocityConstraint, ()> {
j_id,
&mut self.generic_jacobians,
&mut self.velocity_constraints,
true,
);
}
}
@@ -582,6 +585,7 @@ impl SolverConstraints<AnyJointVelocityConstraint, ()> {
j_id,
&mut self.generic_jacobians,
&mut self.velocity_constraints,
true,
)
}
}
@@ -611,6 +615,7 @@ impl SolverConstraints<AnyJointVelocityConstraint, ()> {
j_id,
&mut self.generic_jacobians,
&mut self.velocity_constraints,
true,
)
}
}
@@ -640,6 +645,7 @@ impl SolverConstraints<AnyJointVelocityConstraint, ()> {
impulse_joints,
bodies,
&mut self.velocity_constraints,
true,
);
}
}

38
src/dynamics/solver/velocity_solver.rs
View File

@@ -3,13 +3,12 @@ use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
use crate::dynamics::solver::AnyGenericVelocityConstraint;
use crate::dynamics::{
solver::{AnyVelocityConstraint, DeltaVel},
IntegrationParameters, JointGraphEdge, MultibodyJointSet, RigidBodyForces, RigidBodyType,
IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodyDamping,
RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
RigidBodyVelocity,
};
use crate::dynamics::{IslandManager, RigidBodyIds, RigidBodyMassProps};
use crate::geometry::ContactManifold;
use crate::math::Real;
use crate::prelude::{RigidBodyDamping, RigidBodyPosition};
use crate::utils::WAngularInertia;
use na::DVector;
@@ -151,7 +150,7 @@ impl VelocitySolver {
}
}
// Update positions.
// Integrate positions.
for handle in islands.active_island(island_id) {
if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
let multibody = multibodies
@@ -169,30 +168,33 @@ impl VelocitySolver {
multibody.velocities = prev_vels;
}
} else {
let (ids, mprops): (&RigidBodyIds, &RigidBodyMassProps) =
let (rb_ids, rb_mprops): (&RigidBodyIds, &RigidBodyMassProps) =
bodies.index_bundle(handle.0);
let dvel = self.mj_lambdas[ids.active_set_offset];
let dangvel = mprops
let dvel = self.mj_lambdas[rb_ids.active_set_offset];
let dangvel = rb_mprops
.effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular);
// Update positions.
let (poss, vels, damping, mprops): (
let (rb_pos, rb_vels, rb_damping, rb_mprops): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyDamping,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let mut new_poss = *poss;
let mut new_vels = *vels;
let mut new_pos = *rb_pos;
let mut new_vels = *rb_vels;
new_vels.linvel += dvel.linear;
new_vels.angvel += dangvel;
new_vels = new_vels.apply_damping(params.dt, damping);
new_poss.next_position =
new_vels.integrate(params.dt, &poss.position, &mprops.local_mprops.local_com);
bodies.set_internal(handle.0, new_poss);
new_vels = new_vels.apply_damping(params.dt, rb_damping);
new_pos.next_position = new_vels.integrate(
params.dt,
&rb_pos.position,
&rb_mprops.local_mprops.local_com,
);
bodies.set_internal(handle.0, new_pos);
}
}
@@ -260,17 +262,17 @@ impl VelocitySolver {
multibody.velocities += mj_lambdas;
}
} else {
let (ids, damping, mprops): (
let (rb_ids, rb_damping, rb_mprops): (
&RigidBodyIds,
&RigidBodyDamping,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let dvel = self.mj_lambdas[ids.active_set_offset];
let dangvel = mprops
let dvel = self.mj_lambdas[rb_ids.active_set_offset];
let dangvel = rb_mprops
.effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular);
let damping = *damping; // To avoid borrow issues.
let damping = *rb_damping; // To avoid borrow issues.
bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| {
vels.linvel += dvel.linear;

1
src_testbed/physx_backend.rs
View File

@@ -311,7 +311,6 @@ impl PhysxWorld {
);
}
} else if !parent_body.is_dynamic() {
println!("Ground collider");
let actor = rapier2static.get_mut(&parent_handle).unwrap();
actor.attach_shape(&mut px_shape);
} else {