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

feat: switch to the new Bvh from parry for the broad-phase (#853)

Browse Source 
* feat: switch to the new Bvh from parry for the broad-phase

* chore: cargo fmt + update testbed

* chore: remove the multi-grid SAP broad-phase

* fix soft-ccd handling in broad-phase

* Fix contact cleanup in broad-phase after collider removal

* chore: clippy fixes

* fix CCD regression

* chore: update changelog

* fix build with the parallel feature enabled

* chore: remove the now useless broad-phase proxy index from colliders

* fix tests
This commit is contained in:
Sébastien Crozet
2025-07-11 22:36:40 +02:00
committed by GitHub
parent 86a257d4f1
commit 95bd6fcfeb
212 changed files with 2140 additions and 3953 deletions
Download Patch File Download Diff File Expand all files Collapse all files

404
src/dynamics/ccd/ccd_solver.rs
View File

@@ -3,9 +3,9 @@ use crate::dynamics::{IslandManager, RigidBodyHandle, RigidBodySet};
use crate::geometry::{ColliderParent, ColliderSet, CollisionEvent, NarrowPhase};
use crate::math::Real;
use crate::parry::utils::SortedPair;
use crate::pipeline::{EventHandler, QueryPipeline};
use crate::prelude::{query_pipeline_generators, ActiveEvents, CollisionEventFlags};
use parry::query::{DefaultQueryDispatcher, QueryDispatcher};
use crate::pipeline::{EventHandler, QueryFilter, QueryPipeline};
use crate::prelude::{ActiveEvents, CollisionEventFlags};
use parry::partitioning::{Bvh, BvhBuildStrategy};
use parry::utils::hashmap::HashMap;
use std::collections::BinaryHeap;
@@ -19,8 +19,11 @@ pub enum PredictedImpacts {
#[derive(Clone)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct CCDSolver {
// TODO PERF: for now the CCD solver maintains its own bvh for CCD queries.
// At each frame it get rebuilt.
// We should consider an alternative to directly use the broad-phase’s.
#[cfg_attr(feature = "serde-serialize", serde(skip))]
query_pipeline: QueryPipeline,
bvh: Bvh,
}
impl Default for CCDSolver {
@@ -32,20 +35,7 @@ impl Default for CCDSolver {
impl CCDSolver {
/// Initializes a new CCD solver
pub fn new() -> Self {
Self::with_query_dispatcher(DefaultQueryDispatcher)
}
/// Initializes a CCD solver with a custom `QueryDispatcher` used for computing time-of-impacts.
///
/// Use this constructor in order to use a custom `QueryDispatcher` that is aware of your own
/// user-defined shapes.
pub fn with_query_dispatcher<D>(d: D) -> Self
where
D: 'static + QueryDispatcher,
{
CCDSolver {
query_pipeline: QueryPipeline::with_query_dispatcher(d),
}
Self { bvh: Bvh::new() }
}
/// Apply motion-clamping to the bodies affected by the given `impacts`.
@@ -117,15 +107,37 @@ impl CCDSolver {
colliders: &ColliderSet,
narrow_phase: &NarrowPhase,
) -> Option<Real> {
// Update the query pipeline.
self.query_pipeline.update_with_generator(
query_pipeline_generators::SweptAabbWithPredictedPosition {
bodies,
colliders,
dt,
},
// Update the query pipeline with the colliders’ predicted positions.
self.bvh = Bvh::from_iter(
BvhBuildStrategy::Binned,
colliders.iter_enabled().map(|(co_handle, co)| {
let id = co_handle.into_raw_parts().0;
if let Some(co_parent) = co.parent {
let rb = &bodies[co_parent.handle];
let predicted_pos = rb
.pos
.integrate_forces_and_velocities(dt, &rb.forces, &rb.vels, &rb.mprops);
let next_position = predicted_pos * co_parent.pos_wrt_parent;
(
id as usize,
co.shape.compute_swept_aabb(&co.pos, &next_position),
)
} else {
(id as usize, co.shape.compute_aabb(&co.pos))
}
}),
);
let query_pipeline = QueryPipeline {
// NOTE: the upcast needs at least rust 1.86
dispatcher: narrow_phase.query_dispatcher(),
bvh: &self.bvh,
bodies,
colliders,
filter: QueryFilter::default(),
};
let mut pairs_seen = HashMap::default();
let mut min_toi = dt;
@@ -156,73 +168,66 @@ impl CCDSolver {
.shape
.compute_swept_aabb(&co1.pos, &predicted_collider_pos1);
self.query_pipeline
.colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| {
if *ch1 == *ch2 {
// Ignore self-intersection.
return true;
}
for (ch2, _) in query_pipeline.intersect_aabb_conservative(&aabb1) {
if *ch1 == ch2 {
// Ignore self-intersection.
continue;
}
if pairs_seen
.insert(
SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0),
(),
)
.is_none()
{
let co1 = &colliders[*ch1];
let co2 = &colliders[*ch2];
if pairs_seen
.insert(
SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0),
(),
)
.is_none()
{
let co1 = &colliders[*ch1];
let co2 = &colliders[ch2];
let bh1 = co1.parent.map(|p| p.handle);
let bh2 = co2.parent.map(|p| p.handle);
let bh1 = co1.parent.map(|p| p.handle);
let bh2 = co2.parent.map(|p| p.handle);
// Ignore self-intersection and sensors and apply collision groups filter.
if bh1 == bh2 // Ignore self-intersection.
// Ignore self-intersection and sensors and apply collision groups filter.
if bh1 == bh2 // Ignore self-intersection.
|| (co1.is_sensor() || co2.is_sensor()) // Ignore sensors.
|| !co1.flags.collision_groups.test(co2.flags.collision_groups) // Apply collision groups.
|| !co1.flags.solver_groups.test(co2.flags.solver_groups)
// Apply solver groups.
{
return true;
}
let smallest_dist = narrow_phase
.contact_pair(*ch1, *ch2)
.and_then(|p| p.find_deepest_contact())
.map(|c| c.1.dist)
.unwrap_or(0.0);
let rb2 = bh2.and_then(|h| bodies.get(h));
if let Some(toi) = TOIEntry::try_from_colliders(
self.query_pipeline.query_dispatcher(),
*ch1,
*ch2,
co1,
co2,
Some(rb1),
rb2,
None,
None,
0.0,
min_toi,
smallest_dist,
) {
min_toi = min_toi.min(toi.toi);
}
// Apply solver groups.
{
continue;
}
true
});
let smallest_dist = narrow_phase
.contact_pair(*ch1, ch2)
.and_then(|p| p.find_deepest_contact())
.map(|c| c.1.dist)
.unwrap_or(0.0);
let rb2 = bh2.and_then(|h| bodies.get(h));
if let Some(toi) = TOIEntry::try_from_colliders(
narrow_phase.query_dispatcher(),
*ch1,
ch2,
co1,
co2,
Some(rb1),
rb2,
None,
None,
0.0,
min_toi,
smallest_dist,
) {
min_toi = min_toi.min(toi.toi);
}
}
}
}
}
}
if min_toi < dt {
Some(min_toi)
} else {
None
}
if min_toi < dt { Some(min_toi) } else { None }
}
/// Outputs the set of bodies as well as their first time-of-impact event.
@@ -241,11 +246,32 @@ impl CCDSolver {
let mut pairs_seen = HashMap::default();
let mut min_overstep = dt;
// Update the query pipeline.
self.query_pipeline.update_with_generator(
query_pipeline_generators::SweptAabbWithNextPosition { bodies, colliders },
// Update the query pipeline with the colliders’ `next_position`.
self.bvh = Bvh::from_iter(
BvhBuildStrategy::Binned,
colliders.iter_enabled().map(|(co_handle, co)| {
let id = co_handle.into_raw_parts().0;
if let Some(co_parent) = co.parent {
let rb_next_pos = &bodies[co_parent.handle].pos.next_position;
let next_position = rb_next_pos * co_parent.pos_wrt_parent;
(
id as usize,
co.shape.compute_swept_aabb(&co.pos, &next_position),
)
} else {
(id as usize, co.shape.compute_aabb(&co.pos))
}
}),
);
let query_pipeline = QueryPipeline {
dispatcher: narrow_phase.query_dispatcher(),
bvh: &self.bvh,
bodies,
colliders,
filter: QueryFilter::default(),
};
/*
*
* First, collect all TOIs.
@@ -275,69 +301,66 @@ impl CCDSolver {
.shape
.compute_swept_aabb(&co1.pos, &predicted_collider_pos1);
self.query_pipeline
.colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| {
if *ch1 == *ch2 {
// Ignore self-intersection.
return true;
}
for (ch2, _) in query_pipeline.intersect_aabb_conservative(&aabb1) {
if *ch1 == ch2 {
// Ignore self-intersection.
continue;
}
if pairs_seen
.insert(
SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0),
(),
)
.is_none()
if pairs_seen
.insert(
SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0),
(),
)
.is_none()
{
let co1 = &colliders[*ch1];
let co2 = &colliders[ch2];
let bh1 = co1.parent.map(|p| p.handle);
let bh2 = co2.parent.map(|p| p.handle);
// Ignore self-intersections and apply groups filter.
if bh1 == bh2
|| !co1.flags.collision_groups.test(co2.flags.collision_groups)
{
let co1 = &colliders[*ch1];
let co2 = &colliders[*ch2];
let bh1 = co1.parent.map(|p| p.handle);
let bh2 = co2.parent.map(|p| p.handle);
// Ignore self-intersections and apply groups filter.
if bh1 == bh2
|| !co1.flags.collision_groups.test(co2.flags.collision_groups)
{
return true;
}
let smallest_dist = narrow_phase
.contact_pair(*ch1, *ch2)
.and_then(|p| p.find_deepest_contact())
.map(|c| c.1.dist)
.unwrap_or(0.0);
let rb1 = bh1.map(|h| &bodies[h]);
let rb2 = bh2.map(|h| &bodies[h]);
if let Some(toi) = TOIEntry::try_from_colliders(
self.query_pipeline.query_dispatcher(),
*ch1,
*ch2,
co1,
co2,
rb1,
rb2,
None,
None,
0.0,
// NOTE: we use dt here only once we know that
// there is at least one TOI before dt.
min_overstep,
smallest_dist,
) {
if toi.toi > dt {
min_overstep = min_overstep.min(toi.toi);
} else {
min_overstep = dt;
all_toi.push(toi);
}
}
continue;
}
true
});
let smallest_dist = narrow_phase
.contact_pair(*ch1, ch2)
.and_then(|p| p.find_deepest_contact())
.map(|c| c.1.dist)
.unwrap_or(0.0);
let rb1 = bh1.map(|h| &bodies[h]);
let rb2 = bh2.map(|h| &bodies[h]);
if let Some(toi) = TOIEntry::try_from_colliders(
query_pipeline.dispatcher,
*ch1,
ch2,
co1,
co2,
rb1,
rb2,
None,
None,
0.0,
// NOTE: we use dt here only once we know that
// there is at least one TOI before dt.
min_overstep,
smallest_dist,
) {
if toi.toi > dt {
min_overstep = min_overstep.min(toi.toi);
} else {
min_overstep = dt;
all_toi.push(toi);
}
}
}
}
}
}
}
@@ -378,10 +401,10 @@ impl CCDSolver {
if toi.is_pseudo_intersection_test {
// NOTE: this test is redundant with the previous `if !should_freeze && ...`
// but let's keep it to avoid tricky regressions if we end up swapping both
// `if` for some reasons in the future.
// `if` for some reason in the future.
if should_freeze1 || should_freeze2 {
// This is only an intersection so we don't have to freeze and there is no
// need to resweep. However we will need to see if we have to generate
// need to resweep. However, we will need to see if we have to generate
// intersection events, so push the TOI for further testing.
pseudo_intersections_to_check.push(toi);
}
@@ -410,59 +433,53 @@ impl CCDSolver {
let co_next_pos1 = rb1.pos.next_position * co1_parent.pos_wrt_parent;
let aabb = co1.shape.compute_swept_aabb(&co1.pos, &co_next_pos1);
self.query_pipeline
.colliders_with_aabb_intersecting_aabb(&aabb, |ch2| {
let co2 = &colliders[*ch2];
for (ch2, _) in query_pipeline.intersect_aabb_conservative(&aabb) {
let co2 = &colliders[ch2];
let bh1 = co1.parent.map(|p| p.handle);
let bh2 = co2.parent.map(|p| p.handle);
let bh1 = co1.parent.map(|p| p.handle);
let bh2 = co2.parent.map(|p| p.handle);
// Ignore self-intersection and apply groups filter.
if bh1 == bh2
|| !co1.flags.collision_groups.test(co2.flags.collision_groups)
{
return true;
}
// Ignore self-intersection and apply groups filter.
if bh1 == bh2 || !co1.flags.collision_groups.test(co2.flags.collision_groups) {
continue;
}
let frozen1 = bh1.and_then(|h| frozen.get(&h));
let frozen2 = bh2.and_then(|h| frozen.get(&h));
let frozen1 = bh1.and_then(|h| frozen.get(&h));
let frozen2 = bh2.and_then(|h| frozen.get(&h));
let rb1 = bh1.and_then(|h| bodies.get(h));
let rb2 = bh2.and_then(|h| bodies.get(h));
let rb1 = bh1.and_then(|h| bodies.get(h));
let rb2 = bh2.and_then(|h| bodies.get(h));
if (frozen1.is_some() || !rb1.map(|b| b.ccd.ccd_active).unwrap_or(false))
&& (frozen2.is_some()
|| !rb2.map(|b| b.ccd.ccd_active).unwrap_or(false))
{
// We already did a resweep.
return true;
}
if (frozen1.is_some() || !rb1.map(|b| b.ccd.ccd_active).unwrap_or(false))
&& (frozen2.is_some() || !rb2.map(|b| b.ccd.ccd_active).unwrap_or(false))
{
// We already did a resweep.
continue;
}
let smallest_dist = narrow_phase
.contact_pair(*ch1, *ch2)
.and_then(|p| p.find_deepest_contact())
.map(|c| c.1.dist)
.unwrap_or(0.0);
let smallest_dist = narrow_phase
.contact_pair(*ch1, ch2)
.and_then(|p| p.find_deepest_contact())
.map(|c| c.1.dist)
.unwrap_or(0.0);
if let Some(toi) = TOIEntry::try_from_colliders(
self.query_pipeline.query_dispatcher(),
*ch1,
*ch2,
co1,
co2,
rb1,
rb2,
frozen1.copied(),
frozen2.copied(),
start_time,
dt,
smallest_dist,
) {
all_toi.push(toi);
}
true
});
if let Some(toi) = TOIEntry::try_from_colliders(
query_pipeline.dispatcher,
*ch1,
ch2,
co1,
co2,
rb1,
rb2,
frozen1.copied(),
frozen2.copied(),
start_time,
dt,
smallest_dist,
) {
all_toi.push(toi);
}
}
}
}
@@ -523,12 +540,13 @@ impl CCDSolver {
let prev_coll_pos12 = co1.pos.inv_mul(&co2.pos);
let next_coll_pos12 = co_next_pos1.inv_mul(&co_next_pos2);
let query_dispatcher = self.query_pipeline.query_dispatcher();
let intersect_before = query_dispatcher
let intersect_before = query_pipeline
.dispatcher
.intersection_test(&prev_coll_pos12, co1.shape.as_ref(), co2.shape.as_ref())
.unwrap_or(false);
let intersect_after = query_dispatcher
let intersect_after = query_pipeline
.dispatcher
.intersection_test(&next_coll_pos12, co1.shape.as_ref(), co2.shape.as_ref())
.unwrap_or(false);

6
src/dynamics/integration_parameters.rs
View File

@@ -111,11 +111,7 @@ impl IntegrationParameters {
/// This is zero if `self.dt` is zero.
#[inline(always)]
pub fn inv_dt(&self) -> Real {
if self.dt == 0.0 {
0.0
} else {
1.0 / self.dt
}
if self.dt == 0.0 { 0.0 } else { 1.0 / self.dt }
}
/// Sets the time-stepping length.

2
src/dynamics/joint/generic_joint.rs
View File

@@ -2,7 +2,7 @@
use crate::dynamics::solver::MotorParameters;
use crate::dynamics::{FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint, RopeJoint};
use crate::math::{Isometry, Point, Real, Rotation, UnitVector, Vector, SPATIAL_DIM};
use crate::math::{Isometry, Point, Real, Rotation, SPATIAL_DIM, UnitVector, Vector};
use crate::utils::{SimdBasis, SimdRealCopy};
#[cfg(feature = "dim3")]

2
src/dynamics/joint/impulse_joint/impulse_joint_set.rs
View File

@@ -3,8 +3,8 @@ use parry::utils::hashset::HashSet;
use super::ImpulseJoint;
use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex};
use crate::data::arena::Arena;
use crate::data::Coarena;
use crate::data::arena::Arena;
use crate::dynamics::{GenericJoint, IslandManager, RigidBodyHandle, RigidBodySet};
/// The unique identifier of a joint added to the joint set.

8
src/dynamics/joint/multibody_joint/multibody.rs
View File

@@ -4,13 +4,13 @@ use crate::dynamics::{RigidBodyHandle, RigidBodySet, RigidBodyType, RigidBodyVel
#[cfg(feature = "dim3")]
use crate::math::Matrix;
use crate::math::{
AngDim, AngVector, Dim, Isometry, Jacobian, Point, Real, Vector, ANG_DIM, DIM, SPATIAL_DIM,
ANG_DIM, AngDim, AngVector, DIM, Dim, Isometry, Jacobian, Point, Real, SPATIAL_DIM, Vector,
};
use crate::prelude::MultibodyJoint;
use crate::utils::{IndexMut2, SimdAngularInertia, SimdCross, SimdCrossMatrix};
use na::{
self, DMatrix, DVector, DVectorView, DVectorViewMut, Dyn, OMatrix, SMatrix, SVector,
StorageMut, LU,
self, DMatrix, DVector, DVectorView, DVectorViewMut, Dyn, LU, OMatrix, SMatrix, SVector,
StorageMut,
};
#[cfg(doc)]
@@ -1083,7 +1083,7 @@ impl Multibody {
/// - All the indices must be part of the same kinematic branch.
/// - If a link is `branch[i]`, then `branch[i - 1]` must be its parent.
///
/// In general, this method shouldn’t be used directly and [`Self::forward_kinematics_single_link`̦]
/// In general, this method shouldn’t be used directly and [`Self::forward_kinematics_single_link`]
/// should be preferred since it computes the branch indices automatically.
///
/// If you want to calculate the branch indices manually, see [`Self::kinematic_branch`].

2
src/dynamics/joint/multibody_joint/multibody_ik.rs
View File

@@ -1,5 +1,5 @@
use crate::dynamics::{JointAxesMask, Multibody, MultibodyLink, RigidBodySet};
use crate::math::{Isometry, Jacobian, Real, ANG_DIM, DIM, SPATIAL_DIM};
use crate::math::{ANG_DIM, DIM, Isometry, Jacobian, Real, SPATIAL_DIM};
use na::{self, DVector, SMatrix};
use parry::math::SpacialVector;

8
src/dynamics/joint/multibody_joint/multibody_joint.rs
View File

@@ -1,11 +1,11 @@
use crate::dynamics::solver::JointGenericOneBodyConstraint;
use crate::dynamics::{
joint, FixedJointBuilder, GenericJoint, IntegrationParameters, Multibody, MultibodyLink,
RigidBodyVelocity,
FixedJointBuilder, GenericJoint, IntegrationParameters, Multibody, MultibodyLink,
RigidBodyVelocity, joint,
};
use crate::math::{
Isometry, JacobianViewMut, Real, Rotation, SpacialVector, Translation, Vector, ANG_DIM, DIM,
SPATIAL_DIM,
ANG_DIM, DIM, Isometry, JacobianViewMut, Real, Rotation, SPATIAL_DIM, SpacialVector,
Translation, Vector,
};
use na::{DVector, DVectorViewMut};
#[cfg(feature = "dim3")]

16
src/dynamics/joint/multibody_joint/multibody_joint_set.rs
View File

@@ -4,7 +4,6 @@ use crate::data::{Arena, Coarena, Index};
use crate::dynamics::joint::MultibodyLink;
use crate::dynamics::{GenericJoint, Multibody, MultibodyJoint, RigidBodyHandle};
use crate::geometry::{InteractionGraph, RigidBodyGraphIndex};
use crate::parry::partitioning::IndexedData;
/// The unique handle of an multibody_joint added to a `MultibodyJointSet`.
#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
@@ -44,15 +43,6 @@ impl Default for MultibodyJointHandle {
}
}
impl IndexedData for MultibodyJointHandle {
fn default() -> Self {
Self(IndexedData::default())
}
fn index(&self) -> usize {
self.0.index()
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
/// Indexes usable to get a multibody link from a `MultibodyJointSet`.
@@ -371,12 +361,12 @@ impl MultibodyJointSet {
/// suffer form the ABA problem.
pub fn get_unknown_gen(&self, i: u32) -> Option<(&Multibody, usize, MultibodyJointHandle)> {
let link = self.rb2mb.get_unknown_gen(i)?;
let gen = self.rb2mb.get_gen(i)?;
let generation = self.rb2mb.get_gen(i)?;
let multibody = self.multibodies.get(link.multibody.0)?;
Some((
multibody,
link.id,
MultibodyJointHandle(Index::from_raw_parts(i, gen)),
MultibodyJointHandle(Index::from_raw_parts(i, generation)),
))
}
@@ -427,7 +417,7 @@ impl MultibodyJointSet {
.flat_map(move |link| self.connectivity_graph.interactions_with(link.graph_id))
.map(|inter| {
// NOTE: the joint handle is always equal to the handle of the second rigid-body.
(inter.0, inter.1, MultibodyJointHandle(inter.1 .0))
(inter.0, inter.1, MultibodyJointHandle(inter.1.0))
})
}

11
src/dynamics/rigid_body_components.rs
View File

@@ -9,7 +9,6 @@ use crate::geometry::{
use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector,
};
use crate::parry::partitioning::IndexedData;
use crate::utils::{SimdAngularInertia, SimdCross, SimdDot};
use num::Zero;
@@ -42,16 +41,6 @@ impl RigidBodyHandle {
}
}
impl IndexedData for RigidBodyHandle {
fn default() -> Self {
Self(IndexedData::default())
}
fn index(&self) -> usize {
self.0.index()
}
}
/// The type of a body, governing the way it is affected by external forces.
#[deprecated(note = "renamed as RigidBodyType")]
pub type BodyStatus = RigidBodyType;

4
src/dynamics/solver/contact_constraint/contact_constraints_set.rs
View File

@@ -6,13 +6,13 @@ use crate::dynamics::solver::contact_constraint::{
};
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::solver_vel::SolverVel;
use crate::dynamics::solver::{reset_buffer, ConstraintTypes, SolverConstraintsSet};
use crate::dynamics::solver::{ConstraintTypes, SolverConstraintsSet, reset_buffer};
use crate::dynamics::{
ImpulseJoint, IntegrationParameters, IslandManager, JointAxesMask, MultibodyJointSet,
RigidBodySet,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Real, MAX_MANIFOLD_POINTS};
use crate::math::{MAX_MANIFOLD_POINTS, Real};
use na::DVector;
use parry::math::DIM;

2
src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
View File

@@ -3,7 +3,7 @@ use crate::dynamics::{
IntegrationParameters, MultibodyJointSet, MultibodyLinkId, RigidBodySet, RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS};
use crate::math::{DIM, MAX_MANIFOLD_POINTS, Point, Real};
use crate::utils::SimdCross;
use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart};

2
src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs
View File

@@ -1,7 +1,7 @@
use crate::dynamics::solver::{
OneBodyConstraintElement, OneBodyConstraintNormalPart, OneBodyConstraintTangentPart,
};
use crate::math::{Real, DIM};
use crate::math::{DIM, Real};
use na::DVector;
#[cfg(feature = "dim2")]
use na::SimdPartialOrd;

2
src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
View File

@@ -1,7 +1,7 @@
use crate::dynamics::solver::{GenericRhs, TwoBodyConstraint};
use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS};
use crate::math::{DIM, MAX_MANIFOLD_POINTS, Real};
use crate::utils::{SimdAngularInertia, SimdCross, SimdDot};
use super::{TwoBodyConstraintBuilder, TwoBodyConstraintElement, TwoBodyConstraintNormalPart};

2
src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs
View File

@@ -2,7 +2,7 @@ use crate::dynamics::solver::SolverVel;
use crate::dynamics::solver::{
TwoBodyConstraintElement, TwoBodyConstraintNormalPart, TwoBodyConstraintTangentPart,
};
use crate::math::{AngVector, Real, Vector, DIM};
use crate::math::{AngVector, DIM, Real, Vector};
use crate::utils::SimdDot;
use na::DVector;
#[cfg(feature = "dim2")]

4
src/dynamics/solver/contact_constraint/one_body_constraint.rs
View File

@@ -1,5 +1,5 @@
use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart};
use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::math::{DIM, MAX_MANIFOLD_POINTS, Point, Real, Vector};
#[cfg(feature = "dim2")]
use crate::utils::SimdBasis;
use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot, SimdRealCopy};
@@ -7,8 +7,8 @@ use na::Matrix2;
use parry::math::Isometry;
use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::SolverVel;
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};

4
src/dynamics/solver/contact_constraint/one_body_constraint_element.rs
View File

@@ -1,7 +1,7 @@
use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
use crate::dynamics::solver::contact_constraint::TwoBodyConstraintNormalPart;
use crate::dynamics::solver::SolverVel;
use crate::math::{AngVector, TangentImpulse, Vector, DIM};
use crate::dynamics::solver::contact_constraint::TwoBodyConstraintNormalPart;
use crate::math::{AngVector, DIM, TangentImpulse, Vector};
use crate::utils::{SimdBasis, SimdDot, SimdRealCopy};
use na::Vector2;

4
src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs
View File

@@ -8,8 +8,8 @@ use crate::dynamics::{
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, SimdReal, TangentImpulse, Vector, DIM,
MAX_MANIFOLD_POINTS, SIMD_WIDTH,
AngVector, AngularInertia, DIM, Isometry, MAX_MANIFOLD_POINTS, Point, Real, SIMD_WIDTH,
SimdReal, TangentImpulse, Vector,
};
#[cfg(feature = "dim2")]
use crate::utils::SimdBasis;

2
src/dynamics/solver/contact_constraint/two_body_constraint.rs
View File

@@ -5,7 +5,7 @@ use crate::dynamics::solver::{AnyConstraintMut, SolverBody};
use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Isometry, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::math::{DIM, Isometry, MAX_MANIFOLD_POINTS, Real, Vector};
use crate::utils::{self, SimdAngularInertia, SimdBasis, SimdCross, SimdDot};
use na::{DVector, Matrix2};

2
src/dynamics/solver/contact_constraint/two_body_constraint_element.rs
View File

@@ -1,6 +1,6 @@
use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
use crate::dynamics::solver::SolverVel;
use crate::math::{AngVector, TangentImpulse, Vector, DIM};
use crate::math::{AngVector, DIM, TangentImpulse, Vector};
use crate::utils::{SimdBasis, SimdDot, SimdRealCopy};
use na::Vector2;
use simba::simd::SimdValue;

4
src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs
View File

@@ -8,8 +8,8 @@ use crate::dynamics::{
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, SimdReal, TangentImpulse, Vector, DIM,
MAX_MANIFOLD_POINTS, SIMD_WIDTH,
AngVector, AngularInertia, DIM, Isometry, MAX_MANIFOLD_POINTS, Point, Real, SIMD_WIDTH,
SimdReal, TangentImpulse, Vector,
};
#[cfg(feature = "dim2")]
use crate::utils::SimdBasis;

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

@@ -1,7 +1,7 @@
use super::{JointConstraintsSet, VelocitySolver};
use crate::counters::Counters;
use crate::dynamics::solver::contact_constraint::ContactConstraintsSet;
use crate::dynamics::IslandManager;
use crate::dynamics::solver::contact_constraint::ContactConstraintsSet;
use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::prelude::MultibodyJointSet;

4
src/dynamics/solver/joint_constraint/any_joint_constraint.rs
View File

@@ -1,3 +1,4 @@
use crate::dynamics::JointGraphEdge;
use crate::dynamics::solver::joint_constraint::joint_generic_constraint::{
JointGenericOneBodyConstraint, JointGenericTwoBodyConstraint,
};
@@ -5,7 +6,6 @@ use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
JointOneBodyConstraint, JointTwoBodyConstraint,
};
use crate::dynamics::solver::{AnyConstraintMut, ConstraintTypes};
use crate::dynamics::JointGraphEdge;
use crate::math::Real;
use na::DVector;
@@ -18,7 +18,7 @@ use crate::{
dynamics::solver::joint_constraint::joint_constraint_builder::{
JointOneBodyConstraintBuilderSimd, JointTwoBodyConstraintBuilderSimd,
},
math::{SimdReal, SIMD_WIDTH},
math::{SIMD_WIDTH, SimdReal},
};
use crate::dynamics::solver::joint_constraint::joint_constraint_builder::{

10
src/dynamics/solver/joint_constraint/joint_constraint_builder.rs
View File

@@ -1,12 +1,12 @@
use crate::dynamics::solver::ConstraintsCounts;
use crate::dynamics::solver::MotorParameters;
use crate::dynamics::solver::joint_constraint::JointSolverBody;
use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
JointFixedSolverBody, JointOneBodyConstraint, JointTwoBodyConstraint, WritebackId,
};
use crate::dynamics::solver::joint_constraint::JointSolverBody;
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::ConstraintsCounts;
use crate::dynamics::solver::MotorParameters;
use crate::dynamics::{GenericJoint, ImpulseJoint, IntegrationParameters, JointIndex};
use crate::math::{AngVector, Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM};
use crate::math::{ANG_DIM, AngVector, DIM, Isometry, Matrix, Point, Real, Rotation, Vector};
use crate::prelude::RigidBodySet;
use crate::utils;
use crate::utils::{IndexMut2, SimdCrossMatrix, SimdDot, SimdRealCopy};
@@ -16,7 +16,7 @@ use na::SMatrix;
use crate::utils::{SimdBasis, SimdQuat};
#[cfg(feature = "simd-is-enabled")]
use crate::math::{SimdReal, SIMD_WIDTH};
use crate::math::{SIMD_WIDTH, SimdReal};
pub struct JointTwoBodyConstraintBuilder {
body1: usize,

8
src/dynamics/solver/joint_constraint/joint_constraints_set.rs
View File

@@ -2,10 +2,10 @@ use crate::dynamics::solver::categorization::categorize_joints;
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::solver_vel::SolverVel;
use crate::dynamics::solver::{
reset_buffer, JointConstraintTypes, JointGenericOneBodyConstraint,
JointGenericOneBodyConstraintBuilder, JointGenericTwoBodyConstraint,
JointGenericTwoBodyConstraintBuilder, JointGenericVelocityOneBodyExternalConstraintBuilder,
JointGenericVelocityOneBodyInternalConstraintBuilder, SolverConstraintsSet,
JointConstraintTypes, JointGenericOneBodyConstraint, JointGenericOneBodyConstraintBuilder,
JointGenericTwoBodyConstraint, JointGenericTwoBodyConstraintBuilder,
JointGenericVelocityOneBodyExternalConstraintBuilder,
JointGenericVelocityOneBodyInternalConstraintBuilder, SolverConstraintsSet, reset_buffer,
};
use crate::dynamics::{
IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet,

4
src/dynamics/solver/joint_constraint/joint_generic_constraint.rs
View File

@@ -1,10 +1,10 @@
use crate::dynamics::solver::SolverVel;
use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
JointFixedSolverBody, WritebackId,
};
use crate::dynamics::solver::joint_constraint::{JointSolverBody, JointTwoBodyConstraintHelper};
use crate::dynamics::solver::SolverVel;
use crate::dynamics::{GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, Multibody};
use crate::math::{Isometry, Real, DIM};
use crate::math::{DIM, Isometry, Real};
use crate::prelude::SPATIAL_DIM;
use na::{DVector, DVectorView, DVectorViewMut};

6
src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs
View File

@@ -1,3 +1,4 @@
use crate::dynamics::solver::MotorParameters;
use crate::dynamics::solver::joint_constraint::joint_generic_constraint::{
JointGenericOneBodyConstraint, JointGenericTwoBodyConstraint,
};
@@ -5,19 +6,18 @@ use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
JointFixedSolverBody, WritebackId,
};
use crate::dynamics::solver::joint_constraint::{JointSolverBody, JointTwoBodyConstraintHelper};
use crate::dynamics::solver::MotorParameters;
use crate::dynamics::{
GenericJoint, ImpulseJoint, IntegrationParameters, JointIndex, Multibody, MultibodyJointSet,
MultibodyLinkId, RigidBodySet,
};
use crate::math::{Real, Vector, ANG_DIM, DIM, SPATIAL_DIM};
use crate::math::{ANG_DIM, DIM, Real, SPATIAL_DIM, Vector};
use crate::utils;
use crate::utils::IndexMut2;
use crate::utils::SimdDot;
use na::{DVector, SVector};
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::ConstraintsCounts;
use crate::dynamics::solver::solver_body::SolverBody;
#[cfg(feature = "dim3")]
use crate::utils::SimdAngularInertia;
#[cfg(feature = "dim2")]

6
src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
View File

@@ -1,15 +1,15 @@
use crate::dynamics::solver::joint_constraint::JointTwoBodyConstraintHelper;
use crate::dynamics::solver::SolverVel;
use crate::dynamics::solver::joint_constraint::JointTwoBodyConstraintHelper;
use crate::dynamics::{
GenericJoint, IntegrationParameters, JointAxesMask, JointGraphEdge, JointIndex,
};
use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, Vector, DIM, SPATIAL_DIM};
use crate::math::{AngVector, AngularInertia, DIM, Isometry, Point, Real, SPATIAL_DIM, Vector};
use crate::num::Zero;
use crate::utils::{SimdDot, SimdRealCopy};
#[cfg(feature = "simd-is-enabled")]
use {
crate::math::{SimdReal, SIMD_WIDTH},
crate::math::{SIMD_WIDTH, SimdReal},
na::SimdValue,
};

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

@@ -42,6 +42,9 @@ mod velocity_solver;
pub unsafe fn reset_buffer<T>(buffer: &mut Vec<T>, len: usize) {
buffer.clear();
buffer.reserve(len);
buffer.as_mut_ptr().write_bytes(u8::MAX, len);
buffer.set_len(len);
unsafe {
buffer.as_mut_ptr().write_bytes(u8::MAX, len);
buffer.set_len(len);
}
}

2
src/dynamics/solver/solver_vel.rs
View File

@@ -1,4 +1,4 @@
use crate::math::{AngVector, Vector, SPATIAL_DIM};
use crate::math::{AngVector, SPATIAL_DIM, Vector};
use crate::utils::SimdRealCopy;
use na::{DVectorView, DVectorViewMut, Scalar};
use std::ops::{AddAssign, Sub, SubAssign};

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

@@ -1,9 +1,9 @@
use super::{JointConstraintTypes, SolverConstraintsSet};
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::{
solver::{ContactConstraintTypes, SolverVel},
IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet,
MultibodyLinkId, RigidBodySet,
solver::{ContactConstraintTypes, SolverVel},
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::Real;