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

Fix some solver issues

Browse Source 
- Fix the wrong codepath taken  by the solver for contacts involving a collider without parent.
- Properly adress the non-linear treatment of the friction direction
- Simplify the sleeping strategy
- Add an impulse resolution multiplier
This commit is contained in:
Sébastien Crozet
2022-01-16 16:40:59 +01:00
parent 4454a845e9
commit 0703e5527f
43 changed files with 936 additions and 229 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
src/dynamics/solver/categorization.rs
View File

@@ -9,8 +9,8 @@ pub(crate) fn categorize_contacts(
manifold_indices: &[ContactManifoldIndex],
out_ground: &mut Vec<ContactManifoldIndex>,
out_not_ground: &mut Vec<ContactManifoldIndex>,
out_generic: &mut Vec<ContactManifoldIndex>,
_unused: &mut Vec<ContactManifoldIndex>, // Unused but useful to simplify the parallel code.
out_generic_ground: &mut Vec<ContactManifoldIndex>,
out_generic_not_ground: &mut Vec<ContactManifoldIndex>,
) {
for manifold_i in manifold_indices {
let manifold = &manifolds[*manifold_i];
@@ -18,15 +18,19 @@ pub(crate) fn categorize_contacts(
if manifold
.data
.rigid_body1
.map(|rb| multibody_joints.rigid_body_link(rb))
.and_then(|h| multibody_joints.rigid_body_link(h))
.is_some()
|| manifold
.data
.rigid_body1
.map(|rb| multibody_joints.rigid_body_link(rb))
.rigid_body2
.and_then(|h| multibody_joints.rigid_body_link(h))
.is_some()
{
out_generic.push(*manifold_i);
if manifold.data.relative_dominance != 0 {
out_generic_ground.push(*manifold_i);
} else {
out_generic_not_ground.push(*manifold_i);
}
} else if manifold.data.relative_dominance != 0 {
out_ground.push(*manifold_i)
} else {

13
src/dynamics/solver/delta_vel.rs
View File

@@ -1,7 +1,7 @@
use crate::math::{AngVector, Vector, SPATIAL_DIM};
use na::{DVectorSlice, DVectorSliceMut};
use na::{Scalar, SimdRealField};
use std::ops::AddAssign;
use std::ops::{AddAssign, Sub};
#[derive(Copy, Clone, Debug)]
#[repr(C)]
@@ -44,3 +44,14 @@ impl<N: SimdRealField + Copy> AddAssign for DeltaVel<N> {
self.angular += rhs.angular;
}
}
impl<N: SimdRealField + Copy> Sub for DeltaVel<N> {
type Output = Self;
fn sub(self, rhs: Self) -> Self {
DeltaVel {
linear: self.linear - rhs.linear,
angular: self.angular - rhs.angular,
}
}
}

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

@@ -9,10 +9,60 @@ use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::{WAngularInertia, WCross, WDot};
use super::{DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart};
use crate::dynamics::solver::GenericVelocityGroundConstraint;
#[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,
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(
jacobians,
mj_lambdas,
generic_mj_lambdas,
solve_restitution,
solve_friction,
),
AnyGenericVelocityConstraint::NongroupedGround(c) => c.solve(
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,
@@ -31,7 +81,7 @@ impl GenericVelocityConstraint {
manifold: &ContactManifold,
bodies: &Bodies,
multibodies: &MultibodyJointSet,
out_constraints: &mut Vec<GenericVelocityConstraint>,
out_constraints: &mut Vec<AnyGenericVelocityConstraint>,
jacobians: &mut DVector<Real>,
jacobian_id: &mut usize,
push: bool,
@@ -293,6 +343,9 @@ impl GenericVelocityConstraint {
(vel1 - vel2 + manifold_point.tangent_velocity).dot(&tangents1[j]);
constraint.elements[k].tangent_part.rhs[j] = rhs;
// FIXME: in 3D, we should take into account gcross[0].dot(gcross[1])
// in lhs. See the corresponding code on the `velocity_constraint.rs`
// file.
constraint.elements[k].tangent_part.r[j] = r;
}
}
@@ -316,9 +369,10 @@ impl GenericVelocityConstraint {
};
if push {
out_constraints.push(constraint);
out_constraints.push(AnyGenericVelocityConstraint::Nongrouped(constraint));
} else {
out_constraints[manifold.data.constraint_index + _l] = constraint;
out_constraints[manifold.data.constraint_index + _l] =
AnyGenericVelocityConstraint::Nongrouped(constraint);
}
}
}

29
src/dynamics/solver/generic_velocity_constraint_element.rs
View File

@@ -41,7 +41,7 @@ fn tangent_j_id(j_id: usize, ndofs1: usize, ndofs2: usize) -> usize {
impl GenericRhs {
#[inline(always)]
fn dimpulse(
fn dvel(
&self,
j_id: usize,
ndofs: usize,
@@ -110,14 +110,14 @@ impl VelocityConstraintTangentPart<Real> {
#[cfg(feature = "dim2")]
{
let dimpulse_0 = mj_lambda1.dimpulse(
let dvel_0 = mj_lambda1.dvel(
j_id1,
ndofs1,
jacobians,
&tangents1[0],
&self.gcross1[0],
mj_lambdas,
) + mj_lambda2.dimpulse(
) + mj_lambda2.dvel(
j_id2,
ndofs2,
jacobians,
@@ -126,7 +126,7 @@ impl VelocityConstraintTangentPart<Real> {
mj_lambdas,
) + self.rhs[0];
let new_impulse = (self.impulse[0] - self.r[0] * dimpulse_0).simd_clamp(-limit, limit);
let new_impulse = (self.impulse[0] - self.r[0] * dvel_0).simd_clamp(-limit, limit);
let dlambda = new_impulse - self.impulse[0];
self.impulse[0] = new_impulse;
@@ -154,14 +154,14 @@ impl VelocityConstraintTangentPart<Real> {
#[cfg(feature = "dim3")]
{
let dimpulse_0 = mj_lambda1.dimpulse(
let dvel_0 = mj_lambda1.dvel(
j_id1,
ndofs1,
jacobians,
&tangents1[0],
&self.gcross1[0],
mj_lambdas,
) + mj_lambda2.dimpulse(
) + mj_lambda2.dvel(
j_id2,
ndofs2,
jacobians,
@@ -169,14 +169,14 @@ impl VelocityConstraintTangentPart<Real> {
&self.gcross2[0],
mj_lambdas,
) + self.rhs[0];
let dimpulse_1 = mj_lambda1.dimpulse(
let dvel_1 = mj_lambda1.dvel(
j_id1 + j_step,
ndofs1,
jacobians,
&tangents1[1],
&self.gcross1[1],
mj_lambdas,
) + mj_lambda2.dimpulse(
) + mj_lambda2.dvel(
j_id2 + j_step,
ndofs2,
jacobians,
@@ -186,8 +186,8 @@ impl VelocityConstraintTangentPart<Real> {
) + self.rhs[1];
let new_impulse = na::Vector2::new(
self.impulse[0] - self.r[0] * dimpulse_0,
self.impulse[1] - self.r[1] * dimpulse_1,
self.impulse[0] - self.r[0] * dvel_0,
self.impulse[1] - self.r[1] * dvel_1,
);
let new_impulse = new_impulse.cap_magnitude(limit);
@@ -257,12 +257,11 @@ impl VelocityConstraintNormalPart<Real> {
let j_id1 = j_id1(j_id, ndofs1, ndofs2);
let j_id2 = j_id2(j_id, ndofs1, ndofs2);
let dimpulse =
mj_lambda1.dimpulse(j_id1, ndofs1, jacobians, &dir1, &self.gcross1, mj_lambdas)
+ mj_lambda2.dimpulse(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, mj_lambdas)
+ self.rhs;
let dvel = mj_lambda1.dvel(j_id1, ndofs1, jacobians, &dir1, &self.gcross1, mj_lambdas)
+ mj_lambda2.dvel(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, mj_lambdas)
+ self.rhs;
let new_impulse = (self.impulse - self.r * dimpulse).max(0.0);
let new_impulse = (self.impulse - self.r * dvel).max(0.0);
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;

242
src/dynamics/solver/generic_velocity_ground_constraint.rs Normal file
View File

@@ -0,0 +1,242 @@
use crate::data::{BundleSet, ComponentSet};
use crate::dynamics::solver::VelocityGroundConstraint;
use crate::dynamics::{
IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodyType,
RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::WCross;
use super::{VelocityGroundConstraintElement, VelocityGroundConstraintNormalPart};
use crate::dynamics::solver::AnyGenericVelocityConstraint;
#[cfg(feature = "dim2")]
use crate::utils::WBasis;
use na::DVector;
#[derive(Copy, Clone, Debug)]
pub(crate) struct GenericVelocityGroundConstraint {
// We just build the generic constraint on top of the velocity constraint,
// adding some information we can use in the generic case.
pub velocity_constraint: VelocityGroundConstraint,
pub j_id: usize,
pub ndofs2: usize,
}
impl GenericVelocityGroundConstraint {
pub fn generate<Bodies>(
params: &IntegrationParameters,
manifold_id: ContactManifoldIndex,
manifold: &ContactManifold,
bodies: &Bodies,
multibodies: &MultibodyJointSet,
out_constraints: &mut Vec<AnyGenericVelocityConstraint>,
jacobians: &mut DVector<Real>,
jacobian_id: &mut usize,
push: bool,
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyType>,
{
let inv_dt = params.inv_dt();
let erp_inv_dt = params.erp_inv_dt();
let mut handle1 = manifold.data.rigid_body1;
let mut handle2 = manifold.data.rigid_body2;
let flipped = manifold.data.relative_dominance < 0;
let (force_dir1, flipped_multiplier) = if flipped {
std::mem::swap(&mut handle1, &mut handle2);
(manifold.data.normal, -1.0)
} else {
(-manifold.data.normal, 1.0)
};
let (rb_vels1, world_com1) = if let Some(handle1) = handle1 {
let (vels1, mprops1): (&RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle1.0);
(*vels1, mprops1.world_com)
} else {
(RigidBodyVelocity::zero(), Point::origin())
};
let (rb_vels2, rb_mprops2): (&RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle2.unwrap().0);
let (mb2, link_id2) = handle2
.and_then(|h| multibodies.rigid_body_link(h))
.map(|m| (&multibodies[m.multibody], m.id))
.unwrap();
let mj_lambda2 = mb2.solver_id;
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 = super::compute_tangent_contact_directions(
&force_dir1,
&rb_vels1.linvel,
&rb_vels2.linvel,
);
let multibodies_ndof = mb2.ndofs();
// For each solver contact we generate DIM constraints, and each constraints appends
// the multibodies jacobian and weighted jacobians
let required_jacobian_len =
*jacobian_id + manifold.data.solver_contacts.len() * multibodies_ndof * 2 * DIM;
if jacobians.nrows() < required_jacobian_len {
jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
}
for (_l, manifold_points) in manifold
.data
.solver_contacts
.chunks(MAX_MANIFOLD_POINTS)
.enumerate()
{
let chunk_j_id = *jacobian_id;
let mut constraint = VelocityGroundConstraint {
dir1: force_dir1,
#[cfg(feature = "dim3")]
tangent1: tangents1[0],
elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im2: rb_mprops2.effective_inv_mass,
limit: 0.0,
mj_lambda2,
manifold_id,
manifold_contact_id: [0; MAX_MANIFOLD_POINTS],
num_contacts: manifold_points.len() as u8,
};
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
let dp1 = manifold_point.point - world_com1;
let dp2 = manifold_point.point - rb_mprops2.world_com;
let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(dp1);
let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id;
// Normal part.
{
let torque_dir2 = dp2.gcross(-force_dir1);
let inv_r2 = mb2
.fill_jacobians(
link_id2,
-force_dir1,
#[cfg(feature = "dim2")]
na::vector!(torque_dir2),
#[cfg(feature = "dim3")]
torque_dir2,
jacobian_id,
jacobians,
)
.0;
let r = crate::utils::inv(inv_r2);
let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
let is_resting = 1.0 - is_bouncy;
let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
* (vel1 - vel2).dot(&force_dir1);
rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
let rhs_bias =
/* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0);
constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
gcross2: na::zero(), // Unused for generic constraints.
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
impulse: na::zero(),
r,
};
}
// Tangent parts.
{
constraint.elements[k].tangent_part.impulse = na::zero();
for j in 0..DIM - 1 {
let torque_dir2 = dp2.gcross(-tangents1[j]);
let inv_r2 = mb2
.fill_jacobians(
link_id2,
-tangents1[j],
#[cfg(feature = "dim2")]
na::vector![torque_dir2],
#[cfg(feature = "dim3")]
torque_dir2,
jacobian_id,
jacobians,
)
.0;
let r = crate::utils::inv(inv_r2);
let rhs = (vel1 - vel2
+ flipped_multiplier * manifold_point.tangent_velocity)
.dot(&tangents1[j]);
constraint.elements[k].tangent_part.rhs[j] = rhs;
// FIXME: in 3D, we should take into account gcross[0].dot(gcross[1])
// in lhs. See the corresponding code on the `velocity_constraint.rs`
// file.
constraint.elements[k].tangent_part.r[j] = r;
}
}
}
let constraint = GenericVelocityGroundConstraint {
velocity_constraint: constraint,
j_id: chunk_j_id,
ndofs2: mb2.ndofs(),
};
if push {
out_constraints.push(AnyGenericVelocityConstraint::NongroupedGround(constraint));
} else {
out_constraints[manifold.data.constraint_index + _l] =
AnyGenericVelocityConstraint::NongroupedGround(constraint);
}
}
}
pub fn solve(
&mut self,
jacobians: &DVector<Real>,
generic_mj_lambdas: &mut DVector<Real>,
solve_restitution: bool,
solve_friction: bool,
) {
let mj_lambda2 = self.velocity_constraint.mj_lambda2 as usize;
let elements = &mut self.velocity_constraint.elements
[..self.velocity_constraint.num_contacts as usize];
VelocityGroundConstraintElement::generic_solve_group(
elements,
jacobians,
self.velocity_constraint.limit,
self.ndofs2,
self.j_id,
mj_lambda2,
generic_mj_lambdas,
solve_restitution,
solve_friction,
);
}
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
self.velocity_constraint.writeback_impulses(manifolds_all);
}
pub fn remove_bias_from_rhs(&mut self) {
self.velocity_constraint.remove_bias_from_rhs();
}
}

141
src/dynamics/solver/generic_velocity_ground_constraint_element.rs Normal file
View File

@@ -0,0 +1,141 @@
use crate::dynamics::solver::{
VelocityGroundConstraintElement, VelocityGroundConstraintNormalPart,
VelocityGroundConstraintTangentPart,
};
use crate::math::{Real, DIM};
use na::DVector;
#[cfg(feature = "dim2")]
use na::SimdPartialOrd;
impl VelocityGroundConstraintTangentPart<Real> {
#[inline]
pub fn generic_solve(
&mut self,
j_id2: usize,
jacobians: &DVector<Real>,
ndofs2: usize,
limit: Real,
mj_lambda2: usize,
mj_lambdas: &mut DVector<Real>,
) {
#[cfg(feature = "dim2")]
{
let dvel_0 = jacobians
.rows(j_id2, ndofs2)
.dot(&mj_lambdas.rows(mj_lambda2, ndofs2))
+ self.rhs[0];
let new_impulse = (self.impulse[0] - self.r[0] * dvel_0).simd_clamp(-limit, limit);
let dlambda = new_impulse - self.impulse[0];
self.impulse[0] = new_impulse;
mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy(
dlambda,
&jacobians.rows(j_id2 + ndofs2, ndofs2),
1.0,
);
}
#[cfg(feature = "dim3")]
{
let j_step = ndofs2 * 2;
let dvel_0 = jacobians
.rows(j_id2, ndofs2)
.dot(&mj_lambdas.rows(mj_lambda2, ndofs2))
+ self.rhs[0];
let dvel_1 = jacobians
.rows(j_id2 + j_step, ndofs2)
.dot(&mj_lambdas.rows(mj_lambda2, ndofs2))
+ self.rhs[1];
let new_impulse = na::Vector2::new(
self.impulse[0] - self.r[0] * dvel_0,
self.impulse[1] - self.r[1] * dvel_1,
);
let new_impulse = new_impulse.cap_magnitude(limit);
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy(
dlambda[0],
&jacobians.rows(j_id2 + ndofs2, ndofs2),
1.0,
);
mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy(
dlambda[1],
&jacobians.rows(j_id2 + j_step + ndofs2, ndofs2),
1.0,
);
}
}
}
impl VelocityGroundConstraintNormalPart<Real> {
#[inline]
pub fn generic_solve(
&mut self,
j_id2: usize,
jacobians: &DVector<Real>,
ndofs2: usize,
mj_lambda2: usize,
mj_lambdas: &mut DVector<Real>,
) {
let dvel = jacobians
.rows(j_id2, ndofs2)
.dot(&mj_lambdas.rows(mj_lambda2, ndofs2))
+ self.rhs;
let new_impulse = (self.impulse - self.r * dvel).max(0.0);
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy(
dlambda,
&jacobians.rows(j_id2 + ndofs2, ndofs2),
1.0,
);
}
}
impl VelocityGroundConstraintElement<Real> {
#[inline]
pub fn generic_solve_group(
elements: &mut [Self],
jacobians: &DVector<Real>,
limit: Real,
ndofs2: usize,
// Jacobian index of the first constraint.
j_id: usize,
mj_lambda2: usize,
mj_lambdas: &mut DVector<Real>,
solve_restitution: bool,
solve_friction: bool,
) {
let j_step = ndofs2 * 2 * DIM;
// Solve penetration.
if solve_restitution {
let mut nrm_j_id = j_id;
for element in elements.iter_mut() {
element
.normal_part
.generic_solve(nrm_j_id, jacobians, ndofs2, mj_lambda2, mj_lambdas);
nrm_j_id += j_step;
}
}
// Solve friction.
if solve_friction {
let mut tng_j_id = j_id + ndofs2 * 2;
for element in elements.iter_mut() {
let limit = limit * element.normal_part.impulse;
let part = &mut element.tangent_part;
part.generic_solve(tng_j_id, jacobians, ndofs2, limit, mj_lambda2, mj_lambdas);
tng_j_id += j_step;
}
}
}
}

5
src/dynamics/solver/island_solver.rs
View File

@@ -2,7 +2,8 @@ use super::VelocitySolver;
use crate::counters::Counters;
use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
use crate::dynamics::solver::{
AnyJointVelocityConstraint, AnyVelocityConstraint, GenericVelocityConstraint, SolverConstraints,
AnyGenericVelocityConstraint, AnyJointVelocityConstraint, AnyVelocityConstraint,
SolverConstraints,
};
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyDamping, RigidBodyForces,
@@ -13,7 +14,7 @@ use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::prelude::{MultibodyJointSet, RigidBodyActivation};
pub struct IslandSolver {
contact_constraints: SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint>,
contact_constraints: SolverConstraints<AnyVelocityConstraint, AnyGenericVelocityConstraint>,
joint_constraints: SolverConstraints<AnyJointVelocityConstraint, ()>,
velocity_solver: VelocitySolver,
}

3
src/dynamics/solver/mod.rs
View File

@@ -13,6 +13,7 @@ pub(self) use self::velocity_solver::VelocitySolver;
pub(self) use delta_vel::DeltaVel;
pub(self) use generic_velocity_constraint::*;
pub(self) use generic_velocity_constraint_element::*;
pub(self) use generic_velocity_ground_constraint::*;
pub(self) use interaction_groups::*;
pub(crate) use joint_constraint::MotorParameters;
pub use joint_constraint::*;
@@ -29,6 +30,8 @@ mod categorization;
mod delta_vel;
mod generic_velocity_constraint;
mod generic_velocity_constraint_element;
mod generic_velocity_ground_constraint;
mod generic_velocity_ground_constraint_element;
mod interaction_groups;
#[cfg(not(feature = "parallel"))]
mod island_solver;

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

@@ -5,6 +5,8 @@ use super::{
use super::{WVelocityConstraint, WVelocityGroundConstraint};
use crate::data::ComponentSet;
use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
use crate::dynamics::solver::generic_velocity_ground_constraint::GenericVelocityGroundConstraint;
use crate::dynamics::solver::AnyGenericVelocityConstraint;
use crate::dynamics::solver::GenericVelocityConstraint;
use crate::dynamics::{
solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex,
@@ -58,7 +60,7 @@ impl<VelocityConstraint, GenVelocityConstraint>
}
}
impl SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> {
impl SolverConstraints<AnyVelocityConstraint, AnyGenericVelocityConstraint> {
pub fn init_constraint_groups<Bodies>(
&mut self,
island_id: usize,
@@ -82,8 +84,8 @@ impl SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> {
manifold_indices,
&mut self.ground_interactions,
&mut self.not_ground_interactions,
&mut self.generic_not_ground_interactions,
&mut self.generic_ground_interactions,
&mut self.generic_not_ground_interactions,
);
self.interaction_groups.clear_groups();
@@ -141,18 +143,32 @@ impl SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> {
manifold_indices,
);
let mut jacobian_id = 0;
#[cfg(feature = "simd-is-enabled")]
{
self.compute_grouped_constraints(params, bodies, manifolds);
}
self.compute_nongrouped_constraints(params, bodies, manifolds);
self.compute_generic_constraints(params, bodies, multibody_joints, manifolds);
self.compute_generic_constraints(
params,
bodies,
multibody_joints,
manifolds,
&mut jacobian_id,
);
#[cfg(feature = "simd-is-enabled")]
{
self.compute_grouped_ground_constraints(params, bodies, manifolds);
}
self.compute_nongrouped_ground_constraints(params, bodies, manifolds);
self.compute_generic_ground_constraints(
params,
bodies,
multibody_joints,
manifolds,
&mut jacobian_id,
);
}
#[cfg(feature = "simd-is-enabled")]
@@ -215,6 +231,7 @@ impl SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> {
bodies: &Bodies,
multibody_joints: &MultibodyJointSet,
manifolds_all: &[&mut ContactManifold],
jacobian_id: &mut usize,
) where
Bodies: ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyPosition>
@@ -222,7 +239,6 @@ impl SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> {
+ ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyType>,
{
let mut jacobian_id = 0;
for manifold_i in &self.generic_not_ground_interactions {
let manifold = &manifolds_all[*manifold_i];
GenericVelocityConstraint::generate(
@@ -233,7 +249,37 @@ impl SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> {
multibody_joints,
&mut self.generic_velocity_constraints,
&mut self.generic_jacobians,
&mut jacobian_id,
jacobian_id,
true,
);
}
}
fn compute_generic_ground_constraints<Bodies>(
&mut self,
params: &IntegrationParameters,
bodies: &Bodies,
multibody_joints: &MultibodyJointSet,
manifolds_all: &[&mut ContactManifold],
jacobian_id: &mut usize,
) where
Bodies: ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyType>,
{
for manifold_i in &self.generic_ground_interactions {
let manifold = &manifolds_all[*manifold_i];
GenericVelocityGroundConstraint::generate(
params,
*manifold_i,
manifold,
bodies,
multibody_joints,
&mut self.generic_velocity_constraints,
&mut self.generic_jacobians,
jacobian_id,
true,
);
}

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

@@ -236,7 +236,7 @@ impl VelocityConstraint {
.transform_vector(dp2.gcross(-force_dir1));
let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
let r = 1.0
let r = params.delassus_inv_factor
/ (force_dir1.dot(&imsum.component_mul(&force_dir1))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2));
@@ -246,8 +246,7 @@ impl VelocityConstraint {
let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
* (vel1 - vel2).dot(&force_dir1);
rhs_wo_bias +=
(manifold_point.dist + params.allowed_linear_error).max(0.0) * inv_dt;
rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
let rhs_bias = /* is_resting
* */ erp_inv_dt
@@ -275,17 +274,26 @@ impl VelocityConstraint {
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-tangents1[j]));
let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
let r = 1.0
/ (tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2));
let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2);
let rhs =
(vel1 - vel2 + manifold_point.tangent_velocity).dot(&tangents1[j]);
constraint.elements[k].tangent_part.gcross1[j] = gcross1;
constraint.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.elements[k].tangent_part.rhs[j] = rhs;
constraint.elements[k].tangent_part.r[j] = r;
constraint.elements[k].tangent_part.r[j] =
if cfg!(feature = "dim2") { 1.0 / r } else { r };
}
#[cfg(feature = "dim3")]
{
constraint.elements[k].tangent_part.r[2] = 2.0
* (constraint.elements[k].tangent_part.gcross1[0]
.gdot(constraint.elements[k].tangent_part.gcross1[1])
+ constraint.elements[k].tangent_part.gcross2[0]
.gdot(constraint.elements[k].tangent_part.gcross2[1]));
}
}
}

39
src/dynamics/solver/velocity_constraint_element.rs
View File

@@ -12,7 +12,10 @@ pub(crate) struct VelocityConstraintTangentPart<N: SimdRealField + Copy> {
pub impulse: na::Vector1<N>,
#[cfg(feature = "dim3")]
pub impulse: na::Vector2<N>,
pub r: [N; DIM - 1],
#[cfg(feature = "dim2")]
pub r: [N; 1],
#[cfg(feature = "dim3")]
pub r: [N; DIM],
}
impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
@@ -22,7 +25,10 @@ impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
gcross2: [na::zero(); DIM - 1],
rhs: [na::zero(); DIM - 1],
impulse: na::zero(),
r: [na::zero(); DIM - 1],
#[cfg(feature = "dim2")]
r: [na::zero(); 1],
#[cfg(feature = "dim3")]
r: [na::zero(); DIM],
}
}
@@ -41,12 +47,12 @@ impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
{
#[cfg(feature = "dim2")]
{
let dimpulse = tangents1[0].dot(&mj_lambda1.linear)
let dvel = tangents1[0].dot(&mj_lambda1.linear)
+ self.gcross1[0].gdot(mj_lambda1.angular)
- tangents1[0].dot(&mj_lambda2.linear)
+ self.gcross2[0].gdot(mj_lambda2.angular)
+ self.rhs[0];
let new_impulse = (self.impulse[0] - self.r[0] * dimpulse).simd_clamp(-limit, limit);
let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit);
let dlambda = new_impulse - self.impulse[0];
self.impulse[0] = new_impulse;
@@ -59,25 +65,30 @@ impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
#[cfg(feature = "dim3")]
{
let dimpulse_0 = tangents1[0].dot(&mj_lambda1.linear)
let dvel_0 = tangents1[0].dot(&mj_lambda1.linear)
+ self.gcross1[0].gdot(mj_lambda1.angular)
- tangents1[0].dot(&mj_lambda2.linear)
+ self.gcross2[0].gdot(mj_lambda2.angular)
+ self.rhs[0];
let dimpulse_1 = tangents1[1].dot(&mj_lambda1.linear)
let dvel_1 = tangents1[1].dot(&mj_lambda1.linear)
+ self.gcross1[1].gdot(mj_lambda1.angular)
- tangents1[1].dot(&mj_lambda2.linear)
+ self.gcross2[1].gdot(mj_lambda2.angular)
+ self.rhs[1];
let new_impulse = na::Vector2::new(
self.impulse[0] - self.r[0] * dimpulse_0,
self.impulse[1] - self.r[1] * dimpulse_1,
);
let dvel_00 = dvel_0 * dvel_0;
let dvel_11 = dvel_1 * dvel_1;
let dvel_01 = dvel_0 * dvel_1;
let inv_lhs = (dvel_00 + dvel_11)
* crate::utils::simd_inv(
dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2],
);
let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1];
let new_impulse = self.impulse - delta_impulse;
let new_impulse = {
let _disable_fe_except =
crate::utils::DisableFloatingPointExceptionsFlags::
disable_floating_point_exceptions();
crate::utils::DisableFloatingPointExceptionsFlags::
disable_floating_point_exceptions();
new_impulse.simd_cap_magnitude(limit)
};
@@ -128,11 +139,11 @@ impl<N: SimdRealField + Copy> VelocityConstraintNormalPart<N> {
) where
AngVector<N>: WDot<AngVector<N>, Result = N>,
{
let dimpulse = dir1.dot(&mj_lambda1.linear) + self.gcross1.gdot(mj_lambda1.angular)
let dvel = dir1.dot(&mj_lambda1.linear) + self.gcross1.gdot(mj_lambda1.angular)
- dir1.dot(&mj_lambda2.linear)
+ self.gcross2.gdot(mj_lambda2.angular)
+ self.rhs;
let new_impulse = (self.impulse - self.r * dimpulse).simd_max(N::zero());
let new_impulse = (self.impulse - self.r * dvel).simd_max(N::zero());
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;

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

@@ -49,6 +49,7 @@ impl WVelocityConstraint {
let inv_dt = SimdReal::splat(params.inv_dt());
let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
let delassus_inv_factor = SimdReal::splat(params.delassus_inv_factor);
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()];
@@ -136,14 +137,14 @@ impl WVelocityConstraint {
let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
let imsum = im1 + im2;
let r = SimdReal::splat(1.0)
let r = delassus_inv_factor
/ (force_dir1.dot(&imsum.component_mul(&force_dir1))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2));
let projected_velocity = (vel1 - vel2).dot(&force_dir1);
let mut rhs_wo_bias =
(SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
rhs_wo_bias += (dist + allowed_lin_err).simd_max(SimdReal::zero()) * inv_dt;
rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting * velocity_solve_fraction;
let rhs_bias = (dist + allowed_lin_err).simd_min(SimdReal::zero())
* (erp_inv_dt/* * is_resting */);
@@ -165,16 +166,28 @@ impl WVelocityConstraint {
let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j]));
let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
let imsum = im1 + im2;
let r = SimdReal::splat(1.0)
/ (tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2));
let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2);
let rhs = (vel1 - vel2 + tangent_velocity).dot(&tangents1[j]);
constraint.elements[k].tangent_part.gcross1[j] = gcross1;
constraint.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.elements[k].tangent_part.rhs[j] = rhs;
constraint.elements[k].tangent_part.r[j] = r;
constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
SimdReal::splat(1.0) / r
} else {
r
};
}
#[cfg(feature = "dim3")]
{
constraint.elements[k].tangent_part.r[2] = SimdReal::splat(2.0)
* (constraint.elements[k].tangent_part.gcross1[0]
.gdot(constraint.elements[k].tangent_part.gcross1[1])
+ constraint.elements[k].tangent_part.gcross2[0]
.gdot(constraint.elements[k].tangent_part.gcross2[1]));
}
}

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

@@ -153,7 +153,7 @@ impl VelocityGroundConstraint {
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-force_dir1));
let r = 1.0
let r = params.delassus_inv_factor
/ (force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+ gcross2.gdot(gcross2));
@@ -162,8 +162,7 @@ impl VelocityGroundConstraint {
let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
* (vel1 - vel2).dot(&force_dir1);
rhs_wo_bias +=
(manifold_point.dist + params.allowed_linear_error).max(0.0) * inv_dt;
rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
let rhs_bias = /* is_resting
* */ erp_inv_dt
@@ -186,17 +185,24 @@ impl VelocityGroundConstraint {
let gcross2 = mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-tangents1[j]));
let r = 1.0
/ (tangents1[j]
.dot(&mprops2.effective_inv_mass.component_mul(&tangents1[j]))
+ gcross2.gdot(gcross2));
let r = tangents1[j]
.dot(&mprops2.effective_inv_mass.component_mul(&tangents1[j]))
+ gcross2.gdot(gcross2);
let rhs = (vel1 - vel2
+ flipped_multiplier * manifold_point.tangent_velocity)
.dot(&tangents1[j]);
constraint.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.elements[k].tangent_part.rhs[j] = rhs;
constraint.elements[k].tangent_part.r[j] = r;
constraint.elements[k].tangent_part.r[j] =
if cfg!(feature = "dim2") { 1.0 / r } else { r };
}
#[cfg(feature = "dim3")]
{
constraint.elements[k].tangent_part.r[2] = 2.0
* constraint.elements[k].tangent_part.gcross2[0]
.gdot(constraint.elements[k].tangent_part.gcross2[1]);
}
}
}

40
src/dynamics/solver/velocity_ground_constraint_element.rs
View File

@@ -11,17 +11,22 @@ pub(crate) struct VelocityGroundConstraintTangentPart<N: SimdRealField + Copy> {
pub impulse: na::Vector1<N>,
#[cfg(feature = "dim3")]
pub impulse: na::Vector2<N>,
pub r: [N; DIM - 1],
#[cfg(feature = "dim2")]
pub r: [N; 1],
#[cfg(feature = "dim3")]
pub r: [N; DIM],
}
impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
#[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))]
fn zero() -> Self {
Self {
gcross2: [na::zero(); DIM - 1],
rhs: [na::zero(); DIM - 1],
impulse: na::zero(),
r: [na::zero(); DIM - 1],
#[cfg(feature = "dim2")]
r: [na::zero(); 1],
#[cfg(feature = "dim3")]
r: [na::zero(); DIM],
}
}
@@ -38,10 +43,10 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
{
#[cfg(feature = "dim2")]
{
let dimpulse = -tangents1[0].dot(&mj_lambda2.linear)
let dvel = -tangents1[0].dot(&mj_lambda2.linear)
+ self.gcross2[0].gdot(mj_lambda2.angular)
+ self.rhs[0];
let new_impulse = (self.impulse[0] - self.r[0] * dimpulse).simd_clamp(-limit, limit);
let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit);
let dlambda = new_impulse - self.impulse[0];
self.impulse[0] = new_impulse;
@@ -51,17 +56,22 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
#[cfg(feature = "dim3")]
{
let dimpulse_0 = -tangents1[0].dot(&mj_lambda2.linear)
let dvel_0 = -tangents1[0].dot(&mj_lambda2.linear)
+ self.gcross2[0].gdot(mj_lambda2.angular)
+ self.rhs[0];
let dimpulse_1 = -tangents1[1].dot(&mj_lambda2.linear)
let dvel_1 = -tangents1[1].dot(&mj_lambda2.linear)
+ self.gcross2[1].gdot(mj_lambda2.angular)
+ self.rhs[1];
let new_impulse = na::Vector2::new(
self.impulse[0] - self.r[0] * dimpulse_0,
self.impulse[1] - self.r[1] * dimpulse_1,
);
let dvel_00 = dvel_0 * dvel_0;
let dvel_11 = dvel_1 * dvel_1;
let dvel_01 = dvel_0 * dvel_1;
let inv_lhs = (dvel_00 + dvel_11)
* crate::utils::simd_inv(
dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2],
);
let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1];
let new_impulse = self.impulse - delta_impulse;
let new_impulse = {
let _disable_fe_except =
crate::utils::DisableFloatingPointExceptionsFlags::
@@ -69,7 +79,6 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
new_impulse.simd_cap_magnitude(limit)
};
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
@@ -89,7 +98,6 @@ pub(crate) struct VelocityGroundConstraintNormalPart<N: SimdRealField + Copy> {
}
impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
#[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))]
fn zero() -> Self {
Self {
gcross2: na::zero(),
@@ -105,9 +113,8 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
where
AngVector<N>: WDot<AngVector<N>, Result = N>,
{
let dimpulse =
-dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs;
let new_impulse = (self.impulse - self.r * dimpulse).simd_max(N::zero());
let dvel = -dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs;
let new_impulse = (self.impulse - self.r * dvel).simd_max(N::zero());
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
@@ -123,7 +130,6 @@ pub(crate) struct VelocityGroundConstraintElement<N: SimdRealField + Copy> {
}
impl<N: SimdRealField + Copy> VelocityGroundConstraintElement<N> {
#[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))]
pub fn zero() -> Self {
Self {
normal_part: VelocityGroundConstraintNormalPart::zero(),

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

@@ -44,6 +44,7 @@ impl WVelocityGroundConstraint {
let inv_dt = SimdReal::splat(params.inv_dt());
let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
let delassus_inv_factor = SimdReal::splat(params.delassus_inv_factor);
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
let mut handles1 = gather![|ii| manifolds[ii].data.rigid_body1];
@@ -142,12 +143,12 @@ impl WVelocityGroundConstraint {
{
let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
let r = SimdReal::splat(1.0)
let r = delassus_inv_factor
/ (force_dir1.dot(&im2.component_mul(&force_dir1)) + gcross2.gdot(gcross2));
let projected_velocity = (vel1 - vel2).dot(&force_dir1);
let mut rhs_wo_bias =
(SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
rhs_wo_bias += (dist + allowed_lin_err).simd_max(SimdReal::zero()) * inv_dt;
rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting * velocity_solve_fraction;
let rhs_bias = (dist + allowed_lin_err).simd_min(SimdReal::zero())
* (erp_inv_dt/* * is_resting */);
@@ -166,14 +167,24 @@ impl WVelocityGroundConstraint {
for j in 0..DIM - 1 {
let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
let r = SimdReal::splat(1.0)
/ (tangents1[j].dot(&im2.component_mul(&tangents1[j]))
+ gcross2.gdot(gcross2));
let r =
tangents1[j].dot(&im2.component_mul(&tangents1[j])) + gcross2.gdot(gcross2);
let rhs = (vel1 - vel2 + tangent_velocity * flipped_sign).dot(&tangents1[j]);
constraint.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.elements[k].tangent_part.r[j] = r;
constraint.elements[k].tangent_part.rhs[j] = rhs;
constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
SimdReal::splat(1.0) / r
} else {
r
};
}
#[cfg(feature = "dim3")]
{
constraint.elements[k].tangent_part.r[2] = SimdReal::splat(2.0)
* constraint.elements[k].tangent_part.gcross2[0]
.gdot(constraint.elements[k].tangent_part.gcross2[1]);
}
}

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

@@ -1,6 +1,6 @@
use super::AnyJointVelocityConstraint;
use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
use crate::dynamics::solver::GenericVelocityConstraint;
use crate::dynamics::solver::AnyGenericVelocityConstraint;
use crate::dynamics::{
solver::{AnyVelocityConstraint, DeltaVel},
IntegrationParameters, JointGraphEdge, MultibodyJointSet, RigidBodyForces, RigidBodyType,
@@ -36,7 +36,7 @@ impl VelocitySolver {
manifolds_all: &mut [&mut ContactManifold],
joints_all: &mut [JointGraphEdge],
contact_constraints: &mut [AnyVelocityConstraint],
generic_contact_constraints: &mut [GenericVelocityConstraint],
generic_contact_constraints: &mut [AnyGenericVelocityConstraint],
generic_contact_jacobians: &DVector<Real>,
joint_constraints: &mut [AnyJointVelocityConstraint],
generic_joint_jacobians: &DVector<Real>,