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

Complete the parallel solver fix

Browse Source
This commit is contained in:
Sébastien Crozet
2022-03-06 10:59:29 +01:00
committed by Sébastien Crozet
parent 2e6f133b95
commit 815de4beff
18 changed files with 226 additions and 153 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
.vscode/tasks.json vendored
View File

@@ -56,6 +56,21 @@
],
"group": "build"
},
{
"label": "run 3d (simd - parallel - debug) ",
"type": "shell",
"command": "cargo",
"args": [
"run",
"--bin",
"all_examples3",
"--features",
"simd-stable,parallel",
"--",
"--pause"
],
"group": "build"
},
{
"label": "run 2d (no-simd - release) ",
"type": "shell",

3
Cargo.toml
View File

@@ -27,6 +27,9 @@ resolver = "2"
#opt-level = 1
#lto = true
[profile.dev]
opt-level = 1
#[profile.dev.package.rapier3d]
#opt-level = 3

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

@@ -15,5 +15,4 @@ pub struct ImpulseJoint {
// A joint needs to know its handle to simplify its removal.
pub(crate) handle: ImpulseJointHandle,
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),
constraint_index: 0,
};
let default_id = InteractionGraph::<(), ()>::invalid_graph_index();

41
src/dynamics/joint/multibody_joint/multibody.rs
View File

@@ -970,6 +970,16 @@ impl Multibody {
.any(|link| link.joint().num_velocity_constraints() != 0)
}
#[inline]
pub fn num_active_internal_constraints_and_jacobian_lines(&self) -> (usize, usize) {
let num_constraints: usize = self
.links
.iter()
.map(|l| l.joint().num_velocity_constraints())
.sum();
(num_constraints, num_constraints)
}
#[inline]
pub fn generate_internal_constraints(
&self,
@@ -977,21 +987,32 @@ impl Multibody {
j_id: &mut usize,
jacobians: &mut DVector<Real>,
out: &mut Vec<AnyJointVelocityConstraint>,
mut insert_at: Option<usize>,
) {
let num_constraints: usize = self
.links
.iter()
.map(|l| l.joint().num_velocity_constraints())
.sum();
if !cfg!(feature = "parallel") {
let num_constraints: usize = self
.links
.iter()
.map(|l| l.joint().num_velocity_constraints())
.sum();
let required_jacobian_len = *j_id + num_constraints * self.ndofs * 2;
if jacobians.nrows() < required_jacobian_len {
jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
let required_jacobian_len = *j_id + num_constraints * self.ndofs * 2;
if jacobians.nrows() < required_jacobian_len {
jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
}
}
for link in self.links.iter() {
link.joint()
.velocity_constraints(params, self, link, 0, j_id, jacobians, out);
link.joint().velocity_constraints(
params,
self,
link,
0,
j_id,
jacobians,
out,
&mut insert_at,
);
}
}
}

5
src/dynamics/joint/multibody_joint/multibody_joint.rs
View File

@@ -255,6 +255,7 @@ impl MultibodyJoint {
j_id: &mut usize,
jacobians: &mut DVector<Real>,
constraints: &mut Vec<AnyJointVelocityConstraint>,
insert_at: &mut Option<usize>,
) {
let locked_bits = self.data.locked_axes.bits();
let limit_bits = self.data.limit_axes.bits();
@@ -281,6 +282,7 @@ impl MultibodyJoint {
j_id,
jacobians,
constraints,
insert_at,
);
}
@@ -295,6 +297,7 @@ impl MultibodyJoint {
j_id,
jacobians,
constraints,
insert_at,
);
}
curr_free_dof += 1;
@@ -329,6 +332,7 @@ impl MultibodyJoint {
j_id,
jacobians,
constraints,
insert_at,
);
Some(limits)
} else {
@@ -347,6 +351,7 @@ impl MultibodyJoint {
j_id,
jacobians,
constraints,
insert_at,
);
}
curr_free_dof += 1;

24
src/dynamics/joint/multibody_joint/unit_multibody_joint.rs
View File

