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

First round deleting the component sets.

Browse Source
This commit is contained in:
Sébastien Crozet
2022-04-19 18:57:40 +02:00
committed by Sébastien Crozet
parent ee679427cd
commit 2b1374c596
36 changed files with 722 additions and 1649 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,9 +1,8 @@
use crate::data::ComponentSet;
use crate::dynamics::{JointGraphEdge, JointIndex, MultibodyJointSet, RigidBodyType};
use crate::dynamics::{JointGraphEdge, JointIndex, MultibodyJointSet, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
pub(crate) fn categorize_contacts(
_bodies: &impl ComponentSet<RigidBodyType>, // Unused but useful to simplify the parallel code.
_bodies: &RigidBodySet, // Unused but useful to simplify the parallel code.
multibody_joints: &MultibodyJointSet,
manifolds: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
@@ -40,7 +39,7 @@ pub(crate) fn categorize_contacts(
}
pub(crate) fn categorize_joints(
bodies: &impl ComponentSet<RigidBodyType>,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
impulse_joints: &[JointGraphEdge],
joint_indices: &[JointIndex],

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

@@ -1,8 +1,7 @@
use crate::data::{BundleSet, ComponentSet};
use crate::dynamics::solver::{GenericRhs, VelocityConstraint};
use crate::dynamics::{
IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodyType,
RigidBodyVelocity,
IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodySet,
RigidBodyType, RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS};
@@ -27,22 +26,17 @@ pub(crate) struct GenericVelocityConstraint {
}
impl GenericVelocityConstraint {
pub fn generate<Bodies>(
pub fn generate(
params: &IntegrationParameters,
manifold_id: ContactManifoldIndex,
manifold: &ContactManifold,
bodies: &Bodies,
bodies: &RigidBodySet,
multibodies: &MultibodyJointSet,
out_constraints: &mut Vec<AnyVelocityConstraint>,
jacobians: &mut DVector<Real>,
jacobian_id: &mut usize,
insert_at: Option<usize>,
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyType>,
{
) {
let inv_dt = params.inv_dt();
let erp_inv_dt = params.erp_inv_dt();

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

@@ -1,8 +1,6 @@
use crate::data::{BundleSet, ComponentSet};
use crate::dynamics::solver::VelocityGroundConstraint;
use crate::dynamics::{
IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodyType,
RigidBodyVelocity,
IntegrationParameters, MultibodyJointSet, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS};
@@ -25,22 +23,17 @@ pub(crate) struct GenericVelocityGroundConstraint {
}
impl GenericVelocityGroundConstraint {
pub fn generate<Bodies>(
pub fn generate(
params: &IntegrationParameters,
manifold_id: ContactManifoldIndex,
manifold: &ContactManifold,
bodies: &Bodies,
bodies: &RigidBodySet,
multibodies: &MultibodyJointSet,
out_constraints: &mut Vec<AnyVelocityConstraint>,
jacobians: &mut DVector<Real>,
jacobian_id: &mut usize,
insert_at: Option<usize>,
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyType>,
{
) {
let inv_dt = params.inv_dt();
let erp_inv_dt = params.erp_inv_dt();

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

@@ -1,5 +1,4 @@
use crate::data::ComponentSet;
use crate::dynamics::{IslandManager, JointGraphEdge, JointIndex, RigidBodyIds};
use crate::dynamics::{IslandManager, JointGraphEdge, JointIndex, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
#[cfg(feature = "simd-is-enabled")]
@@ -62,17 +61,15 @@ impl ParallelInteractionGroups {
self.groups.len() - 1
}
pub fn group_interactions<Bodies, Interaction: PairInteraction>(
pub fn group_interactions<Interaction: PairInteraction>(
&mut self,
island_id: usize,
islands: &IslandManager,
bodies: &Bodies,
bodies: &RigidBodySet,
multibodies: &MultibodyJointSet,
interactions: &[Interaction],
interaction_indices: &[usize],
) where
Bodies: ComponentSet<RigidBodyIds> + ComponentSet<RigidBodyType>,
{
) {
let num_island_bodies = islands.active_island(island_id).len();
self.bodies_color.clear();
self.interaction_indices.clear();
@@ -217,7 +214,7 @@ impl InteractionGroups {
&mut self,
_island_id: usize,
_islands: &IslandManager,
_bodies: &impl ComponentSet<RigidBodyIds>,
_bodies: &RigidBodySet,
_interactions: &[JointGraphEdge],
interaction_indices: &[JointIndex],
) {
@@ -226,16 +223,14 @@ impl InteractionGroups {
}
#[cfg(feature = "simd-is-enabled")]
pub fn group_joints<Bodies>(
pub fn group_joints(
&mut self,
island_id: usize,
islands: &IslandManager,
bodies: &Bodies,
bodies: &RigidBodySet,
interactions: &[JointGraphEdge],
interaction_indices: &[JointIndex],
) where
Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds>,
{
) {
// TODO: right now, we only sort based on the axes locked by the joint.
// We could also take motors and limits into account in the future (most of
// the SIMD constraints generation for motors and limits is already implemented).
@@ -376,7 +371,7 @@ impl InteractionGroups {
&mut self,
_island_id: usize,
_islands: &IslandManager,
_bodies: &impl ComponentSet<RigidBodyIds>,
_bodies: &RigidBodySet,
_interactions: &[&mut ContactManifold],
interaction_indices: &[ContactManifoldIndex],
) {
@@ -385,16 +380,14 @@ impl InteractionGroups {
}
#[cfg(feature = "simd-is-enabled")]
pub fn group_manifolds<Bodies>(
pub fn group_manifolds(
&mut self,
island_id: usize,
islands: &IslandManager,
bodies: &Bodies,
bodies: &RigidBodySet,
interactions: &[&mut ContactManifold],
interaction_indices: &[ContactManifoldIndex],
) where
Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds>,
{
) {
// Note: each bit of a body mask indicates what bucket already contains
// a constraints involving this body.
// TODO: currently, this is a bit overconservative because when a bucket

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

@@ -1,14 +1,10 @@
use super::VelocitySolver;
use crate::counters::Counters;
use crate::data::{ComponentSet, ComponentSetMut};
use crate::dynamics::solver::{
AnyJointVelocityConstraint, AnyVelocityConstraint, SolverConstraints,
};
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyDamping, RigidBodyForces,
RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
};
use crate::dynamics::{IslandManager, RigidBodyVelocity};
use crate::dynamics::IslandManager;
use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::prelude::MultibodyJointSet;
@@ -33,27 +29,19 @@ impl IslandSolver {
}
}
pub fn init_and_solve<Bodies>(
pub fn init_and_solve(
&mut self,
island_id: usize,
counters: &mut Counters,
params: &IntegrationParameters,
islands: &IslandManager,
bodies: &mut Bodies,
bodies: &mut RigidBodySet,
manifolds: &mut [&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
impulse_joints: &mut [JointGraphEdge],
joint_indices: &[JointIndex],
multibody_joints: &mut MultibodyJointSet,
) where
Bodies: ComponentSet<RigidBodyForces>
+ ComponentSetMut<RigidBodyPosition>
+ ComponentSetMut<RigidBodyVelocity>
+ ComponentSetMut<RigidBodyMassProps>
+ ComponentSet<RigidBodyDamping>
+ ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyType>,
{
) {
// Init the solver id for multibody_joints.
// We need that for building the constraints.
let mut solver_id = 0;

49
src/dynamics/solver/joint_constraint/joint_constraint.rs
View File

@@ -1,4 +1,3 @@
use crate::data::{BundleSet, ComponentSet};
use crate::dynamics::solver::joint_constraint::joint_generic_velocity_constraint::{
JointGenericVelocityConstraint, JointGenericVelocityGroundConstraint,
};
@@ -8,7 +7,7 @@ use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
RigidBodyMassProps, RigidBodyPosition, RigidBodySet, RigidBodyType, RigidBodyVelocity,
};
use crate::math::{Real, SPATIAL_DIM};
use crate::prelude::MultibodyJointSet;
@@ -51,22 +50,17 @@ impl AnyJointVelocityConstraint {
(num_constraints, num_constraints)
}
pub fn from_joint<Bodies>(
pub fn from_joint(
params: &IntegrationParameters,
joint_id: JointIndex,
joint: &ImpulseJoint,
bodies: &Bodies,
bodies: &RigidBodySet,
multibodies: &MultibodyJointSet,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
out: &mut Vec<Self>,
insert_at: Option<usize>,
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
) {
let local_frame1 = joint.data.local_frame1;
let local_frame2 = joint.data.local_frame2;
let rb1: (
@@ -184,19 +178,14 @@ impl AnyJointVelocityConstraint {
}
#[cfg(feature = "simd-is-enabled")]
pub fn from_wide_joint<Bodies>(
pub fn from_wide_joint(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
impulse_joints: [&ImpulseJoint; SIMD_WIDTH],
bodies: &Bodies,
bodies: &RigidBodySet,
out: &mut Vec<Self>,
insert_at: Option<usize>,
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
) {
let rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
@@ -274,23 +263,17 @@ impl AnyJointVelocityConstraint {
}
}
pub fn from_joint_ground<Bodies>(
pub fn from_joint_ground(
params: &IntegrationParameters,
joint_id: JointIndex,
joint: &ImpulseJoint,
bodies: &Bodies,
bodies: &RigidBodySet,
multibodies: &MultibodyJointSet,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
out: &mut Vec<Self>,
insert_at: Option<usize>,
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
) {
let mut handle1 = joint.body1;
let mut handle2 = joint.body2;
let status2: &RigidBodyType = bodies.index(handle2.0);
@@ -408,20 +391,14 @@ impl AnyJointVelocityConstraint {
}
#[cfg(feature = "simd-is-enabled")]
pub fn from_wide_joint_ground<Bodies>(
pub fn from_wide_joint_ground(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
impulse_joints: [&ImpulseJoint; SIMD_WIDTH],
bodies: &Bodies,
bodies: &RigidBodySet,
out: &mut Vec<Self>,
insert_at: Option<usize>,
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
) {
let mut handles1 = gather![|ii| impulse_joints[ii].body1];
let mut handles2 = gather![|ii| impulse_joints[ii].body2];
let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];

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

@@ -2,7 +2,6 @@ use std::sync::atomic::{AtomicUsize, Ordering};
use rayon::Scope;
use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
use crate::dynamics::solver::{
AnyJointVelocityConstraint, AnyVelocityConstraint, ParallelSolverConstraints,
};
@@ -162,27 +161,19 @@ impl ParallelIslandSolver {
}
}
pub fn init_and_solve<'s, Bodies>(
pub fn init_and_solve<'s>(
&'s mut self,
scope: &Scope<'s>,
island_id: usize,
islands: &'s IslandManager,
params: &'s IntegrationParameters,
bodies: &'s mut Bodies,
bodies: &'s mut RigidBodySet,
manifolds: &'s mut Vec<&'s mut ContactManifold>,
manifold_indices: &'s [ContactManifoldIndex],
impulse_joints: &'s mut Vec<JointGraphEdge>,
joint_indices: &[JointIndex],
multibodies: &mut MultibodyJointSet,
) where
Bodies: ComponentSet<RigidBodyForces>
+ ComponentSetMut<RigidBodyPosition>
+ ComponentSetMut<RigidBodyVelocity>
+ ComponentSetMut<RigidBodyMassProps>
+ ComponentSet<RigidBodyDamping>
+ ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyType>,
{
) {
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?
@@ -288,7 +279,7 @@ impl ParallelIslandSolver {
// Transmute *mut -> &mut
let velocity_solver: &mut ParallelVelocitySolver =
unsafe { std::mem::transmute(velocity_solver.load(Ordering::Relaxed)) };
let bodies: &mut Bodies =
let bodies: &mut RigidBodySet =
unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
let multibodies: &mut MultibodyJointSet =
unsafe { std::mem::transmute(multibodies.load(Ordering::Relaxed)) };

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

@@ -1,6 +1,5 @@
use super::ParallelInteractionGroups;
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::{
@@ -88,16 +87,16 @@ macro_rules! impl_init_constraints_group {
$num_active_constraints_and_jacobian_lines: path,
$empty_velocity_constraint: expr $(, $weight: ident)*) => {
impl ParallelSolverConstraints<$VelocityConstraint> {
pub fn init_constraint_groups<Bodies>(
pub fn init_constraint_groups(
&mut self,
island_id: usize,
islands: &IslandManager,
bodies: &Bodies,
bodies: &RigidBodySet,
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();
@@ -316,20 +315,14 @@ impl_init_constraints_group!(
);
impl ParallelSolverConstraints<AnyVelocityConstraint> {
pub fn fill_constraints<Bodies>(
pub fn fill_constraints(
&mut self,
thread: &ThreadContext,
params: &IntegrationParameters,
bodies: &Bodies,
bodies: &RigidBodySet,
multibodies: &MultibodyJointSet,
manifolds_all: &[&mut ContactManifold],
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyType>,
{
) {
let descs = &self.constraint_descs;
crate::concurrent_loop! {
@@ -372,20 +365,14 @@ impl ParallelSolverConstraints<AnyVelocityConstraint> {
}
impl ParallelSolverConstraints<AnyJointVelocityConstraint> {
pub fn fill_constraints<Bodies>(
pub fn fill_constraints(
&mut self,
thread: &ThreadContext,
params: &IntegrationParameters,
bodies: &Bodies,
bodies: &RigidBodySet,
multibodies: &MultibodyJointSet,
joints_all: &[JointGraphEdge],
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyType>,
{
) {
let descs = &self.constraint_descs;
crate::concurrent_loop! {

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

@@ -1,6 +1,5 @@
use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel, ThreadContext};
use crate::concurrent_loop;
use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
use crate::dynamics::{
solver::ParallelSolverConstraints, IntegrationParameters, IslandManager, JointGraphEdge,
MultibodyJointSet, RigidBodyDamping, RigidBodyForces, RigidBodyIds, RigidBodyMassProps,
@@ -26,27 +25,19 @@ impl ParallelVelocitySolver {
}
}
pub fn solve<Bodies>(
pub fn solve(
&mut self,
thread: &ThreadContext,
params: &IntegrationParameters,
island_id: usize,
islands: &IslandManager,
bodies: &mut Bodies,
bodies: &mut RigidBodySet,
multibodies: &mut MultibodyJointSet,
manifolds_all: &mut [&mut ContactManifold],
joints_all: &mut [JointGraphEdge],
contact_constraints: &mut ParallelSolverConstraints<AnyVelocityConstraint>,
joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint>,
) where
Bodies: ComponentSet<RigidBodyForces>
+ ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyVelocity>
+ ComponentSetMut<RigidBodyMassProps>
+ ComponentSetMut<RigidBodyPosition>
+ ComponentSet<RigidBodyDamping>,
{
) {
let mut start_index = thread
.solve_interaction_index
.fetch_add(thread.batch_size, Ordering::SeqCst);

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

@@ -3,15 +3,13 @@ use super::{
};
#[cfg(feature = "simd-is-enabled")]
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::GenericVelocityConstraint;
use crate::dynamics::{
solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex,
MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
solver::AnyVelocityConstraint, IntegrationParameters, IslandManager, JointGraphEdge,
JointIndex, MultibodyJointSet, RigidBodySet,
};
use crate::dynamics::{IslandManager, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::Real;
#[cfg(feature = "simd-is-enabled")]
@@ -55,17 +53,15 @@ impl<VelocityConstraint> SolverConstraints<VelocityConstraint> {
}
impl SolverConstraints<AnyVelocityConstraint> {
pub fn init_constraint_groups<Bodies>(
pub fn init_constraint_groups(
&mut self,
island_id: usize,
islands: &IslandManager,
bodies: &Bodies,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
manifolds: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
) where
Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds>,
{
) {
self.not_ground_interactions.clear();
self.ground_interactions.clear();
self.generic_not_ground_interactions.clear();
@@ -109,22 +105,16 @@ impl SolverConstraints<AnyVelocityConstraint> {
// .append(&mut self.ground_interaction_groups.grouped_interactions);
}
pub fn init<Bodies>(
pub fn init(
&mut self,
island_id: usize,
params: &IntegrationParameters,
islands: &IslandManager,
bodies: &Bodies,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
manifolds: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyType>,
{
) {
self.velocity_constraints.clear();
self.init_constraint_groups(
@@ -165,17 +155,12 @@ impl SolverConstraints<AnyVelocityConstraint> {
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_constraints<Bodies>(
fn compute_grouped_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &Bodies,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
) where
Bodies: ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
) {
for manifolds_i in self
.interaction_groups
.grouped_interactions
@@ -194,17 +179,12 @@ impl SolverConstraints<AnyVelocityConstraint> {
}
}
fn compute_nongrouped_constraints<Bodies>(
fn compute_nongrouped_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &Bodies,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
) where
Bodies: ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
) {
for manifold_i in &self.interaction_groups.nongrouped_interactions {
let manifold = &manifolds_all[*manifold_i];
VelocityConstraint::generate(
@@ -218,20 +198,14 @@ impl SolverConstraints<AnyVelocityConstraint> {
}
}
fn compute_generic_constraints<Bodies>(
fn compute_generic_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &Bodies,
bodies: &RigidBodySet,
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_not_ground_interactions {
let manifold = &manifolds_all[*manifold_i];
GenericVelocityConstraint::generate(
@@ -248,20 +222,14 @@ impl SolverConstraints<AnyVelocityConstraint> {
}
}
fn compute_generic_ground_constraints<Bodies>(
fn compute_generic_ground_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &Bodies,
bodies: &RigidBodySet,
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(
@@ -279,17 +247,12 @@ impl SolverConstraints<AnyVelocityConstraint> {
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_ground_constraints<Bodies>(
fn compute_grouped_ground_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &Bodies,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
) {
for manifolds_i in self
.ground_interaction_groups
.grouped_interactions
@@ -308,17 +271,12 @@ impl SolverConstraints<AnyVelocityConstraint> {
}
}
fn compute_nongrouped_ground_constraints<Bodies>(
fn compute_nongrouped_ground_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &Bodies,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
) {
for manifold_i in &self.ground_interaction_groups.nongrouped_interactions {
let manifold = &manifolds_all[*manifold_i];
VelocityGroundConstraint::generate(
@@ -334,22 +292,16 @@ impl SolverConstraints<AnyVelocityConstraint> {
}
impl SolverConstraints<AnyJointVelocityConstraint> {
pub fn init<Bodies>(
pub fn init(
&mut self,
island_id: usize,
params: &IntegrationParameters,
islands: &IslandManager,
bodies: &Bodies,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
impulse_joints: &[JointGraphEdge],
joint_constraint_indices: &[JointIndex],
) where
Bodies: ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>,
{
) {
// Generate constraints for impulse_joints.
self.not_ground_interactions.clear();
self.ground_interactions.clear();
@@ -464,19 +416,13 @@ impl SolverConstraints<AnyJointVelocityConstraint> {
}
}
fn compute_nongrouped_joint_ground_constraints<Bodies>(
fn compute_nongrouped_joint_ground_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &Bodies,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
joints_all: &[JointGraphEdge],
) where
Bodies: ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyIds>,
{
) {
let mut j_id = 0;
for joint_i in &self.ground_interaction_groups.nongrouped_interactions {
let joint = &joints_all[*joint_i].weight;
@@ -495,18 +441,12 @@ impl SolverConstraints<AnyJointVelocityConstraint> {
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_joint_ground_constraints<Bodies>(
fn compute_grouped_joint_ground_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &Bodies,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
) where
Bodies: ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
) {
for joints_i in self
.ground_interaction_groups
.grouped_interactions
@@ -525,19 +465,14 @@ impl SolverConstraints<AnyJointVelocityConstraint> {
}
}
fn compute_nongrouped_joint_constraints<Bodies>(
fn compute_nongrouped_joint_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &Bodies,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
joints_all: &[JointGraphEdge],
j_id: &mut usize,
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
) {
for joint_i in &self.interaction_groups.nongrouped_interactions {
let joint = &joints_all[*joint_i].weight;
AnyJointVelocityConstraint::from_joint(
@@ -554,19 +489,14 @@ impl SolverConstraints<AnyJointVelocityConstraint> {
}
}
fn compute_generic_joint_constraints<Bodies>(
fn compute_generic_joint_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &Bodies,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
joints_all: &[JointGraphEdge],
j_id: &mut usize,
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
) {
for joint_i in &self.generic_not_ground_interactions {
let joint = &joints_all[*joint_i].weight;
AnyJointVelocityConstraint::from_joint(
@@ -583,20 +513,14 @@ impl SolverConstraints<AnyJointVelocityConstraint> {
}
}
fn compute_generic_ground_joint_constraints<Bodies>(
fn compute_generic_ground_joint_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &Bodies,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
joints_all: &[JointGraphEdge],
j_id: &mut usize,
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyIds>,
{
) {
for joint_i in &self.generic_ground_interactions {
let joint = &joints_all[*joint_i].weight;
AnyJointVelocityConstraint::from_joint_ground(
@@ -614,17 +538,12 @@ impl SolverConstraints<AnyJointVelocityConstraint> {
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_joint_constraints<Bodies>(
fn compute_grouped_joint_constraints(
&mut self,
params: &IntegrationParameters,
bodies: &Bodies,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
) {
for joints_i in self
.interaction_groups
.grouped_interactions

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

@@ -1,10 +1,11 @@
use crate::data::{BundleSet, ComponentSet};
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::dynamics::{
IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::{self, WAngularInertia, WBasis, WCross, WDot};
@@ -144,18 +145,14 @@ impl VelocityConstraint {
)
}
pub fn generate<Bodies>(
pub fn generate(
params: &IntegrationParameters,
manifold_id: ContactManifoldIndex,
manifold: &ContactManifold,
bodies: &Bodies,
bodies: &RigidBodySet,
out_constraints: &mut Vec<AnyVelocityConstraint>,
insert_at: Option<usize>,
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
) {
assert_eq!(manifold.data.relative_dominance, 0);
let inv_dt = params.inv_dt();

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

@@ -1,7 +1,6 @@
use super::{
AnyVelocityConstraint, DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart,
};
use crate::data::ComponentSet;
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
@@ -30,18 +29,14 @@ pub(crate) struct WVelocityConstraint {
}
impl WVelocityConstraint {
pub fn generate<Bodies>(
pub fn generate(
params: &IntegrationParameters,
manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
manifolds: [&ContactManifold; SIMD_WIDTH],
bodies: &Bodies,
bodies: &RigidBodySet,
out_constraints: &mut Vec<AnyVelocityConstraint>,
insert_at: Option<usize>,
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
) {
for ii in 0..SIMD_WIDTH {
assert_eq!(manifolds[ii].data.relative_dominance, 0);
}

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

@@ -7,8 +7,9 @@ use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::WBasis;
use crate::utils::{self, WAngularInertia, WCross, WDot};
use crate::data::{BundleSet, ComponentSet};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
use crate::dynamics::{
IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
#[derive(Copy, Clone, Debug)]
@@ -27,18 +28,14 @@ pub(crate) struct VelocityGroundConstraint {
}
impl VelocityGroundConstraint {
pub fn generate<Bodies>(
pub fn generate(
params: &IntegrationParameters,
manifold_id: ContactManifoldIndex,
manifold: &ContactManifold,
bodies: &Bodies,
bodies: &RigidBodySet,
out_constraints: &mut Vec<AnyVelocityConstraint>,
insert_at: Option<usize>,
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
) {
let inv_dt = params.inv_dt();
let erp_inv_dt = params.erp_inv_dt();

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

@@ -2,7 +2,6 @@ use super::{
AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement,
VelocityGroundConstraintNormalPart,
};
use crate::data::ComponentSet;
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
@@ -29,18 +28,14 @@ pub(crate) struct WVelocityGroundConstraint {
}
impl WVelocityGroundConstraint {
pub fn generate<Bodies>(
pub fn generate(
params: &IntegrationParameters,
manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
manifolds: [&ContactManifold; SIMD_WIDTH],
bodies: &Bodies,
bodies: &RigidBodySet,
out_constraints: &mut Vec<AnyVelocityConstraint>,
insert_at: Option<usize>,
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
) {
let inv_dt = SimdReal::splat(params.inv_dt());
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());

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

@@ -1,9 +1,8 @@
use super::AnyJointVelocityConstraint;
use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
use crate::dynamics::{
solver::{AnyVelocityConstraint, DeltaVel},
IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodyDamping,
RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodySet,
RigidBodyVelocity,
};
use crate::geometry::ContactManifold;
@@ -24,12 +23,12 @@ impl VelocitySolver {
}
}
pub fn solve<Bodies>(
pub fn solve(
&mut self,
island_id: usize,
params: &IntegrationParameters,
islands: &IslandManager,
bodies: &mut Bodies,
bodies: &mut RigidBodySet,
multibodies: &mut MultibodyJointSet,
manifolds_all: &mut [&mut ContactManifold],
joints_all: &mut [JointGraphEdge],
@@ -37,15 +36,7 @@ impl VelocitySolver {
generic_contact_jacobians: &DVector<Real>,
joint_constraints: &mut [AnyJointVelocityConstraint],
generic_joint_jacobians: &DVector<Real>,
) where
Bodies: ComponentSet<RigidBodyForces>
+ ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyVelocity>
+ ComponentSetMut<RigidBodyMassProps>
+ ComponentSetMut<RigidBodyPosition>
+ ComponentSet<RigidBodyDamping>,
{
) {
let cfm_factor = params.cfm_factor();
self.mj_lambdas.clear();
self.mj_lambdas