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

Second round to fix the parallel solver.

Browse Source
This commit is contained in:
Sébastien Crozet
2022-02-27 22:04:51 +01:00
committed by Sébastien Crozet
parent 28cc19d104
commit 2e6f133b95
11 changed files with 398 additions and 230 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -8,64 +8,13 @@ use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::{WAngularInertia, WCross, WDot};
use super::{DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart};
use crate::dynamics::solver::GenericVelocityGroundConstraint;
use super::{
AnyVelocityConstraint, DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart,
};
#[cfg(feature = "dim2")]
use crate::utils::WBasis;
use na::DVector;
#[derive(Copy, Clone, Debug)]
pub(crate) enum AnyGenericVelocityConstraint {
NongroupedGround(GenericVelocityGroundConstraint),
Nongrouped(GenericVelocityConstraint),
}
impl AnyGenericVelocityConstraint {
pub fn solve(
&mut self,
cfm_factor: Real,
jacobians: &DVector<Real>,
mj_lambdas: &mut [DeltaVel<Real>],
generic_mj_lambdas: &mut DVector<Real>,
solve_restitution: bool,
solve_friction: bool,
) {
match self {
AnyGenericVelocityConstraint::Nongrouped(c) => c.solve(
cfm_factor,
jacobians,
mj_lambdas,
generic_mj_lambdas,
solve_restitution,
solve_friction,
),
AnyGenericVelocityConstraint::NongroupedGround(c) => c.solve(
cfm_factor,
jacobians,
generic_mj_lambdas,
solve_restitution,
solve_friction,
),
}
}
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
match self {
AnyGenericVelocityConstraint::Nongrouped(c) => c.writeback_impulses(manifolds_all),
AnyGenericVelocityConstraint::NongroupedGround(c) => {
c.writeback_impulses(manifolds_all)
}
}
}
pub fn remove_bias_from_rhs(&mut self) {
match self {
AnyGenericVelocityConstraint::Nongrouped(c) => c.remove_bias_from_rhs(),
AnyGenericVelocityConstraint::NongroupedGround(c) => c.remove_bias_from_rhs(),
}
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct GenericVelocityConstraint {
// We just build the generic constraint on top of the velocity constraint,
@@ -84,7 +33,7 @@ impl GenericVelocityConstraint {
manifold: &ContactManifold,
bodies: &Bodies,
multibodies: &MultibodyJointSet,
out_constraints: &mut Vec<AnyGenericVelocityConstraint>,
out_constraints: &mut Vec<AnyVelocityConstraint>,
jacobians: &mut DVector<Real>,
jacobian_id: &mut usize,
push: bool,
@@ -372,10 +321,10 @@ impl GenericVelocityConstraint {
};
if push {
out_constraints.push(AnyGenericVelocityConstraint::Nongrouped(constraint));
out_constraints.push(AnyVelocityConstraint::NongroupedGeneric(constraint));
} else {
out_constraints[manifold.data.constraint_index + _l] =
AnyGenericVelocityConstraint::Nongrouped(constraint);
AnyVelocityConstraint::NongroupedGeneric(constraint);
}
}
}