@@ -20,6 +20,7 @@ pub fn unit_joint_limit_constraint(
j_id: &mut usize,
jacobians: &mut DVector<Real>,
constraints: &mut Vec<AnyJointVelocityConstraint>,
insert_at: &mut Option<usize>,
) {
let ndofs = multibody.ndofs();
let joint_velocity = multibody.joint_velocity(link);
@@ -60,9 +61,14 @@ pub fn unit_joint_limit_constraint(
writeback_id: WritebackId::Limit(dof_id),
};
constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(
constraint,
));
if let Some(at) = insert_at {
constraints[*at] = AnyJointVelocityConstraint::JointGenericGroundConstraint(constraint);
*at += 1;
} else {
constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(
constraint,
));
}
*j_id += 2 * ndofs;
}
@@ -79,6 +85,7 @@ pub fn unit_joint_motor_constraint(
j_id: &mut usize,
jacobians: &mut DVector<Real>,
constraints: &mut Vec<AnyJointVelocityConstraint>,
insert_at: &mut Option<usize>,
) {
let inv_dt = params.inv_dt();
let ndofs = multibody.ndofs();
@@ -128,8 +135,13 @@ pub fn unit_joint_motor_constraint(
writeback_id: WritebackId::Limit(dof_id),
};
constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(
constraint,
));
if let Some(at) = insert_at {
constraints[*at] = AnyJointVelocityConstraint::JointGenericGroundConstraint(constraint);
*at += 1;
} else {
constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(
constraint,
));
}
*j_id += 2 * ndofs;
}

11
src/dynamics/solver/generic_velocity_constraint.rs
View File

