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

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