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

6
.vscode/settings.json vendored Normal file
View File

@@ -0,0 +1,6 @@
{
"rust-analyzer.cargo.features": [
"simd-stable",
"parallel"
]
}

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

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

@@ -8,8 +8,9 @@ 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;
use super::{
AnyVelocityConstraint, VelocityGroundConstraintElement, VelocityGroundConstraintNormalPart,
};
#[cfg(feature = "dim2")]
use crate::utils::WBasis;
use na::DVector;
@@ -30,7 +31,7 @@ impl GenericVelocityGroundConstraint {
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,
@@ -145,7 +146,7 @@ impl GenericVelocityGroundConstraint {
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 ;
rhs_wo_bias *= is_bouncy + is_resting;
let rhs_bias =
/* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0);
@@ -200,10 +201,10 @@ impl GenericVelocityGroundConstraint {
};
if push {
out_constraints.push(AnyGenericVelocityConstraint::NongroupedGround(constraint));
out_constraints.push(AnyVelocityConstraint::NongroupedGenericGround(constraint));
} else {
out_constraints[manifold.data.constraint_index + _l] =
AnyGenericVelocityConstraint::NongroupedGround(constraint);
AnyVelocityConstraint::NongroupedGenericGround(constraint);
}
}
}

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

@@ -1,7 +1,7 @@
use crate::data::ComponentSet;
#[cfg(feature = "parallel")]
use crate::dynamics::RigidBodyHandle;
use crate::dynamics::{IslandManager, JointGraphEdge, JointIndex, RigidBodyIds};
use crate::dynamics::{IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet, RigidBodyIds};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
#[cfg(feature = "simd-is-enabled")]
use {
@@ -64,6 +64,7 @@ impl ParallelInteractionGroups {
island_id: usize,
islands: &IslandManager,
bodies: &Bodies,
multibodies: &MultibodyJointSet,
interactions: &[Interaction],
interaction_indices: &[usize],
) where
@@ -88,7 +89,7 @@ impl ParallelInteractionGroups {
.iter()
.zip(self.interaction_colors.iter_mut())
{
let body_pair = interactions[*interaction_id].body_pair();
let mut body_pair = interactions[*interaction_id].body_pair();
let is_static1 = body_pair
.0
.map(|b| ComponentSet::<RigidBodyType>::index(bodies, b.0).is_static())
@@ -98,6 +99,24 @@ impl ParallelInteractionGroups {
.map(|b| ComponentSet::<RigidBodyType>::index(bodies, b.0).is_static())
.unwrap_or(true);
let representative = |handle: RigidBodyHandle| {
if let Some(link) = multibodies.rigid_body_link(handle).copied() {
let multibody = multibodies.get_multibody(link.multibody).unwrap();
multibody
.link(1) // Use the link 1 to cover the case where the multibody root is static.
.or(multibody.link(0)) // TODO: Never happens?
.map(|l| l.rigid_body)
.unwrap()
} else {
handle
}
};
body_pair = (
body_pair.0.map(representative),
body_pair.1.map(representative),
);
match (is_static1, is_static2) {
(false, false) => {
let rb_ids1: &RigidBodyIds = bodies.index(body_pair.0.unwrap().0);
@@ -112,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).trailing_zeros() as usize;
*color = (!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).trailing_zeros() as usize;
*color = (!color_mask).leading_zeros() as usize;
color_len[*color] += 1;
bcolors[rb_ids1.active_set_offset] |= 1 << *color;
}
@@ -131,13 +150,11 @@ impl ParallelInteractionGroups {
let mut last_offset = 0;
for i in 0..128 {
if color_len[i] == 0 {
break;
if color_len[i] != 0 {
self.groups.push(last_offset);
sort_offsets[i] = last_offset;
last_offset += color_len[i];
}
self.groups.push(last_offset);
sort_offsets[i] = last_offset;
last_offset += color_len[i];
}
self.sorted_interactions

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

@@ -2,8 +2,7 @@ use super::VelocitySolver;
use crate::counters::Counters;
use crate::data::{ComponentSet, ComponentSetMut};
use crate::dynamics::solver::{
AnyGenericVelocityConstraint, AnyJointVelocityConstraint, AnyVelocityConstraint,
SolverConstraints,
AnyJointVelocityConstraint, AnyVelocityConstraint, SolverConstraints,
};
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyDamping, RigidBodyForces,
@@ -14,8 +13,8 @@ use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::prelude::MultibodyJointSet;
pub struct IslandSolver {
contact_constraints: SolverConstraints<AnyVelocityConstraint, AnyGenericVelocityConstraint>,
joint_constraints: SolverConstraints<AnyJointVelocityConstraint, ()>,
contact_constraints: SolverConstraints<AnyVelocityConstraint>,
joint_constraints: SolverConstraints<AnyJointVelocityConstraint>,
velocity_solver: VelocitySolver,
}
@@ -94,7 +93,6 @@ impl IslandSolver {
manifolds,
impulse_joints,
&mut self.contact_constraints.velocity_constraints,
&mut self.contact_constraints.generic_velocity_constraints,
&self.contact_constraints.generic_jacobians,
&mut self.joint_constraints.velocity_constraints,
&self.joint_constraints.generic_jacobians,

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

@@ -142,9 +142,8 @@ pub struct ParallelIslandSolver {
positions: Vec<Isometry<Real>>,
parallel_groups: ParallelInteractionGroups,
parallel_joint_groups: ParallelInteractionGroups,
parallel_contact_constraints:
ParallelSolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint>,
parallel_joint_constraints: ParallelSolverConstraints<AnyJointVelocityConstraint, ()>,
parallel_contact_constraints: ParallelSolverConstraints<AnyVelocityConstraint>,
parallel_joint_constraints: ParallelSolverConstraints<AnyJointVelocityConstraint>,
thread: ThreadContext,
}
@@ -191,10 +190,14 @@ impl ParallelIslandSolver {
let num_threads = rayon::current_num_threads();
let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island?
self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here?
// Interactions grouping.
let mut j_id = 0;
self.parallel_groups.group_interactions(
island_id,
islands,
bodies,
multibodies,
manifolds,
manifold_indices,
);
@@ -202,9 +205,12 @@ impl ParallelIslandSolver {
island_id,
islands,
bodies,
multibodies,
impulse_joints,
joint_indices,
);
let mut contact_j_id = 0;
self.parallel_contact_constraints.init_constraint_groups(
island_id,
islands,
@@ -212,7 +218,9 @@ impl ParallelIslandSolver {
multibodies,
manifolds,
&self.parallel_groups,
&mut contact_j_id,
);
let mut joint_j_id = 0;
self.parallel_joint_constraints.init_constraint_groups(
island_id,
islands,
@@ -220,12 +228,49 @@ impl ParallelIslandSolver {
multibodies,
impulse_joints,
&self.parallel_joint_groups,
&mut joint_j_id,
);
self.velocity_solver.mj_lambdas.clear();
self.velocity_solver
.mj_lambdas
.resize(islands.active_island(island_id).len(), DeltaVel::zero());
if self.parallel_contact_constraints.generic_jacobians.len() < contact_j_id {
self.parallel_contact_constraints.generic_jacobians = DVector::zeros(contact_j_id);
} else {
self.parallel_contact_constraints.generic_jacobians.fill(0.0);
}
if self.parallel_joint_constraints.generic_jacobians.len() < joint_j_id {
self.parallel_joint_constraints.generic_jacobians = DVector::zeros(joint_j_id);
} else {
self.parallel_joint_constraints.generic_jacobians.fill(0.0);
}
// Init solver ids for multibodies.
{
let mut solver_id = 0;
let island_range = islands.active_island_range(island_id);
let active_bodies = &islands.active_dynamic_set[island_range];
for handle in active_bodies {
if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
let multibody = multibodies
.get_multibody_mut_internal(link.multibody)
.unwrap();
if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
multibody.solver_id = solver_id;
solver_id += multibody.ndofs();
}
}
}
if self.velocity_solver.generic_mj_lambdas.len() < solver_id {
self.velocity_solver.generic_mj_lambdas = DVector::zeros(solver_id);
} else {
self.velocity_solver.generic_mj_lambdas.fill(0.0);
}
self.velocity_solver.mj_lambdas.clear();
self.velocity_solver
.mj_lambdas
.resize(islands.active_island(island_id).len(), DeltaVel::zero());
}
for _ in 0..num_task_per_island {
// We use AtomicPtr because it is Send+Sync while *mut is not.
@@ -254,10 +299,10 @@ impl ParallelIslandSolver {
unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) };
let impulse_joints: &mut Vec<JointGraphEdge> =
unsafe { std::mem::transmute(impulse_joints.load(Ordering::Relaxed)) };
let parallel_contact_constraints: &mut ParallelSolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> = unsafe {
let parallel_contact_constraints: &mut ParallelSolverConstraints<AnyVelocityConstraint> = unsafe {
std::mem::transmute(parallel_contact_constraints.load(Ordering::Relaxed))
};
let parallel_joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint, ()> = unsafe {
let parallel_joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint> = unsafe {
std::mem::transmute(parallel_joint_constraints.load(Ordering::Relaxed))
};
@@ -271,13 +316,28 @@ impl ParallelIslandSolver {
concurrent_loop! {
let batch_size = thread.batch_size;
for handle in active_bodies[thread.body_force_integration_index, thread.num_force_integrated_bodies] {
let (rb_ids, rb_forces, rb_mass_props): (&RigidBodyIds, &RigidBodyForces, &RigidBodyMassProps) = bodies.index_bundle(handle.0);
let dvel = &mut velocity_solver.mj_lambdas[rb_ids.active_set_offset];
// NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
// by the square root of the inertia tensor:
dvel.angular += rb_mass_props.effective_world_inv_inertia_sqrt * rb_forces.torque * params.dt;
dvel.linear += rb_forces.force.component_mul(&rb_mass_props.effective_inv_mass) * params.dt;
if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
let multibody = multibodies
.get_multibody_mut_internal(link.multibody)
.unwrap();
if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
let mut mj_lambdas = velocity_solver
.generic_mj_lambdas
.rows_mut(multibody.solver_id, multibody.ndofs());
mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0);
}
} else {
let (ids, mprops, forces): (&RigidBodyIds, &RigidBodyMassProps, &RigidBodyForces) =
bodies.index_bundle(handle.0);
let dvel = &mut velocity_solver.mj_lambdas[ids.active_set_offset];
// NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
// by the square root of the inertia tensor:
dvel.angular += mprops.effective_world_inv_inertia_sqrt * forces.torque * params.dt;
dvel.linear += forces.force.component_mul(&mprops.effective_inv_mass) * params.dt;
}
}
}
@@ -287,10 +347,8 @@ impl ParallelIslandSolver {
}
let mut j_id = 0; // TODO
let mut jacobians = DVector::zeros(0); // TODO
parallel_contact_constraints.fill_constraints(&thread, params, bodies, manifolds);
parallel_joint_constraints.fill_constraints(&thread, params, bodies, multibodies, impulse_joints, &mut j_id, &mut jacobians);
parallel_contact_constraints.fill_constraints(&thread, params, bodies, multibodies, manifolds);
parallel_joint_constraints.fill_constraints(&thread, params, bodies, multibodies, impulse_joints);
ThreadContext::lock_until_ge(
&thread.num_initialized_constraints,
parallel_contact_constraints.constraint_descs.len(),
@@ -306,6 +364,7 @@ impl ParallelIslandSolver {
island_id,
islands,
bodies,
multibodies,
manifolds,
impulse_joints,
parallel_contact_constraints,

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

@@ -3,13 +3,17 @@ use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, ThreadContext};
use crate::data::ComponentSet;
use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
use crate::dynamics::solver::generic_velocity_constraint::GenericVelocityConstraint;
use crate::dynamics::solver::{InteractionGroups, VelocityConstraint, VelocityGroundConstraint};
use crate::dynamics::solver::{
GenericVelocityGroundConstraint, InteractionGroups, VelocityConstraint,
VelocityGroundConstraint,
};
use crate::dynamics::{
IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
ImpulseJoint, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet,
RigidBodyHandle, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
RigidBodyVelocity,
};
use crate::geometry::ContactManifold;
use crate::math::Real;
use crate::math::{Real, SPATIAL_DIM};
#[cfg(feature = "simd-is-enabled")]
use crate::{
dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint},
@@ -40,9 +44,11 @@ pub(crate) enum ConstraintDesc {
NongroundGrouped([usize; SIMD_WIDTH]),
#[cfg(feature = "simd-is-enabled")]
GroundGrouped([usize; SIMD_WIDTH]),
GenericNongroundNongrouped(usize, usize),
GenericGroundNongrouped(usize, usize),
}
pub(crate) struct ParallelSolverConstraints<VelocityConstraint, GenVelocityConstraint> {
pub(crate) struct ParallelSolverConstraints<VelocityConstraint> {
pub generic_jacobians: DVector<Real>,
pub not_ground_interactions: Vec<usize>,
pub ground_interactions: Vec<usize>,
@@ -51,14 +57,11 @@ pub(crate) struct ParallelSolverConstraints<VelocityConstraint, GenVelocityConst
pub interaction_groups: InteractionGroups,
pub ground_interaction_groups: InteractionGroups,
pub velocity_constraints: Vec<VelocityConstraint>,
pub generic_velocity_constraints: Vec<GenVelocityConstraint>,
pub constraint_descs: Vec<(usize, ConstraintDesc)>,
pub parallel_desc_groups: Vec<usize>,
}
impl<VelocityConstraint, GenVelocityConstraint>
ParallelSolverConstraints<VelocityConstraint, GenVelocityConstraint>
{
impl<VelocityConstraint> ParallelSolverConstraints<VelocityConstraint> {
pub fn new() -> Self {
Self {
generic_jacobians: DVector::zeros(0),
@@ -69,7 +72,6 @@ impl<VelocityConstraint, GenVelocityConstraint>
interaction_groups: InteractionGroups::new(),
ground_interaction_groups: InteractionGroups::new(),
velocity_constraints: vec![],
generic_velocity_constraints: vec![],
constraint_descs: vec![],
parallel_desc_groups: vec![],
}
@@ -77,11 +79,14 @@ impl<VelocityConstraint, GenVelocityConstraint>
}
macro_rules! impl_init_constraints_group {
($VelocityConstraint: ty, $GenVelocityConstraint: ty, $Interaction: ty,
($VelocityConstraint: ty, $Interaction: ty,
$categorize: ident, $group: ident,
$data: ident$(.$constraint_index: ident)*,
$num_active_constraints: path, $empty_velocity_constraint: expr $(, $weight: ident)*) => {
impl ParallelSolverConstraints<$VelocityConstraint, $GenVelocityConstraint> {
$body1: ident,
$body2: ident,
$num_active_constraints: path,
$empty_velocity_constraint: expr $(, $weight: ident)*) => {
impl ParallelSolverConstraints<$VelocityConstraint> {
pub fn init_constraint_groups<Bodies>(
&mut self,
island_id: usize,
@@ -90,6 +95,7 @@ macro_rules! impl_init_constraints_group {
multibodies: &MultibodyJointSet,
interactions: &mut [$Interaction],
interaction_groups: &ParallelInteractionGroups,
j_id: &mut usize,
) where Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds> {
let mut total_num_constraints = 0;
let num_groups = interaction_groups.num_groups();
@@ -105,6 +111,9 @@ macro_rules! impl_init_constraints_group {
self.not_ground_interactions.clear();
self.ground_interactions.clear();
self.generic_not_ground_interactions.clear();
self.generic_ground_interactions.clear();
$categorize(
bodies,
multibodies,
@@ -119,6 +128,7 @@ macro_rules! impl_init_constraints_group {
#[cfg(feature = "simd-is-enabled")]
let start_grouped = self.interaction_groups.grouped_interactions.len();
let start_nongrouped = self.interaction_groups.nongrouped_interactions.len();
#[cfg(feature = "simd-is-enabled")]
let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len();
let start_nongrouped_ground = self.ground_interaction_groups.nongrouped_interactions.len();
@@ -192,6 +202,48 @@ macro_rules! impl_init_constraints_group {
total_num_constraints += $num_active_constraints(interaction);
}
let multibody_ndofs = |handle| {
if let Some(link) = multibodies.rigid_body_link(handle).copied() {
let multibody = multibodies
.get_multibody(link.multibody)
.unwrap();
multibody.ndofs()
} else {
SPATIAL_DIM
}
};
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 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;
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 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;
total_num_constraints += num_constraints;
}
self.parallel_desc_groups.push(self.constraint_descs.len());
}
@@ -204,41 +256,58 @@ macro_rules! impl_init_constraints_group {
}
}
fn joint_body1(joint: &ImpulseJoint) -> Option<RigidBodyHandle> {
Some(joint.body1)
}
fn joint_body2(joint: &ImpulseJoint) -> Option<RigidBodyHandle> {
Some(joint.body2)
}
fn manifold_body1(manifold: &ContactManifold) -> Option<RigidBodyHandle> {
manifold.data.rigid_body1
}
fn manifold_body2(manifold: &ContactManifold) -> Option<RigidBodyHandle> {
manifold.data.rigid_body2
}
impl_init_constraints_group!(
AnyVelocityConstraint,
GenericVelocityConstraint,
&mut ContactManifold,
categorize_contacts,
group_manifolds,
data.constraint_index,
manifold_body1,
manifold_body2,
VelocityConstraint::num_active_constraints,
AnyVelocityConstraint::Empty
);
impl_init_constraints_group!(
AnyJointVelocityConstraint,
(),
JointGraphEdge,
categorize_joints,
group_joints,
constraint_index,
joint_body1,
joint_body2,
AnyJointVelocityConstraint::num_active_constraints,
AnyJointVelocityConstraint::Empty,
weight
);
impl ParallelSolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> {
impl ParallelSolverConstraints<AnyVelocityConstraint> {
pub fn fill_constraints<Bodies>(
&mut self,
thread: &ThreadContext,
params: &IntegrationParameters,
bodies: &Bodies,
multibodies: &MultibodyJointSet,
manifolds_all: &[&mut ContactManifold],
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyType>,
{
let descs = &self.constraint_descs;
@@ -264,13 +333,23 @@ impl ParallelSolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint>
let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]];
WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false);
}
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);
}
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);
}
}
}
}
}
}
impl ParallelSolverConstraints<AnyJointVelocityConstraint, ()> {
impl ParallelSolverConstraints<AnyJointVelocityConstraint> {
pub fn fill_constraints<Bodies>(
&mut self,
thread: &ThreadContext,
@@ -278,8 +357,6 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, ()> {
bodies: &Bodies,
multibodies: &MultibodyJointSet,
joints_all: &[JointGraphEdge],
j_id: &mut usize,
jacobians: &mut DVector<Real>,
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
@@ -295,11 +372,11 @@ 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, j_id, 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, false);
}
ConstraintDesc::GroundNongrouped(joint_id) => {
let joint = &joints_all[*joint_id].weight;
AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, j_id, 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, false);
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::NongroundGrouped(joint_id) => {
@@ -311,6 +388,16 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, ()> {
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);
}
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);
}
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);
}
}
}
}

150
src/dynamics/solver/parallel_velocity_solver.rs
View File

@@ -3,8 +3,9 @@ use crate::concurrent_loop;
use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
use crate::dynamics::{
solver::{GenericVelocityConstraint, ParallelSolverConstraints},
IntegrationParameters, IslandManager, JointGraphEdge, RigidBodyDamping, RigidBodyForces,
RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodyDamping,
RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
RigidBodyVelocity,
};
use crate::geometry::ContactManifold;
use crate::math::Real;
@@ -33,13 +34,11 @@ impl ParallelVelocitySolver {
island_id: usize,
islands: &IslandManager,
bodies: &mut Bodies,
multibodies: &mut MultibodyJointSet,
manifolds_all: &mut [&mut ContactManifold],
joints_all: &mut [JointGraphEdge],
contact_constraints: &mut ParallelSolverConstraints<
AnyVelocityConstraint,
GenericVelocityConstraint,
>,
joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint, ()>,
contact_constraints: &mut ParallelSolverConstraints<AnyVelocityConstraint>,
joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint>,
) where
Bodies: ComponentSet<RigidBodyForces>
+ ComponentSet<RigidBodyIds>
@@ -121,7 +120,7 @@ impl ParallelVelocitySolver {
let solve_friction = params.interleave_restitution_and_friction_resolution
&& params.max_velocity_friction_iterations + i
>= params.max_velocity_iterations;
// Solve joints.
solve!(
joint_constraints,
&joint_constraints.generic_jacobians,
@@ -130,10 +129,27 @@ impl ParallelVelocitySolver {
);
shift += joint_descs.len();
start_index -= joint_descs.len();
// Solve rigid-body contacts.
solve!(
contact_constraints,
cfm_factor,
&contact_constraints.generic_jacobians,
&mut self.mj_lambdas,
&mut self.generic_mj_lambdas,
true,
false
);
shift += contact_descs.len();
start_index -= contact_descs.len();
// Solve generic rigid-body contacts.
solve!(
contact_constraints,
cfm_factor,
&contact_constraints.generic_jacobians,
&mut self.mj_lambdas,
&mut self.generic_mj_lambdas,
true,
false
);
@@ -144,7 +160,9 @@ impl ParallelVelocitySolver {
solve!(
contact_constraints,
cfm_factor,
&contact_constraints.generic_jacobians,
&mut self.mj_lambdas,
&mut self.generic_mj_lambdas,
false,
true
);
@@ -167,7 +185,9 @@ impl ParallelVelocitySolver {
solve!(
contact_constraints,
cfm_factor,
&contact_constraints.generic_jacobians,
&mut self.mj_lambdas,
&mut self.generic_mj_lambdas,
false,
true
);
@@ -184,35 +204,54 @@ impl ParallelVelocitySolver {
concurrent_loop! {
let batch_size = thread.batch_size;
for handle in active_bodies[thread.body_integration_pos_index, thread.num_integrated_pos_bodies] {
let (rb_ids, rb_mprops): (&RigidBodyIds, &RigidBodyMassProps) =
bodies.index_bundle(handle.0);
if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
let multibody = multibodies
.get_multibody_mut_internal(link.multibody)
.unwrap();
let dvel = self.mj_lambdas[rb_ids.active_set_offset];
let dangvel = rb_mprops
.effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular);
if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
let mj_lambdas = self
.generic_mj_lambdas
.rows(multibody.solver_id, multibody.ndofs());
let prev_vels = multibody.velocities.clone(); // FIXME: avoid allocations.
multibody.velocities += mj_lambdas;
multibody.integrate(params.dt);
multibody.forward_kinematics(bodies, false);
multibody.velocities = prev_vels;
}
} else {
let (rb_ids, rb_mprops): (&RigidBodyIds, &RigidBodyMassProps) =
bodies.index_bundle(handle.0);
// Update positions.
let (rb_pos, rb_vels, rb_damping, rb_mprops): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyDamping,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let dvel = self.mj_lambdas[rb_ids.active_set_offset];
let dangvel = rb_mprops
.effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular);
let mut new_pos = *rb_pos;
let mut new_vels = *rb_vels;
new_vels.linvel += dvel.linear;
new_vels.angvel += dangvel;
new_vels = new_vels.apply_damping(params.dt, rb_damping);
new_pos.next_position = new_vels.integrate(
params.dt,
&rb_pos.position,
&rb_mprops.local_mprops.local_com,
);
bodies.set_internal(handle.0, new_pos);
// Update positions.
let (rb_pos, rb_vels, rb_damping, rb_mprops): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyDamping,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let mut new_pos = *rb_pos;
let mut new_vels = *rb_vels;
new_vels.linvel += dvel.linear;
new_vels.angvel += dangvel;
new_vels = new_vels.apply_damping(params.dt, rb_damping);
new_pos.next_position = new_vels.integrate(
params.dt,
&rb_pos.position,
&rb_mprops.local_mprops.local_com,
);
bodies.set_internal(handle.0, new_pos);
}
}
}
ThreadContext::lock_until_ge(&thread.num_integrated_pos_bodies, active_bodies.len());
}
// Remove bias from constraints.
@@ -249,7 +288,9 @@ impl ParallelVelocitySolver {
solve!(
contact_constraints,
cfm_factor,
&contact_constraints.generic_jacobians,
&mut self.mj_lambdas,
&mut self.generic_mj_lambdas,
true,
false
);
@@ -259,7 +300,9 @@ impl ParallelVelocitySolver {
solve!(
contact_constraints,
cfm_factor,
&contact_constraints.generic_jacobians,
&mut self.mj_lambdas,
&mut self.generic_mj_lambdas,
false,
true
);
@@ -276,23 +319,36 @@ impl ParallelVelocitySolver {
concurrent_loop! {
let batch_size = thread.batch_size;
for handle in active_bodies[thread.body_integration_vel_index, thread.num_integrated_vel_bodies] {
let (rb_ids, rb_damping, rb_mprops): (
&RigidBodyIds,
&RigidBodyDamping,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
let multibody = multibodies
.get_multibody_mut_internal(link.multibody)
.unwrap();
let dvel = self.mj_lambdas[rb_ids.active_set_offset];
let dangvel = rb_mprops
.effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular);
let damping = *rb_damping; // To avoid borrow issues.
if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
let mj_lambdas = self
.generic_mj_lambdas
.rows(multibody.solver_id, multibody.ndofs());
multibody.velocities += mj_lambdas;
}
} else {
let (rb_ids, rb_damping, rb_mprops): (
&RigidBodyIds,
&RigidBodyDamping,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| {
vels.linvel += dvel.linear;
vels.angvel += dangvel;
*vels = vels.apply_damping(params.dt, &damping);
});
let dvel = self.mj_lambdas[rb_ids.active_set_offset];
let dangvel = rb_mprops
.effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular);
let damping = *rb_damping; // To avoid borrow issues.
bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| {
vels.linvel += dvel.linear;
vels.angvel += dangvel;
*vels = vels.apply_damping(params.dt, &damping);
});
}
}
}
}

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

@@ -6,7 +6,6 @@ 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,
@@ -19,7 +18,7 @@ use crate::math::Real;
use crate::math::SIMD_WIDTH;
use na::DVector;
pub(crate) struct SolverConstraints<VelocityConstraint, GenVelocityConstraint> {
pub(crate) struct SolverConstraints<VelocityConstraint> {
pub generic_jacobians: DVector<Real>,
pub not_ground_interactions: Vec<usize>,
pub ground_interactions: Vec<usize>,
@@ -28,12 +27,9 @@ pub(crate) struct SolverConstraints<VelocityConstraint, GenVelocityConstraint> {
pub interaction_groups: InteractionGroups,
pub ground_interaction_groups: InteractionGroups,
pub velocity_constraints: Vec<VelocityConstraint>,
pub generic_velocity_constraints: Vec<GenVelocityConstraint>,
}
impl<VelocityConstraint, GenVelocityConstraint>
SolverConstraints<VelocityConstraint, GenVelocityConstraint>
{
impl<VelocityConstraint> SolverConstraints<VelocityConstraint> {
pub fn new() -> Self {
Self {
generic_jacobians: DVector::zeros(0),
@@ -44,7 +40,6 @@ impl<VelocityConstraint, GenVelocityConstraint>
interaction_groups: InteractionGroups::new(),
ground_interaction_groups: InteractionGroups::new(),
velocity_constraints: vec![],
generic_velocity_constraints: vec![],
}
}
@@ -56,11 +51,10 @@ impl<VelocityConstraint, GenVelocityConstraint>
self.interaction_groups.clear();
self.ground_interaction_groups.clear();
self.velocity_constraints.clear();
self.generic_velocity_constraints.clear();
}
}
impl SolverConstraints<AnyVelocityConstraint, AnyGenericVelocityConstraint> {
impl SolverConstraints<AnyVelocityConstraint> {
pub fn init_constraint_groups<Bodies>(
&mut self,
island_id: usize,
@@ -132,7 +126,6 @@ impl SolverConstraints<AnyVelocityConstraint, AnyGenericVelocityConstraint> {
+ ComponentSet<RigidBodyType>,
{
self.velocity_constraints.clear();
self.generic_velocity_constraints.clear();
self.init_constraint_groups(
island_id,
@@ -247,7 +240,7 @@ impl SolverConstraints<AnyVelocityConstraint, AnyGenericVelocityConstraint> {
manifold,
bodies,
multibody_joints,
&mut self.generic_velocity_constraints,
&mut self.velocity_constraints,
&mut self.generic_jacobians,
jacobian_id,
true,
@@ -277,7 +270,7 @@ impl SolverConstraints<AnyVelocityConstraint, AnyGenericVelocityConstraint> {
manifold,
bodies,
multibody_joints,
&mut self.generic_velocity_constraints,
&mut self.velocity_constraints,
&mut self.generic_jacobians,
jacobian_id,
true,
@@ -340,7 +333,7 @@ impl SolverConstraints<AnyVelocityConstraint, AnyGenericVelocityConstraint> {
}
}
impl SolverConstraints<AnyJointVelocityConstraint, ()> {
impl SolverConstraints<AnyJointVelocityConstraint> {
pub fn init<Bodies>(
&mut self,
island_id: usize,
@@ -375,7 +368,6 @@ impl SolverConstraints<AnyJointVelocityConstraint, ()> {
);
self.velocity_constraints.clear();
self.generic_velocity_constraints.clear();
self.interaction_groups.clear_groups();
self.interaction_groups.group_joints(

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

@@ -1,11 +1,14 @@
use crate::data::{BundleSet, ComponentSet};
use crate::dynamics::solver::VelocityGroundConstraint;
use crate::dynamics::solver::{
GenericVelocityConstraint, GenericVelocityGroundConstraint, VelocityGroundConstraint,
};
#[cfg(feature = "simd-is-enabled")]
use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::{self, WAngularInertia, WBasis, WCross, WDot, WReal};
use na::DVector;
use super::{DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart};
@@ -18,6 +21,8 @@ pub(crate) enum AnyVelocityConstraint {
GroupedGround(WVelocityGroundConstraint),
#[cfg(feature = "simd-is-enabled")]
Grouped(WVelocityConstraint),
NongroupedGenericGround(GenericVelocityGroundConstraint),
NongroupedGeneric(GenericVelocityConstraint),
#[allow(dead_code)] // The Empty variant is only used with parallel code.
Empty,
}
@@ -49,32 +54,51 @@ impl AnyVelocityConstraint {
AnyVelocityConstraint::Grouped(c) => c.remove_bias_from_rhs(),
#[cfg(feature = "simd-is-enabled")]
AnyVelocityConstraint::GroupedGround(c) => c.remove_bias_from_rhs(),
AnyVelocityConstraint::Empty => {}
AnyVelocityConstraint::NongroupedGeneric(c) => c.remove_bias_from_rhs(),
AnyVelocityConstraint::NongroupedGenericGround(c) => c.remove_bias_from_rhs(),
AnyVelocityConstraint::Empty => unreachable!(),
}
}
pub fn solve(
&mut self,
cfm_factor: Real,
jacobians: &DVector<Real>,
mj_lambdas: &mut [DeltaVel<Real>],
solve_normal: bool,
generic_mj_lambdas: &mut DVector<Real>,
solve_restitution: bool,
solve_friction: bool,
) {
match self {
AnyVelocityConstraint::NongroupedGround(c) => {
c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction)
c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction)
}
AnyVelocityConstraint::Nongrouped(c) => {
c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction)
c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction)
}
#[cfg(feature = "simd-is-enabled")]
AnyVelocityConstraint::GroupedGround(c) => {
c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction)
c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction)
}
#[cfg(feature = "simd-is-enabled")]
AnyVelocityConstraint::Grouped(c) => {
c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction)
c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction)
}
AnyVelocityConstraint::NongroupedGeneric(c) => c.solve(
cfm_factor,
jacobians,
mj_lambdas,
generic_mj_lambdas,
solve_restitution,
solve_friction,
),
AnyVelocityConstraint::NongroupedGenericGround(c) => c.solve(
cfm_factor,
jacobians,
generic_mj_lambdas,
solve_restitution,
solve_friction,
),
AnyVelocityConstraint::Empty => unreachable!(),
}
}
@@ -87,6 +111,8 @@ impl AnyVelocityConstraint {
AnyVelocityConstraint::GroupedGround(c) => c.writeback_impulses(manifold_all),
#[cfg(feature = "simd-is-enabled")]
AnyVelocityConstraint::Grouped(c) => c.writeback_impulses(manifold_all),
AnyVelocityConstraint::NongroupedGeneric(c) => c.writeback_impulses(manifold_all),
AnyVelocityConstraint::NongroupedGenericGround(c) => c.writeback_impulses(manifold_all),
AnyVelocityConstraint::Empty => unreachable!(),
}
}

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

@@ -1,6 +1,5 @@
use super::AnyJointVelocityConstraint;
use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
use crate::dynamics::solver::AnyGenericVelocityConstraint;
use crate::dynamics::{
solver::{AnyVelocityConstraint, DeltaVel},
IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodyDamping,
@@ -35,7 +34,6 @@ impl VelocitySolver {
manifolds_all: &mut [&mut ContactManifold],
joints_all: &mut [JointGraphEdge],
contact_constraints: &mut [AnyVelocityConstraint],
generic_contact_constraints: &mut [AnyGenericVelocityConstraint],
generic_contact_jacobians: &DVector<Real>,
joint_constraints: &mut [AnyJointVelocityConstraint],
generic_joint_jacobians: &DVector<Real>,
@@ -58,22 +56,28 @@ impl VelocitySolver {
// Initialize delta-velocities (`mj_lambdas`) with external forces (gravity etc):
for handle in islands.active_island(island_id) {
let (ids, mprops, forces): (&RigidBodyIds, &RigidBodyMassProps, &RigidBodyForces) =
bodies.index_bundle(handle.0);
if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
let multibody = multibodies
.get_multibody_mut_internal(link.multibody)
.unwrap();
let dvel = &mut self.mj_lambdas[ids.active_set_offset];
if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
let mut mj_lambdas = self
.generic_mj_lambdas
.rows_mut(multibody.solver_id, multibody.ndofs());
mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0);
}
} else {
let (ids, mprops, forces): (&RigidBodyIds, &RigidBodyMassProps, &RigidBodyForces) =
bodies.index_bundle(handle.0);
// NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
// by the square root of the inertia tensor:
dvel.angular += mprops.effective_world_inv_inertia_sqrt * forces.torque * params.dt;
dvel.linear += forces.force.component_mul(&mprops.effective_inv_mass) * params.dt;
}
let dvel = &mut self.mj_lambdas[ids.active_set_offset];
for (_, multibody) in multibodies.multibodies.iter_mut() {
let mut mj_lambdas = self
.generic_mj_lambdas
.rows_mut(multibody.solver_id, multibody.ndofs());
mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0);
// NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
// by the square root of the inertia tensor:
dvel.angular += mprops.effective_world_inv_inertia_sqrt * forces.torque * params.dt;
dvel.linear += forces.force.component_mul(&mprops.effective_inv_mass) * params.dt;
}
}
/*
@@ -92,10 +96,6 @@ impl VelocitySolver {
}
for constraint in &mut *contact_constraints {
constraint.solve(cfm_factor, &mut self.mj_lambdas[..], true, false);
}
for constraint in &mut *generic_contact_constraints {
constraint.solve(
cfm_factor,
generic_contact_jacobians,
@@ -108,10 +108,6 @@ impl VelocitySolver {
if solve_friction {
for constraint in &mut *contact_constraints {
constraint.solve(cfm_factor, &mut self.mj_lambdas[..], false, true);
}
for constraint in &mut *generic_contact_constraints {
constraint.solve(
cfm_factor,
generic_contact_jacobians,
@@ -135,10 +131,6 @@ impl VelocitySolver {
for _ in 0..remaining_friction_iterations {
for constraint in &mut *contact_constraints {
constraint.solve(cfm_factor, &mut self.mj_lambdas[..], false, true);
}
for constraint in &mut *generic_contact_constraints {
constraint.solve(
cfm_factor,
generic_contact_jacobians,
@@ -204,9 +196,6 @@ impl VelocitySolver {
for constraint in &mut *contact_constraints {
constraint.remove_bias_from_rhs();
}
for constraint in &mut *generic_contact_constraints {
constraint.remove_bias_from_rhs();
}
for _ in 0..params.max_stabilization_iterations {
for constraint in &mut *joint_constraints {
@@ -218,10 +207,6 @@ impl VelocitySolver {
}
for constraint in &mut *contact_constraints {
constraint.solve(1.0, &mut self.mj_lambdas[..], true, false);
}
for constraint in &mut *generic_contact_constraints {
constraint.solve(
1.0,
generic_contact_jacobians,
@@ -233,10 +218,6 @@ impl VelocitySolver {
}
for constraint in &mut *contact_constraints {
constraint.solve(1.0, &mut self.mj_lambdas[..], false, true);
}
for constraint in &mut *generic_contact_constraints {
constraint.solve(
1.0,
generic_contact_jacobians,
@@ -290,9 +271,5 @@ impl VelocitySolver {
for constraint in &*contact_constraints {
constraint.writeback_impulses(manifolds_all);
}
for constraint in &*generic_contact_constraints {
constraint.writeback_impulses(manifolds_all);
}
}
}