@@ -36,7 +36,7 @@ impl GenericVelocityConstraint {
out_constraints: &mut Vec<AnyVelocityConstraint>,
jacobians: &mut DVector<Real>,
jacobian_id: &mut usize,
push: bool,
insert_at: Option<usize>,
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
@@ -99,7 +99,7 @@ impl GenericVelocityConstraint {
let required_jacobian_len =
*jacobian_id + manifold.data.solver_contacts.len() * multibodies_ndof * 2 * DIM;
if jacobians.nrows() < required_jacobian_len {
if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
}
@@ -320,11 +320,10 @@ impl GenericVelocityConstraint {
generic_constraint_mask,
};
if push {
out_constraints.push(AnyVelocityConstraint::NongroupedGeneric(constraint));
if let Some(at) = insert_at {
out_constraints[at + _l] = AnyVelocityConstraint::NongroupedGeneric(constraint);
} else {
out_constraints[manifold.data.constraint_index + _l] =
AnyVelocityConstraint::NongroupedGeneric(constraint);
out_constraints.push(AnyVelocityConstraint::NongroupedGeneric(constraint));
}
}
}

12
src/dynamics/solver/generic_velocity_ground_constraint.rs
View File

@@ -34,7 +34,7 @@ impl GenericVelocityGroundConstraint {
out_constraints: &mut Vec<AnyVelocityConstraint>,
jacobians: &mut DVector<Real>,
jacobian_id: &mut usize,
push: bool,
insert_at: Option<usize>,
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
@@ -87,7 +87,7 @@ impl GenericVelocityGroundConstraint {
let required_jacobian_len =
*jacobian_id + manifold.data.solver_contacts.len() * multibodies_ndof * 2 * DIM;
if jacobians.nrows() < required_jacobian_len {
if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
}
@@ -200,11 +200,11 @@ impl GenericVelocityGroundConstraint {
ndofs2: mb2.ndofs(),
};
if push {
out_constraints.push(AnyVelocityConstraint::NongroupedGenericGround(constraint));
} else {
out_constraints[manifold.data.constraint_index + _l] =
if let Some(at) = insert_at {
out_constraints[at + _l] =
AnyVelocityConstraint::NongroupedGenericGround(constraint);
} else {
out_constraints.push(AnyVelocityConstraint::NongroupedGenericGround(constraint));
}
}
}

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

@@ -131,14 +131,14 @@ impl ParallelInteractionGroups {
(true, false) => {
let rb_ids2: &RigidBodyIds = bodies.index(body_pair.1.unwrap().0);
let color_mask = bcolors[rb_ids2.active_set_offset];
*color = (!color_mask).leading_zeros() as usize;
*color = 127 - (!color_mask).leading_zeros() as usize;
color_len[*color] += 1;
bcolors[rb_ids2.active_set_offset] |= 1 << *color;
}
(false, true) => {
let rb_ids1: &RigidBodyIds = bodies.index(body_pair.0.unwrap().0);
let color_mask = bcolors[rb_ids1.active_set_offset];
*color = (!color_mask).leading_zeros() as usize;
*color = 127 - (!color_mask).leading_zeros() as usize;
color_len[*color] += 1;
bcolors[rb_ids1.active_set_offset] |= 1 << *color;
}

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

@@ -30,20 +30,21 @@ pub enum AnyJointVelocityConstraint {
impl AnyJointVelocityConstraint {
#[cfg(feature = "parallel")]
pub fn num_active_constraints(joint: &ImpulseJoint) -> usize {
pub fn num_active_constraints_and_jacobian_lines(joint: &ImpulseJoint) -> (usize, 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
let num_constraints = (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
+ ((limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0) as usize;
(num_constraints, num_constraints)
}
pub fn from_joint<Bodies>(
@@ -55,7 +56,7 @@ impl AnyJointVelocityConstraint {
j_id: &mut usize,
jacobians: &mut DVector<Real>,
out: &mut Vec<Self>,
push: bool,
insert_at: Option<usize>,
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
@@ -122,7 +123,7 @@ impl AnyJointVelocityConstraint {
// TODO: is this count correct when we take both motors and limits into account?
let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM;
if jacobians.nrows() < required_jacobian_len {
if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
}
@@ -143,14 +144,13 @@ impl AnyJointVelocityConstraint {
&mut out_tmp,
);
if push {
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointGenericConstraint(c));
if let Some(at) = insert_at {
for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
out[at + i] = 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);
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointGenericConstraint(c));
}
}
} else {
@@ -167,14 +167,13 @@ impl AnyJointVelocityConstraint {
&mut out_tmp,
);
if push {
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointConstraint(c));
if let Some(at) = insert_at {
for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
out[at + i] = 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);
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointConstraint(c));
}
}
}
@@ -187,7 +186,7 @@ impl AnyJointVelocityConstraint {
impulse_joints: [&ImpulseJoint; SIMD_WIDTH],
bodies: &Bodies,
out: &mut Vec<Self>,
push: bool,
insert_at: Option<usize>,
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
@@ -260,14 +259,13 @@ impl AnyJointVelocityConstraint {
&mut out_tmp,
);
if push {
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointConstraintSimd(c));
if let Some(at) = insert_at {
for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
out[at + i] = 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);
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointConstraintSimd(c));
}
}
}
@@ -281,7 +279,7 @@ impl AnyJointVelocityConstraint {
j_id: &mut usize,
jacobians: &mut DVector<Real>,
out: &mut Vec<Self>,
push: bool,
insert_at: Option<usize>,
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyType>
@@ -350,7 +348,7 @@ impl AnyJointVelocityConstraint {
// TODO: is this count correct when we take both motors and limits into account?
let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM;
if jacobians.nrows() < required_jacobian_len {
if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
}
@@ -370,14 +368,13 @@ impl AnyJointVelocityConstraint {
&mut out_tmp,
);
if push {
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(c));
if let Some(at) = insert_at {
for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
out[at + i] = 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);
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(c));
}
}
} else {
@@ -394,14 +391,13 @@ impl AnyJointVelocityConstraint {
&mut out_tmp,
);
if push {
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointGroundConstraint(c));
if let Some(at) = insert_at {
for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
out[at + i] = 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);
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointGroundConstraint(c));
}
}
}
@@ -414,7 +410,7 @@ impl AnyJointVelocityConstraint {
impulse_joints: [&ImpulseJoint; SIMD_WIDTH],
bodies: &Bodies,
out: &mut Vec<Self>,
push: bool,
insert_at: Option<usize>,
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyType>
@@ -506,14 +502,13 @@ impl AnyJointVelocityConstraint {
&mut out_tmp,
);
if push {
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointGroundConstraintSimd(c));
if let Some(at) = insert_at {
for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
out[at + i] = 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);
for c in out_tmp.into_iter().take(out_tmp_len) {
out.push(AnyJointVelocityConstraint::JointGroundConstraintSimd(c));
}
}
}

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

