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

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