@@ -8,12 +8,12 @@ use crate::dynamics::solver::{
VelocityGroundConstraint,
};
use crate::dynamics::{
ImpulseJoint, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet,
RigidBodyHandle, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
RigidBodyVelocity,
ImpulseJoint, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyIndex,
MultibodyJointSet, RigidBodyHandle, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
RigidBodyType, RigidBodyVelocity,
};
use crate::geometry::ContactManifold;
use crate::math::{Real, SPATIAL_DIM};
use crate::math::{Real, DIM, SPATIAL_DIM};
#[cfg(feature = "simd-is-enabled")]
use crate::{
dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint},
@@ -46,6 +46,7 @@ pub(crate) enum ConstraintDesc {
GroundGrouped([usize; SIMD_WIDTH]),
GenericNongroundNongrouped(usize, usize),
GenericGroundNongrouped(usize, usize),
GenericMultibodyInternal(MultibodyIndex, usize),
}
pub(crate) struct ParallelSolverConstraints<VelocityConstraint> {
@@ -81,10 +82,10 @@ impl<VelocityConstraint> ParallelSolverConstraints<VelocityConstraint> {
macro_rules! impl_init_constraints_group {
($VelocityConstraint: ty, $Interaction: ty,
$categorize: ident, $group: ident,
$data: ident$(.$constraint_index: ident)*,
$body1: ident,
$body2: ident,
$num_active_constraints: path,
$generate_internal_constraints: expr,
$num_active_constraints_and_jacobian_lines: path,
$empty_velocity_constraint: expr $(, $weight: ident)*) => {
impl ParallelSolverConstraints<$VelocityConstraint> {
pub fn init_constraint_groups<Bodies>(
@@ -151,12 +152,11 @@ macro_rules! impl_init_constraints_group {
// 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);
total_num_constraints += $num_active_constraints_and_jacobian_lines(interaction).0;
}
#[cfg(feature = "simd-is-enabled")]
@@ -164,26 +164,24 @@ macro_rules! impl_init_constraints_group {
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(
gather![|ii| interaction_i[ii]],
),
));
total_num_constraints += $num_active_constraints(interaction);
total_num_constraints += $num_active_constraints_and_jacobian_lines(interaction).0;
}
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);
total_num_constraints += $num_active_constraints_and_jacobian_lines(interaction).0;
}
#[cfg(feature = "simd-is-enabled")]
@@ -192,14 +190,13 @@ macro_rules! impl_init_constraints_group {
.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(
gather![|ii| interaction_i[ii]],
),
));
total_num_constraints += $num_active_constraints(interaction);
total_num_constraints += $num_active_constraints_and_jacobian_lines(interaction).0;
}
let multibody_ndofs = |handle| {
@@ -215,38 +212,62 @@ macro_rules! impl_init_constraints_group {
for interaction_i in &self.generic_not_ground_interactions[..] {
let interaction = &mut interactions[*interaction_i]$(.$weight)*;
interaction.$data$(.$constraint_index)* = total_num_constraints;
self.constraint_descs.push((
total_num_constraints,
ConstraintDesc::GenericNongroundNongrouped(*interaction_i, *j_id),
));
let num_constraints = $num_active_constraints(interaction);
let (num_constraints, num_jac_lines) = $num_active_constraints_and_jacobian_lines(interaction);
let ndofs1 = $body1(interaction).map(multibody_ndofs).unwrap_or(0);
let ndofs2 = $body2(interaction).map(multibody_ndofs).unwrap_or(0);
*j_id += num_constraints * (ndofs1 + ndofs2) * 2;
*j_id += (ndofs1 + ndofs2) * 2 * num_jac_lines;
total_num_constraints += num_constraints;
}
for interaction_i in &self.generic_ground_interactions[..] {
let interaction = &mut interactions[*interaction_i]$(.$weight)*;
interaction.$data$(.$constraint_index)* = total_num_constraints;
self.constraint_descs.push((
total_num_constraints,
ConstraintDesc::GenericGroundNongrouped(*interaction_i, *j_id),
));
let num_constraints = $num_active_constraints(interaction);
let (num_constraints, num_jac_lines) = $num_active_constraints_and_jacobian_lines(interaction);
let ndofs1 = $body1(interaction).map(multibody_ndofs).unwrap_or(0);
let ndofs2 = $body2(interaction).map(multibody_ndofs).unwrap_or(0);
*j_id += num_constraints * (ndofs1 + ndofs2) * 2;
*j_id += (ndofs1 + ndofs2) * 2 * num_jac_lines;
total_num_constraints += num_constraints;
}
self.parallel_desc_groups.push(self.constraint_descs.len());
}
if $generate_internal_constraints {
let mut had_any_internal_constraint = false;
for handle in islands.active_island(island_id) {
if let Some(link) = multibodies.rigid_body_link(*handle) {
let multibody = multibodies.get_multibody(link.multibody).unwrap();
if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
let (num_constraints, num_jac_lines) = multibody.num_active_internal_constraints_and_jacobian_lines();
let ndofs = multibody.ndofs();
self.constraint_descs.push((
total_num_constraints,
ConstraintDesc::GenericMultibodyInternal(link.multibody, *j_id)
));
*j_id += ndofs * 2 * num_jac_lines;
total_num_constraints += num_constraints;
had_any_internal_constraint = true;
}
}
}
if had_any_internal_constraint {
self.parallel_desc_groups.push(self.constraint_descs.len());
}
}
// Resize the constraint sets.
self.velocity_constraints.clear();
self.velocity_constraints
@@ -274,10 +295,10 @@ impl_init_constraints_group!(
&mut ContactManifold,
categorize_contacts,
group_manifolds,
data.constraint_index,
manifold_body1,
manifold_body2,
VelocityConstraint::num_active_constraints,
false,
VelocityConstraint::num_active_constraints_and_jacobian_lines,
AnyVelocityConstraint::Empty
);
@@ -286,10 +307,10 @@ impl_init_constraints_group!(
JointGraphEdge,
categorize_joints,
group_joints,
constraint_index,
joint_body1,
joint_body2,
AnyJointVelocityConstraint::num_active_constraints,
true,
AnyJointVelocityConstraint::num_active_constraints_and_jacobian_lines,
AnyJointVelocityConstraint::Empty,
weight
);
@@ -317,32 +338,33 @@ impl ParallelSolverConstraints<AnyVelocityConstraint> {
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);
VelocityConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, Some(desc.0));
}
ConstraintDesc::GroundNongrouped(manifold_id) => {
let manifold = &*manifolds_all[*manifold_id];
VelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, false);
VelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, Some(desc.0));
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::NongroundGrouped(manifold_id) => {
let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]];
WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false);
WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, Some(desc.0));
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::GroundGrouped(manifold_id) => {
let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]];
WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false);
WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, Some(desc.0));
}
ConstraintDesc::GenericNongroundNongrouped(manifold_id, j_id) => {
let mut j_id = *j_id;
let manifold = &*manifolds_all[*manifold_id];
GenericVelocityConstraint::generate(params, *manifold_id, manifold, bodies, multibodies, &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, false);
GenericVelocityConstraint::generate(params, *manifold_id, manifold, bodies, multibodies, &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, Some(desc.0));
}
ConstraintDesc::GenericGroundNongrouped(manifold_id, j_id) => {
let mut j_id = *j_id;
let manifold = &*manifolds_all[*manifold_id];
GenericVelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, multibodies, &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, false);
GenericVelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, multibodies, &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, Some(desc.0));
}
ConstraintDesc::GenericMultibodyInternal(..) => unreachable!()
}
}
}
@@ -372,31 +394,36 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint> {
match &desc.1 {
ConstraintDesc::NongroundNongrouped(joint_id) => {
let joint = &joints_all[*joint_id].weight;
AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, false);
AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
}
ConstraintDesc::GroundNongrouped(joint_id) => {
let joint = &joints_all[*joint_id].weight;
AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, false);
AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::NongroundGrouped(joint_id) => {
let impulse_joints = gather![|ii| &joints_all[joint_id[ii]].weight];
AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, false);
AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, Some(desc.0));
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::GroundGrouped(joint_id) => {
let impulse_joints = gather![|ii| &joints_all[joint_id[ii]].weight];
AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, false);
AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, Some(desc.0));
}
ConstraintDesc::GenericNongroundNongrouped(joint_id, j_id) => {
let mut j_id = *j_id;
let joint = &joints_all[*joint_id].weight;
AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, false);
AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
}
ConstraintDesc::GenericGroundNongrouped(joint_id, j_id) => {
let mut j_id = *j_id;
let joint = &joints_all[*joint_id].weight;
AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, false);
AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
}
ConstraintDesc::GenericMultibodyInternal(multibody_id, j_id) => {
let mut j_id = *j_id;
let multibody = multibodies.get_multibody(*multibody_id).unwrap();
multibody.generate_internal_constraints(params, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
}
}
}

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

@@ -189,7 +189,7 @@ impl SolverConstraints<AnyVelocityConstraint> {
manifolds,
bodies,
&mut self.velocity_constraints,
true,
None,
);
}
}
@@ -213,7 +213,7 @@ impl SolverConstraints<AnyVelocityConstraint> {
manifold,
bodies,
&mut self.velocity_constraints,
true,
None,
);
}
}
@@ -243,7 +243,7 @@ impl SolverConstraints<AnyVelocityConstraint> {
&mut self.velocity_constraints,
&mut self.generic_jacobians,
jacobian_id,
true,
None,
);
}
}
@@ -273,7 +273,7 @@ impl SolverConstraints<AnyVelocityConstraint> {
&mut self.velocity_constraints,
&mut self.generic_jacobians,
jacobian_id,
true,
None,
);
}
}
@@ -303,7 +303,7 @@ impl SolverConstraints<AnyVelocityConstraint> {
manifolds,
bodies,
&mut self.velocity_constraints,
true,
None,
);
}
}
@@ -327,7 +327,7 @@ impl SolverConstraints<AnyVelocityConstraint> {
manifold,
bodies,
&mut self.velocity_constraints,
true,
None,
);
}
}
@@ -457,6 +457,7 @@ impl SolverConstraints<AnyJointVelocityConstraint> {
j_id,
&mut self.generic_jacobians,
&mut self.velocity_constraints,
None,
)
}
}
@@ -488,7 +489,7 @@ impl SolverConstraints<AnyJointVelocityConstraint> {
&mut j_id,
&mut self.generic_jacobians,
&mut self.velocity_constraints,
true,
None,
);
}
}
@@ -519,7 +520,7 @@ impl SolverConstraints<AnyJointVelocityConstraint> {
impulse_joints,
bodies,
&mut self.velocity_constraints,
true,
None,
);
}
}
@@ -548,7 +549,7 @@ impl SolverConstraints<AnyJointVelocityConstraint> {
j_id,
&mut self.generic_jacobians,
&mut self.velocity_constraints,
true,
None,
);
}
}
@@ -577,7 +578,7 @@ impl SolverConstraints<AnyJointVelocityConstraint> {
j_id,
&mut self.generic_jacobians,
&mut self.velocity_constraints,
true,
None,
)
}
}
@@ -607,7 +608,7 @@ impl SolverConstraints<AnyJointVelocityConstraint> {
j_id,
&mut self.generic_jacobians,
&mut self.velocity_constraints,
true,
None,
)
}
}
@@ -637,7 +638,7 @@ impl SolverConstraints<AnyJointVelocityConstraint> {
impulse_joints,
bodies,
&mut self.velocity_constraints,
true,
None,
);
}
}

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

@@ -136,9 +136,12 @@ pub(crate) struct VelocityConstraint {
impl VelocityConstraint {
#[cfg(feature = "parallel")]
pub fn num_active_constraints(manifold: &ContactManifold) -> usize {
pub fn num_active_constraints_and_jacobian_lines(manifold: &ContactManifold) -> (usize, usize) {
let rest = manifold.data.solver_contacts.len() % MAX_MANIFOLD_POINTS != 0;
manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS + rest as usize
(
manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS + rest as usize,
manifold.data.solver_contacts.len() * DIM,
)
}
pub fn generate<Bodies>(
@@ -147,7 +150,7 @@ impl VelocityConstraint {
manifold: &ContactManifold,
bodies: &Bodies,
out_constraints: &mut Vec<AnyVelocityConstraint>,
push: bool,
insert_at: Option<usize>,
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
@@ -209,7 +212,7 @@ impl VelocityConstraint {
// NOTE: impulse_joints have the same problem, but it is not easy to refactor the code that way
// for the moment.
#[cfg(target_arch = "wasm32")]
let constraint = if push {
let constraint = if insert_at.is_none() {
let new_len = out_constraints.len() + 1;
unsafe {
out_constraints.resize_with(new_len, || {
@@ -331,11 +334,10 @@ impl VelocityConstraint {
}
#[cfg(not(target_arch = "wasm32"))]
if push {
out_constraints.push(AnyVelocityConstraint::Nongrouped(constraint));
if let Some(at) = insert_at {
out_constraints[at + _l] = AnyVelocityConstraint::Nongrouped(constraint);
} else {
out_constraints[manifold.data.constraint_index + _l] =
AnyVelocityConstraint::Nongrouped(constraint);
out_constraints.push(AnyVelocityConstraint::Nongrouped(constraint));
}
}
}

10
src/dynamics/solver/velocity_constraint_wide.rs
View File

@@ -36,7 +36,7 @@ impl WVelocityConstraint {
manifolds: [&ContactManifold; SIMD_WIDTH],
bodies: &Bodies,
out_constraints: &mut Vec<AnyVelocityConstraint>,
push: bool,
insert_at: Option<usize>,
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
@@ -190,11 +190,11 @@ impl WVelocityConstraint {
}
}
if push {
out_constraints.push(AnyVelocityConstraint::Grouped(constraint));
} else {
out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
if let Some(at) = insert_at {
out_constraints[at + l / MAX_MANIFOLD_POINTS] =
AnyVelocityConstraint::Grouped(constraint);
} else {
out_constraints.push(AnyVelocityConstraint::Grouped(constraint));
}
}
}

11
src/dynamics/solver/velocity_ground_constraint.rs
View File

@@ -33,7 +33,7 @@ impl VelocityGroundConstraint {
manifold: &ContactManifold,
bodies: &Bodies,
out_constraints: &mut Vec<AnyVelocityConstraint>,
push: bool,
insert_at: Option<usize>,
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
@@ -104,7 +104,7 @@ impl VelocityGroundConstraint {
// NOTE: impulse_joints have the same problem, but it is not easy to refactor the code that way
// for the moment.
#[cfg(target_arch = "wasm32")]
let constraint = if push {
let constraint = if insert_at.is_none() {
let new_len = out_constraints.len() + 1;
unsafe {
out_constraints.resize_with(new_len, || {
@@ -212,11 +212,10 @@ impl VelocityGroundConstraint {
}
#[cfg(not(target_arch = "wasm32"))]
if push {
out_constraints.push(AnyVelocityConstraint::NongroupedGround(constraint));
if let Some(at) = insert_at {
out_constraints[at + _l] = AnyVelocityConstraint::NongroupedGround(constraint);
} else {
out_constraints[manifold.data.constraint_index + _l] =
AnyVelocityConstraint::NongroupedGround(constraint);
out_constraints.push(AnyVelocityConstraint::NongroupedGround(constraint));
}
}
}

10
src/dynamics/solver/velocity_ground_constraint_wide.rs
View File

@@ -35,7 +35,7 @@ impl WVelocityGroundConstraint {
manifolds: [&ContactManifold; SIMD_WIDTH],
bodies: &Bodies,
out_constraints: &mut Vec<AnyVelocityConstraint>,
push: bool,
insert_at: Option<usize>,
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
@@ -188,11 +188,11 @@ impl WVelocityGroundConstraint {
}
}
if push {
out_constraints.push(AnyVelocityConstraint::GroupedGround(constraint));
} else {
out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
if let Some(at) = insert_at {
out_constraints[at + l / MAX_MANIFOLD_POINTS] =
AnyVelocityConstraint::GroupedGround(constraint);
} else {
out_constraints.push(AnyVelocityConstraint::GroupedGround(constraint));
}
}
}

4
src/geometry/contact_pair.rs
View File

@@ -122,9 +122,6 @@ pub struct ContactManifoldData {
pub rigid_body1: Option<RigidBodyHandle>,
/// The second rigid-body involved in this contact manifold.
pub rigid_body2: Option<RigidBodyHandle>,
// The two following are set by the constraints solver.
#[cfg_attr(feature = "serde-serialize", serde(skip))]
pub(crate) constraint_index: usize,
// We put the following fields here to avoids reading the colliders inside of the
// contact preparation method.
/// Flags used to control some aspects of the constraints solver for this contact manifold.
@@ -211,7 +208,6 @@ impl ContactManifoldData {
Self {
rigid_body1,
rigid_body2,
constraint_index: 0,
solver_flags,
normal: Vector::zeros(),
solver_contacts: Vec::new(),