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

8
Cargo.toml
View File

@@ -8,10 +8,10 @@ resolver = "2"
#simba = { path = "../simba" }
#kiss3d = { path = "../kiss3d" }
#parry2d = { path = "../parry/crates/parry2d" }
#parry3d = { path = "../parry/crates/parry3d" }
#parry2d-f64 = { path = "../parry/crates/parry2d-f64" }
#parry3d-f64 = { path = "../parry/crates/parry3d-f64" }
parry2d = { path = "../parry/crates/parry2d" }
parry3d = { path = "../parry/crates/parry3d" }
parry2d-f64 = { path = "../parry/crates/parry2d-f64" }
parry3d-f64 = { path = "../parry/crates/parry3d-f64" }
# nalgebra = { path = "../nalgebra" }
#kiss3d = { git = "https://github.com/sebcrozet/kiss3d" }

123
src/data/component_set.rs
View File

@@ -1,123 +0,0 @@
use crate::data::Index;
// TODO ECS: use this to handle optional components properly.
// pub trait OptionalComponentSet<T> {
// fn get(&self, handle: Index) -> Option<&T>;
// }
/// A set of optional elements of type `T`.
pub trait ComponentSetOption<T>: Sync {
/// Get the element associated to the given `handle`, if there is one.
fn get(&self, handle: Index) -> Option<&T>;
}
/// A set of elements of type `T`.
pub trait ComponentSet<T>: ComponentSetOption<T> {
/// The estimated number of elements in this set.
///
/// This value is typically used for preallocating some arrays for
/// better performances.
fn size_hint(&self) -> usize;
// TODO ECS: remove this, its only needed by the query pipeline update
// which should only take the modified colliders into account.
/// Iterate through all the elements on this set.
fn for_each(&self, f: impl FnMut(Index, &T));
/// Get the element associated to the given `handle`.
fn index(&self, handle: Index) -> &T {
self.get(handle).unwrap()
}
}
/// A set of mutable elements of type `T`.
pub trait ComponentSetMut<T>: ComponentSet<T> {
/// Applies the given closure to the element associated to the given `handle`.
///
/// Return `None` if the element doesn't exist.
fn map_mut_internal<Result>(
&mut self,
handle: crate::data::Index,
f: impl FnOnce(&mut T) -> Result,
) -> Option<Result>;
/// Set the value of this element.
fn set_internal(&mut self, handle: crate::data::Index, val: T);
}
/// Helper trait to address multiple elements at once.
pub trait BundleSet<'a, T> {
/// Access multiple elements from this set.
fn index_bundle(&'a self, handle: Index) -> T;
}
impl<'a, T, A, B> BundleSet<'a, (&'a A, &'a B)> for T
where
T: ComponentSet<A> + ComponentSet<B>,
{
#[inline(always)]
fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B) {
(self.index(handle), self.index(handle))
}
}
impl<'a, T, A, B, C> BundleSet<'a, (&'a A, &'a B, &'a C)> for T
where
T: ComponentSet<A> + ComponentSet<B> + ComponentSet<C>,
{
#[inline(always)]
fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C) {
(self.index(handle), self.index(handle), self.index(handle))
}
}
impl<'a, T, A, B, C, D> BundleSet<'a, (&'a A, &'a B, &'a C, &'a D)> for T
where
T: ComponentSet<A> + ComponentSet<B> + ComponentSet<C> + ComponentSet<D>,
{
#[inline(always)]
fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C, &'a D) {
(
self.index(handle),
self.index(handle),
self.index(handle),
self.index(handle),
)
}
}
impl<'a, T, A, B, C, D, E> BundleSet<'a, (&'a A, &'a B, &'a C, &'a D, &'a E)> for T
where
T: ComponentSet<A> + ComponentSet<B> + ComponentSet<C> + ComponentSet<D> + ComponentSet<E>,
{
#[inline(always)]
fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C, &'a D, &'a E) {
(
self.index(handle),
self.index(handle),
self.index(handle),
self.index(handle),
self.index(handle),
)
}
}
impl<'a, T, A, B, C, D, E, F> BundleSet<'a, (&'a A, &'a B, &'a C, &'a D, &'a E, &'a F)> for T
where
T: ComponentSet<A>
+ ComponentSet<B>
+ ComponentSet<C>
+ ComponentSet<D>
+ ComponentSet<E>
+ ComponentSet<F>,
{
#[inline(always)]
fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C, &'a D, &'a E, &'a F) {
(
self.index(handle),
self.index(handle),
self.index(handle),
self.index(handle),
self.index(handle),
self.index(handle),
)
}
}

2
src/data/mod.rs
View File

@@ -2,10 +2,8 @@
pub use self::arena::{Arena, Index};
pub use self::coarena::Coarena;
pub use self::component_set::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
pub mod arena;
mod coarena;
mod component_set;
pub(crate) mod graph;
pub mod pubsub;

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

@@ -1,11 +1,11 @@
use super::TOIEntry;
use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
use crate::dynamics::{IslandManager, RigidBodyColliders, RigidBodyForces};
use crate::dynamics::{
RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
IslandManager, RigidBodyCcd, RigidBodyColliders, RigidBodyForces, RigidBodyHandle,
RigidBodyMassProps, RigidBodyPosition, RigidBodySet, RigidBodyVelocity,
};
use crate::geometry::{
ColliderParent, ColliderPosition, ColliderShape, ColliderType, CollisionEvent, NarrowPhase,
ColliderParent, ColliderPosition, ColliderSet, ColliderShape, ColliderType, CollisionEvent,
NarrowPhase,
};
use crate::math::Real;
use crate::parry::utils::SortedPair;
@@ -57,13 +57,7 @@ impl CCDSolver {
/// Apply motion-clamping to the bodies affected by the given `impacts`.
///
/// The `impacts` should be the result of a previous call to `self.predict_next_impacts`.
pub fn clamp_motions<Bodies>(&self, dt: Real, bodies: &mut Bodies, impacts: &PredictedImpacts)
where
Bodies: ComponentSet<RigidBodyCcd>
+ ComponentSetMut<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
pub fn clamp_motions(&self, dt: Real, bodies: &mut RigidBodySet, impacts: &PredictedImpacts) {
match impacts {
PredictedImpacts::Impacts(tois) => {
for (handle, toi) in tois {
@@ -93,18 +87,13 @@ impl CCDSolver {
/// Updates the set of bodies that needs CCD to be resolved.
///
/// Returns `true` if any rigid-body must have CCD resolved.
pub fn update_ccd_active_flags<Bodies>(
pub fn update_ccd_active_flags(
&self,
islands: &IslandManager,
bodies: &mut Bodies,
bodies: &mut RigidBodySet,
dt: Real,
include_forces: bool,
) -> bool
where
Bodies: ComponentSetMut<RigidBodyCcd>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyForces>,
{
) -> bool {
let mut ccd_active = false;
// println!("Checking CCD activation");
@@ -128,27 +117,14 @@ impl CCDSolver {
}
/// Find the first time a CCD-enabled body has a non-sensor collider hitting another non-sensor collider.
pub fn find_first_impact<Bodies, Colliders>(
pub fn find_first_impact(
&mut self,
dt: Real,
islands: &IslandManager,
bodies: &Bodies,
colliders: &Colliders,
bodies: &RigidBodySet,
colliders: &ColliderSet,
narrow_phase: &NarrowPhase,
) -> Option<Real>
where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyCcd>
+ ComponentSet<RigidBodyColliders>
+ ComponentSet<RigidBodyForces>
+ ComponentSet<RigidBodyMassProps>,
Colliders: ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>
+ ComponentSet<ColliderType>
+ ComponentSet<ColliderFlags>,
{
) -> Option<Real> {
// Update the query pipeline.
self.query_pipeline.update_with_mode(
islands,
@@ -266,29 +242,15 @@ impl CCDSolver {
}
/// Outputs the set of bodies as well as their first time-of-impact event.
pub fn predict_impacts_at_next_positions<Bodies, Colliders>(
pub fn predict_impacts_at_next_positions(
&mut self,
dt: Real,
islands: &IslandManager,
bodies: &Bodies,
colliders: &Colliders,
bodies: &RigidBodySet,
colliders: &ColliderSet,
narrow_phase: &NarrowPhase,
events: &dyn EventHandler,
) -> PredictedImpacts
where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyCcd>
+ ComponentSet<RigidBodyColliders>
+ ComponentSet<RigidBodyForces>
+ ComponentSet<RigidBodyMassProps>,
Colliders: ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>
+ ComponentSet<ColliderType>
+ ComponentSet<ColliderFlags>
+ ComponentSet<ColliderFlags>,
{
) -> PredictedImpacts {
let mut frozen = HashMap::<_, Real>::default();
let mut all_toi = BinaryHeap::new();
let mut pairs_seen = HashMap::default();

48
src/dynamics/island_manager.rs
View File

@@ -1,9 +1,8 @@
use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
use crate::dynamics::{
ImpulseJointSet, MultibodyJointSet, RigidBodyActivation, RigidBodyColliders, RigidBodyHandle,
RigidBodyIds, RigidBodyType, RigidBodyVelocity,
RigidBodyIds, RigidBodySet, RigidBodyType, RigidBodyVelocity,
};
use crate::geometry::{ColliderParent, NarrowPhase};
use crate::geometry::{ColliderSet, NarrowPhase};
use crate::math::Real;
use crate::utils::WDot;
@@ -40,10 +39,7 @@ impl IslandManager {
}
/// Update this data-structure after one or multiple rigid-bodies have been removed for `bodies`.
pub fn cleanup_removed_rigid_bodies(
&mut self,
bodies: &mut impl ComponentSetMut<RigidBodyIds>,
) {
pub fn cleanup_removed_rigid_bodies(&mut self, bodies: &mut RigidBodySet) {
let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set];
for active_set in &mut active_sets {
@@ -69,7 +65,7 @@ impl IslandManager {
&mut self,
removed_handle: RigidBodyHandle,
removed_ids: &RigidBodyIds,
bodies: &mut impl ComponentSetMut<RigidBodyIds>,
bodies: &mut RigidBodySet,
) {
let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set];
@@ -77,10 +73,11 @@ impl IslandManager {
if active_set.get(removed_ids.active_set_id) == Some(&removed_handle) {
active_set.swap_remove(removed_ids.active_set_id);
if let Some(replacement) = active_set.get(removed_ids.active_set_id) {
bodies.map_mut_internal(replacement.0, |ids| {
ids.active_set_id = removed_ids.active_set_id;
});
if let Some(replacement) = active_set
.get(removed_ids.active_set_id)
.and_then(|h| bodies.get_mut_internal(*h))
{
replacement.ids.active_set_id = removed_ids.active_set_id;
}
}
}
@@ -90,17 +87,11 @@ impl IslandManager {
///
/// If `strong` is `true` then it is assured that the rigid-body will
/// remain awake during multiple subsequent timesteps.
pub fn wake_up<Bodies>(&mut self, bodies: &mut Bodies, handle: RigidBodyHandle, strong: bool)
where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSetOption<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
{
pub fn wake_up(&mut self, bodies: &mut RigidBodySet, handle: RigidBodyHandle, strong: bool) {
// NOTE: the use an Option here because there are many legitimate cases (like when
// deleting a joint attached to an already-removed body) where we could be
// attempting to wake-up a rigid-body that has already been deleted.
let rb_type: Option<RigidBodyType> = bodies.get(handle.0).copied();
if rb_type == Some(RigidBodyType::Dynamic) {
if bodies.get(handle).map(|rb| rb.body_type()) == Some(RigidBodyType::Dynamic) {
bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| {
activation.wake_up(strong)
});
@@ -141,23 +132,16 @@ impl IslandManager {
self.active_islands[island_id]..self.active_islands[island_id + 1]
}
pub(crate) fn update_active_set_with_contacts<Bodies, Colliders>(
pub(crate) fn update_active_set_with_contacts(
&mut self,
dt: Real,
bodies: &mut Bodies,
colliders: &Colliders,
bodies: &mut RigidBodySet,
colliders: &ColliderSet,
narrow_phase: &NarrowPhase,
impulse_joints: &ImpulseJointSet,
multibody_joints: &MultibodyJointSet,
min_island_size: usize,
) where
Bodies: ComponentSetMut<RigidBodyIds>
+ ComponentSetMut<RigidBodyActivation>
+ ComponentSetMut<RigidBodyVelocity>
+ ComponentSet<RigidBodyColliders>
+ ComponentSet<RigidBodyType>,
Colliders: ComponentSetOption<ColliderParent>,
{
) {
assert!(
min_island_size > 0,
"The minimum island size must be at least 1."
@@ -203,7 +187,7 @@ impl IslandManager {
#[inline(always)]
fn push_contacting_bodies(
rb_colliders: &RigidBodyColliders,
colliders: &impl ComponentSetOption<ColliderParent>,
colliders: &ColliderSet,
narrow_phase: &NarrowPhase,
stack: &mut Vec<RigidBodyHandle>,
) {

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

@@ -2,9 +2,11 @@ use super::ImpulseJoint;
use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex};
use crate::data::arena::Arena;
use crate::data::{BundleSet, Coarena, ComponentSet, ComponentSetMut};
use crate::dynamics::{GenericJoint, RigidBodyHandle};
use crate::dynamics::{IslandManager, RigidBodyActivation, RigidBodyIds, RigidBodyType};
use crate::data::Coarena;
use crate::dynamics::{
GenericJoint, IslandManager, RigidBodyActivation, RigidBodyHandle, RigidBodyIds, RigidBodySet,
RigidBodyType,
};
/// The unique identifier of a joint added to the joint set.
/// The unique identifier of a collider added to a collider set.
@@ -215,16 +217,12 @@ impl ImpulseJointSet {
/// Retrieve all the impulse_joints happening between two active bodies.
// NOTE: this is very similar to the code from NarrowPhase::select_active_interactions.
pub(crate) fn select_active_interactions<Bodies>(
pub(crate) fn select_active_interactions(
&self,
islands: &IslandManager,
bodies: &Bodies,
bodies: &RigidBodySet,
out: &mut Vec<Vec<JointIndex>>,
) where
Bodies: ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyActivation>
+ ComponentSet<RigidBodyIds>,
{
) {
for out_island in &mut out[..islands.num_islands()] {
out_island.clear();
}
@@ -263,18 +261,13 @@ impl ImpulseJointSet {
///
/// If `wake_up` is set to `true`, then the bodies attached to this joint will be
/// automatically woken up.
pub fn remove<Bodies>(
pub fn remove(
&mut self,
handle: ImpulseJointHandle,
islands: &mut IslandManager,
bodies: &mut Bodies,
bodies: &mut RigidBodySet,
wake_up: bool,
) -> Option<ImpulseJoint>
where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
{
) -> Option<ImpulseJoint> {
let id = self.joint_ids.remove(handle.0)?;
let endpoints = self.joint_graph.graph.edge_endpoints(id)?;
@@ -302,17 +295,12 @@ impl ImpulseJointSet {
/// The provided rigid-body handle is not required to identify a rigid-body that
/// is still contained by the `bodies` component set.
/// Returns the (now invalid) handles of the removed impulse_joints.
pub fn remove_joints_attached_to_rigid_body<Bodies>(
pub fn remove_joints_attached_to_rigid_body(
&mut self,
handle: RigidBodyHandle,
islands: &mut IslandManager,
bodies: &mut Bodies,
) -> Vec<ImpulseJointHandle>
where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
{
bodies: &mut RigidBodySet,
) -> Vec<ImpulseJointHandle> {
let mut deleted = vec![];
if let Some(deleted_id) = self

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

@@ -1,10 +1,8 @@
use super::multibody_link::{MultibodyLink, MultibodyLinkVec};
use super::multibody_workspace::MultibodyWorkspace;
use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
use crate::dynamics::solver::AnyJointVelocityConstraint;
use crate::dynamics::{
IntegrationParameters, RigidBodyForces, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition,
RigidBodyType, RigidBodyVelocity,
solver::AnyJointVelocityConstraint, IntegrationParameters, RigidBodyForces, RigidBodyHandle,
RigidBodyMassProps, RigidBodySet, RigidBodyType, RigidBodyVelocity,
};
#[cfg(feature = "dim3")]
use crate::math::Matrix;
@@ -369,12 +367,7 @@ impl Multibody {
.extend((0..num_jacobians).map(|_| Jacobian::zeros(0)));
}
pub(crate) fn update_acceleration<Bodies>(&mut self, bodies: &Bodies)
where
Bodies: ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyForces>
+ ComponentSet<RigidBodyVelocity>,
{
pub(crate) fn update_acceleration(&mut self, bodies: &RigidBodySet) {
if self.ndofs == 0 {
return; // Nothing to do.
}
@@ -452,10 +445,7 @@ impl Multibody {
}
/// Computes the constant terms of the dynamics.
pub(crate) fn update_dynamics<Bodies>(&mut self, dt: Real, bodies: &mut Bodies)
where
Bodies: ComponentSetMut<RigidBodyVelocity> + ComponentSet<RigidBodyMassProps>,
{
pub(crate) fn update_dynamics(&mut self, dt: Real, bodies: &mut RigidBodySet) {
/*
* Compute velocities.
* NOTE: this is needed for kinematic bodies too.
@@ -545,10 +535,7 @@ impl Multibody {
}
}
fn update_inertias<Bodies>(&mut self, dt: Real, bodies: &Bodies)
where
Bodies: ComponentSet<RigidBodyMassProps> + ComponentSet<RigidBodyVelocity>,
{
fn update_inertias(&mut self, dt: Real, bodies: &RigidBodySet) {
if self.ndofs == 0 {
return; // Nothing to do.
}
@@ -790,17 +777,11 @@ impl Multibody {
}
}
pub(crate) fn update_root_type<Bodies>(&mut self, bodies: &mut Bodies)
where
Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyPosition>,
{
let rb_type: Option<&RigidBodyType> = bodies.get(self.links[0].rigid_body.0);
if let Some(rb_type) = rb_type {
let rb_pos: &RigidBodyPosition = bodies.index(self.links[0].rigid_body.0);
if rb_type.is_dynamic() != self.root_is_dynamic {
if rb_type.is_dynamic() {
let free_joint = MultibodyJoint::free(rb_pos.position);
pub(crate) fn update_root_type(&mut self, bodies: &mut RigidBodySet) {
if let Some(rb) = bodies.get(self.links[0].rigid_body) {
if rb.is_dynamic() != self.root_is_dynamic {
if rb.is_dynamic() {
let free_joint = MultibodyJoint::free(*rb.position());
let prev_root_ndofs = self.links[0].joint().ndofs();
self.links[0].joint = free_joint;
self.links[0].assembly_id = 0;
@@ -819,7 +800,7 @@ impl Multibody {
assert!(self.damping.len() >= SPATIAL_DIM);
assert!(self.accelerations.len() >= SPATIAL_DIM);
let fixed_joint = MultibodyJoint::fixed(rb_pos.position);
let fixed_joint = MultibodyJoint::fixed(*rb.position());
let prev_root_ndofs = self.links[0].joint().ndofs();
self.links[0].joint = fixed_joint;
self.links[0].assembly_id = 0;
@@ -844,39 +825,31 @@ impl Multibody {
}
}
self.root_is_dynamic = rb_type.is_dynamic();
self.root_is_dynamic = rb.is_dynamic();
}
// Make sure the positions are properly set to match the rigid-body’s.
if self.links[0].joint.data.locked_axes.is_empty() {
self.links[0].joint.set_free_pos(rb_pos.position);
self.links[0].joint.set_free_pos(*rb.position());
} else {
self.links[0].joint.data.local_frame1 = rb_pos.position;
self.links[0].joint.data.local_frame1 = *rb.position();
}
}
}
/// Apply forward-kinematics to this multibody and its related rigid-bodies.
pub fn forward_kinematics<Bodies>(&mut self, bodies: &mut Bodies, update_mass_props: bool)
where
Bodies: ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyMassProps>
+ ComponentSetMut<RigidBodyPosition>,
{
pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_mass_props: bool) {
// Special case for the root, which has no parent.
{
let link = &mut self.links[0];
link.local_to_parent = link.joint.body_to_parent();
link.local_to_world = link.local_to_parent;
bodies.map_mut_internal(link.rigid_body.0, |rb_pos: &mut RigidBodyPosition| {
rb_pos.next_position = link.local_to_world;
});
if let Some(rb) = bodies.get_mut_internal(link.rigid_body) {
rb.pos.next_position = link.local_to_world;
if update_mass_props {
bodies.map_mut_internal(link.rigid_body.0, |mprops: &mut RigidBodyMassProps| {
mprops.update_world_mass_properties(&link.local_to_world)
});
rb.mprops.update_world_mass_properties(&link.local_to_world);
}
}
}
@@ -888,32 +861,30 @@ impl Multibody {
link.local_to_world = parent_link.local_to_world * link.local_to_parent;
{
let parent_rb_mprops: &RigidBodyMassProps = bodies.index(parent_link.rigid_body.0);
let rb_mprops: &RigidBodyMassProps = bodies.index(link.rigid_body.0);
let c0 = parent_link.local_to_world * parent_rb_mprops.local_mprops.local_com;
let parent_rb = &bodies[parent_link.rigid_body];
let link_rb = &bodies[link.rigid_body];
let c0 = parent_link.local_to_world * parent_rb.mprops.local_mprops.local_com;
let c2 = link.local_to_world
* Point::from(link.joint.data.local_frame2.translation.vector);
let c3 = link.local_to_world * rb_mprops.local_mprops.local_com;
let c3 = link.local_to_world * link_rb.mprops.local_mprops.local_com;
link.shift02 = c2 - c0;
link.shift23 = c3 - c2;
}
bodies.map_mut_internal(link.rigid_body.0, |rb_pos: &mut RigidBodyPosition| {
rb_pos.next_position = link.local_to_world;
});
let link_rb = bodies.index_mut_internal(link.rigid_body);
link_rb.pos.next_position = link.local_to_world;
let rb_type: &RigidBodyType = bodies.index(link.rigid_body.0);
assert_eq!(
*rb_type,
link_rb.body_type,
RigidBodyType::Dynamic,
"A rigid-body that is not at the root of a multibody must be dynamic."
);
if update_mass_props {
bodies.map_mut_internal(link.rigid_body.0, |rb_mprops: &mut RigidBodyMassProps| {
rb_mprops.update_world_mass_properties(&link.local_to_world)
});
link_rb
.mprops
.update_world_mass_properties(&link.local_to_world);
}
}

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

@@ -1,8 +1,7 @@
use crate::data::{Arena, Coarena, ComponentSet, ComponentSetMut, Index};
use crate::data::{Arena, Coarena, Index};
use crate::dynamics::joint::MultibodyLink;
use crate::dynamics::{
GenericJoint, IslandManager, Multibody, MultibodyJoint, RigidBodyActivation, RigidBodyHandle,
RigidBodyIds, RigidBodyType,
GenericJoint, IslandManager, Multibody, MultibodyJoint, RigidBodyHandle, RigidBodySet,
};
use crate::geometry::{InteractionGraph, RigidBodyGraphIndex};
use crate::parry::partitioning::IndexedData;
@@ -163,17 +162,13 @@ impl MultibodyJointSet {
}
/// Removes an multibody_joint from this set.
pub fn remove<Bodies>(
pub fn remove(
&mut self,
handle: MultibodyJointHandle,
islands: &mut IslandManager,
bodies: &mut Bodies,
bodies: &mut RigidBodySet,
wake_up: bool,
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
{
) {
if let Some(removed) = self.rb2mb.get(handle.0).copied() {
let multibody = self.multibodies.remove(removed.multibody.0).unwrap();
@@ -216,17 +211,13 @@ impl MultibodyJointSet {
}
/// Removes all the multibody_joints from the multibody the given rigid-body is part of.
pub fn remove_multibody_articulations<Bodies>(
pub fn remove_multibody_articulations(
&mut self,
handle: RigidBodyHandle,
islands: &mut IslandManager,
bodies: &mut Bodies,
bodies: &mut RigidBodySet,
wake_up: bool,
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
{
) {
if let Some(removed) = self.rb2mb.get(handle.0).copied() {
// Remove the multibody.
let multibody = self.multibodies.remove(removed.multibody.0).unwrap();
@@ -248,16 +239,12 @@ impl MultibodyJointSet {
}
/// Removes all the multibody joints attached to a rigid-body.
pub fn remove_joints_attached_to_rigid_body<Bodies>(
pub fn remove_joints_attached_to_rigid_body(
&mut self,
rb_to_remove: RigidBodyHandle,
islands: &mut IslandManager,
bodies: &mut Bodies,
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
{
bodies: &mut RigidBodySet,
) {
// TODO: optimize this.
if let Some(link_to_remove) = self.rb2mb.get(rb_to_remove.0).copied() {
let mut articulations_to_remove = vec![];

390
src/dynamics/rigid_body.rs
View File

@@ -17,21 +17,21 @@ use num::Zero;
/// To create a new rigid-body, use the `RigidBodyBuilder` structure.
#[derive(Debug, Clone)]
pub struct RigidBody {
pub(crate) rb_pos: RigidBodyPosition,
pub(crate) rb_mprops: RigidBodyMassProps,
pub(crate) rb_vels: RigidBodyVelocity,
pub(crate) rb_damping: RigidBodyDamping,
pub(crate) rb_forces: RigidBodyForces,
pub(crate) rb_ccd: RigidBodyCcd,
pub(crate) rb_ids: RigidBodyIds,
pub(crate) rb_colliders: RigidBodyColliders,
pub(crate) pos: RigidBodyPosition,
pub(crate) mprops: RigidBodyMassProps,
pub(crate) vels: RigidBodyVelocity,
pub(crate) damping: RigidBodyDamping,
pub(crate) forces: RigidBodyForces,
pub(crate) ccd: RigidBodyCcd,
pub(crate) ids: RigidBodyIds,
pub(crate) colliders: RigidBodyColliders,
/// Whether or not this rigid-body is sleeping.
pub(crate) rb_activation: RigidBodyActivation,
pub(crate) activation: RigidBodyActivation,
pub(crate) changes: RigidBodyChanges,
/// The status of the body, governing how it is affected by external forces.
pub(crate) rb_type: RigidBodyType,
pub(crate) body_type: RigidBodyType,
/// The dominance group this rigid-body is part of.
pub(crate) rb_dominance: RigidBodyDominance,
pub(crate) dominance: RigidBodyDominance,
/// User-defined data associated to this rigid-body.
pub user_data: u128,
}
@@ -45,75 +45,75 @@ impl Default for RigidBody {
impl RigidBody {
fn new() -> Self {
Self {
rb_pos: RigidBodyPosition::default(),
rb_mprops: RigidBodyMassProps::default(),
rb_vels: RigidBodyVelocity::default(),
rb_damping: RigidBodyDamping::default(),
rb_forces: RigidBodyForces::default(),
rb_ccd: RigidBodyCcd::default(),
rb_ids: RigidBodyIds::default(),
rb_colliders: RigidBodyColliders::default(),
rb_activation: RigidBodyActivation::active(),
pos: RigidBodyPosition::default(),
mprops: RigidBodyMassProps::default(),
vels: RigidBodyVelocity::default(),
damping: RigidBodyDamping::default(),
forces: RigidBodyForces::default(),
ccd: RigidBodyCcd::default(),
ids: RigidBodyIds::default(),
colliders: RigidBodyColliders::default(),
activation: RigidBodyActivation::active(),
changes: RigidBodyChanges::all(),
rb_type: RigidBodyType::Dynamic,
rb_dominance: RigidBodyDominance::default(),
body_type: RigidBodyType::Dynamic,
dominance: RigidBodyDominance::default(),
user_data: 0,
}
}
pub(crate) fn reset_internal_references(&mut self) {
self.rb_colliders.0 = Vec::new();
self.rb_ids = Default::default();
self.colliders.0 = Vec::new();
self.ids = Default::default();
}
/// The activation status of this rigid-body.
pub fn activation(&self) -> &RigidBodyActivation {
&self.rb_activation
&self.activation
}
/// Mutable reference to the activation status of this rigid-body.
pub fn activation_mut(&mut self) -> &mut RigidBodyActivation {
self.changes |= RigidBodyChanges::SLEEP;
&mut self.rb_activation
&mut self.activation
}
/// The linear damping coefficient of this rigid-body.
#[inline]
pub fn linear_damping(&self) -> Real {
self.rb_damping.linear_damping
self.damping.linear_damping
}
/// Sets the linear damping coefficient of this rigid-body.
#[inline]
pub fn set_linear_damping(&mut self, damping: Real) {
self.rb_damping.linear_damping = damping;
self.damping.linear_damping = damping;
}
/// The angular damping coefficient of this rigid-body.
#[inline]
pub fn angular_damping(&self) -> Real {
self.rb_damping.angular_damping
self.damping.angular_damping
}
/// Sets the angular damping coefficient of this rigid-body.
#[inline]
pub fn set_angular_damping(&mut self, damping: Real) {
self.rb_damping.angular_damping = damping
self.damping.angular_damping = damping
}
/// The type of this rigid-body.
pub fn body_type(&self) -> RigidBodyType {
self.rb_type
self.body_type
}
/// Sets the type of this rigid-body.
pub fn set_body_type(&mut self, status: RigidBodyType) {
if status != self.rb_type {
if status != self.body_type {
self.changes.insert(RigidBodyChanges::TYPE);
self.rb_type = status;
self.body_type = status;
if status == RigidBodyType::Fixed {
self.rb_vels = RigidBodyVelocity::zero();
self.vels = RigidBodyVelocity::zero();
}
}
}
@@ -121,7 +121,7 @@ impl RigidBody {
/// The mass properties of this rigid-body.
#[inline]
pub fn mass_properties(&self) -> &MassProperties {
&self.rb_mprops.local_mprops
&self.mprops.local_mprops
}
/// The dominance group of this rigid-body.
@@ -130,18 +130,18 @@ impl RigidBody {
/// rigid-bodies.
#[inline]
pub fn effective_dominance_group(&self) -> i16 {
self.rb_dominance.effective_group(&self.rb_type)
self.dominance.effective_group(&self.body_type)
}
/// Sets the axes along which this rigid-body cannot translate or rotate.
#[inline]
pub fn set_locked_axes(&mut self, locked_axes: LockedAxes, wake_up: bool) {
if locked_axes != self.rb_mprops.flags {
if locked_axes != self.mprops.flags {
if self.is_dynamic() && wake_up {
self.wake_up(true);
}
self.rb_mprops.flags = locked_axes;
self.mprops.flags = locked_axes;
self.update_world_mass_properties();
}
}
@@ -149,20 +149,14 @@ impl RigidBody {
#[inline]
/// Locks or unlocks all the rotations of this rigid-body.
pub fn lock_rotations(&mut self, locked: bool, wake_up: bool) {
if !self.rb_mprops.flags.contains(LockedAxes::ROTATION_LOCKED) {
if !self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED) {
if self.is_dynamic() && wake_up {
self.wake_up(true);
}
self.rb_mprops
.flags
.set(LockedAxes::ROTATION_LOCKED_X, locked);
self.rb_mprops
.flags
.set(LockedAxes::ROTATION_LOCKED_Y, locked);
self.rb_mprops
.flags
.set(LockedAxes::ROTATION_LOCKED_Z, locked);
self.mprops.flags.set(LockedAxes::ROTATION_LOCKED_X, locked);
self.mprops.flags.set(LockedAxes::ROTATION_LOCKED_Y, locked);
self.mprops.flags.set(LockedAxes::ROTATION_LOCKED_Z, locked);
self.update_world_mass_properties();
}
}
@@ -176,21 +170,21 @@ impl RigidBody {
allow_rotations_z: bool,
wake_up: bool,
) {
if self.rb_mprops.flags.contains(LockedAxes::ROTATION_LOCKED_X) != !allow_rotations_x
|| self.rb_mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Y) != !allow_rotations_y
|| self.rb_mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z) != !allow_rotations_z
if self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_X) != !allow_rotations_x
|| self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Y) != !allow_rotations_y
|| self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z) != !allow_rotations_z
{
if self.is_dynamic() && wake_up {
self.wake_up(true);
}
self.rb_mprops
self.mprops
.flags
.set(LockedAxes::ROTATION_LOCKED_X, !allow_rotations_x);
self.rb_mprops
self.mprops
.flags
.set(LockedAxes::ROTATION_LOCKED_Y, !allow_rotations_y);
self.rb_mprops
self.mprops
.flags
.set(LockedAxes::ROTATION_LOCKED_Z, !allow_rotations_z);
self.update_world_mass_properties();
@@ -200,16 +194,12 @@ impl RigidBody {
#[inline]
/// Locks or unlocks all the rotations of this rigid-body.
pub fn lock_translations(&mut self, locked: bool, wake_up: bool) {
if !self
.rb_mprops
.flags
.contains(LockedAxes::TRANSLATION_LOCKED)
{
if !self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED) {
if self.is_dynamic() && wake_up {
self.wake_up(true);
}
self.rb_mprops
self.mprops
.flags
.set(LockedAxes::TRANSLATION_LOCKED, locked);
self.update_world_mass_properties();
@@ -226,36 +216,16 @@ impl RigidBody {
wake_up: bool,
) {
#[cfg(feature = "dim2")]
if self
.rb_mprops
.flags
.contains(LockedAxes::TRANSLATION_LOCKED_X)
== !allow_translation_x
&& self
.rb_mprops
.flags
.contains(LockedAxes::TRANSLATION_LOCKED_Y)
== !allow_translation_y
if self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED_X) == !allow_translation_x
&& self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED_Y) == !allow_translation_y
{
// Nothing to change.
return;
}
#[cfg(feature = "dim3")]
if self
.rb_mprops
.flags
.contains(LockedAxes::TRANSLATION_LOCKED_X)
== !allow_translation_x
&& self
.rb_mprops
.flags
.contains(LockedAxes::TRANSLATION_LOCKED_Y)
== !allow_translation_y
&& self
.rb_mprops
.flags
.contains(LockedAxes::TRANSLATION_LOCKED_Z)
== !allow_translation_z
if self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED_X) == !allow_translation_x
&& self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED_Y) == !allow_translation_y
&& self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED_Z) == !allow_translation_z
{
// Nothing to change.
return;
@@ -265,14 +235,14 @@ impl RigidBody {
self.wake_up(true);
}
self.rb_mprops
self.mprops
.flags
.set(LockedAxes::TRANSLATION_LOCKED_X, !allow_translation_x);
self.rb_mprops
self.mprops
.flags
.set(LockedAxes::TRANSLATION_LOCKED_Y, !allow_translation_y);
#[cfg(feature = "dim3")]
self.rb_mprops
self.mprops
.flags
.set(LockedAxes::TRANSLATION_LOCKED_Z, !allow_translation_z);
self.update_world_mass_properties();
@@ -281,7 +251,7 @@ impl RigidBody {
/// Are the translations of this rigid-body locked?
#[cfg(feature = "dim2")]
pub fn is_translation_locked(&self) -> bool {
self.rb_mprops
self.mprops
.flags
.contains(LockedAxes::TRANSLATION_LOCKED_X | LockedAxes::TRANSLATION_LOCKED_Y)
}
@@ -289,24 +259,22 @@ impl RigidBody {
/// Are the translations of this rigid-body locked?
#[cfg(feature = "dim3")]
pub fn is_translation_locked(&self) -> bool {
self.rb_mprops
.flags
.contains(LockedAxes::TRANSLATION_LOCKED)
self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED)
}
/// Are the rotations of this rigid-body locked?
#[cfg(feature = "dim2")]
pub fn is_rotation_locked(&self) -> bool {
self.rb_mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z)
self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z)
}
/// Returns `true` for each rotational degrees of freedom locked on this rigid-body.
#[cfg(feature = "dim3")]
pub fn is_rotation_locked(&self) -> [bool; 3] {
[
self.rb_mprops.flags.contains(LockedAxes::ROTATION_LOCKED_X),
self.rb_mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Y),
self.rb_mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z),
self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_X),
self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Y),
self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z),
]
}
@@ -314,12 +282,12 @@ impl RigidBody {
///
/// CCD prevents tunneling, but may still allow limited interpenetration of colliders.
pub fn enable_ccd(&mut self, enabled: bool) {
self.rb_ccd.ccd_enabled = enabled;
self.ccd.ccd_enabled = enabled;
}
/// Is CCD (continous collision-detection) enabled for this rigid-body?
pub fn is_ccd_enabled(&self) -> bool {
self.rb_ccd.ccd_enabled
self.ccd.ccd_enabled
}
// This is different from `is_ccd_enabled`. This checks that CCD
@@ -334,7 +302,7 @@ impl RigidBody {
/// checks if CCD is allowed to run for this rigid-body or if
/// it is completely disabled (independently from its velocity).
pub fn is_ccd_active(&self) -> bool {
self.rb_ccd.ccd_active
self.ccd.ccd_active
}
/// Sets the rigid-body's initial mass properties.
@@ -343,47 +311,47 @@ impl RigidBody {
/// put to sleep because it did not move for a while.
#[inline]
pub fn set_mass_properties(&mut self, props: MassProperties, wake_up: bool) {
if self.rb_mprops.local_mprops != props {
if self.mprops.local_mprops != props {
if self.is_dynamic() && wake_up {
self.wake_up(true);
}
self.rb_mprops.local_mprops = props;
self.mprops.local_mprops = props;
self.update_world_mass_properties();
}
}
/// The handles of colliders attached to this rigid body.
pub fn colliders(&self) -> &[ColliderHandle] {
&self.rb_colliders.0[..]
&self.colliders.0[..]
}
/// Is this rigid body dynamic?
///
/// A dynamic body can move freely and is affected by forces.
pub fn is_dynamic(&self) -> bool {
self.rb_type == RigidBodyType::Dynamic
self.body_type == RigidBodyType::Dynamic
}
/// Is this rigid body kinematic?
///
/// A kinematic body can move freely but is not affected by forces.
pub fn is_kinematic(&self) -> bool {
self.rb_type.is_kinematic()
self.body_type.is_kinematic()
}
/// Is this rigid body fixed?
///
/// A fixed body cannot move and is not affected by forces.
pub fn is_fixed(&self) -> bool {
self.rb_type == RigidBodyType::Fixed
self.body_type == RigidBodyType::Fixed
}
/// The mass of this rigid body.
///
/// Returns zero if this rigid body has an infinite mass.
pub fn mass(&self) -> Real {
self.rb_mprops.local_mprops.mass()
self.mprops.local_mprops.mass()
}
/// The predicted position of this rigid-body.
@@ -392,36 +360,36 @@ impl RigidBody {
/// method and is used for estimating the kinematic body velocity at the next timestep.
/// For non-kinematic bodies, this value is currently unspecified.
pub fn next_position(&self) -> &Isometry<Real> {
&self.rb_pos.next_position
&self.pos.next_position
}
/// The scale factor applied to the gravity affecting this rigid-body.
pub fn gravity_scale(&self) -> Real {
self.rb_forces.gravity_scale
self.forces.gravity_scale
}
/// Sets the gravity scale facter for this rigid-body.
pub fn set_gravity_scale(&mut self, scale: Real, wake_up: bool) {
if self.rb_forces.gravity_scale != scale {
if wake_up && self.rb_activation.sleeping {
if self.forces.gravity_scale != scale {
if wake_up && self.activation.sleeping {
self.changes.insert(RigidBodyChanges::SLEEP);
self.rb_activation.sleeping = false;
self.activation.sleeping = false;
}
self.rb_forces.gravity_scale = scale;
self.forces.gravity_scale = scale;
}
}
/// The dominance group of this rigid-body.
pub fn dominance_group(&self) -> i8 {
self.rb_dominance.0
self.dominance.0
}
/// The dominance group of this rigid-body.
pub fn set_dominance_group(&mut self, dominance: i8) {
if self.rb_dominance.0 != dominance {
if self.dominance.0 != dominance {
self.changes.insert(RigidBodyChanges::DOMINANCE);
self.rb_dominance.0 = dominance
self.dominance.0 = dominance
}
}
@@ -435,11 +403,11 @@ impl RigidBody {
co_shape: &ColliderShape,
co_mprops: &ColliderMassProps,
) {
self.rb_colliders.attach_collider(
self.colliders.attach_collider(
&mut self.changes,
&mut self.rb_ccd,
&mut self.rb_mprops,
&self.rb_pos,
&mut self.ccd,
&mut self.mprops,
&self.pos,
co_handle,
co_pos,
co_parent,
@@ -450,14 +418,14 @@ impl RigidBody {
/// Removes a collider from this rigid-body.
pub(crate) fn remove_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
if let Some(i) = self.rb_colliders.0.iter().position(|e| *e == handle) {
if let Some(i) = self.colliders.0.iter().position(|e| *e == handle) {
self.changes.set(RigidBodyChanges::COLLIDERS, true);
self.rb_colliders.0.swap_remove(i);
self.colliders.0.swap_remove(i);
let mass_properties = coll
.mass_properties()
.transform_by(coll.position_wrt_parent().unwrap());
self.rb_mprops.local_mprops -= mass_properties;
self.mprops.local_mprops -= mass_properties;
self.update_world_mass_properties();
}
}
@@ -468,8 +436,8 @@ impl RigidBody {
/// it is waken up. It can be woken manually with `self.wake_up` or automatically due to
/// external forces like contacts.
pub fn sleep(&mut self) {
self.rb_activation.sleep();
self.rb_vels = RigidBodyVelocity::zero();
self.activation.sleep();
self.vels = RigidBodyVelocity::zero();
}
/// Wakes up this rigid body if it is sleeping.
@@ -477,11 +445,11 @@ impl RigidBody {
/// If `strong` is `true` then it is assured that the rigid-body will
/// remain awake during multiple subsequent timesteps.
pub fn wake_up(&mut self, strong: bool) {
if self.rb_activation.sleeping {
if self.activation.sleeping {
self.changes.insert(RigidBodyChanges::SLEEP);
}
self.rb_activation.wake_up(strong);
self.activation.wake_up(strong);
}
/// Is this rigid body sleeping?
@@ -490,29 +458,29 @@ impl RigidBody {
// - return false for fixed bodies.
// - return true for non-sleeping dynamic bodies.
// - return true only for kinematic bodies with non-zero velocity?
self.rb_activation.sleeping
self.activation.sleeping
}
/// Is the velocity of this body not zero?
pub fn is_moving(&self) -> bool {
!self.rb_vels.linvel.is_zero() || !self.rb_vels.angvel.is_zero()
!self.vels.linvel.is_zero() || !self.vels.angvel.is_zero()
}
/// The linear velocity of this rigid-body.
pub fn linvel(&self) -> &Vector<Real> {
&self.rb_vels.linvel
&self.vels.linvel
}
/// The angular velocity of this rigid-body.
#[cfg(feature = "dim2")]
pub fn angvel(&self) -> Real {
self.rb_vels.angvel
self.vels.angvel
}
/// The angular velocity of this rigid-body.
#[cfg(feature = "dim3")]
pub fn angvel(&self) -> &Vector<Real> {
&self.rb_vels.angvel
&self.vels.angvel
}
/// The linear velocity of this rigid-body.
@@ -520,16 +488,16 @@ impl RigidBody {
/// If `wake_up` is `true` then the rigid-body will be woken up if it was
/// put to sleep because it did not move for a while.
pub fn set_linvel(&mut self, linvel: Vector<Real>, wake_up: bool) {
if self.rb_vels.linvel != linvel {
match self.rb_type {
if self.vels.linvel != linvel {
match self.body_type {
RigidBodyType::Dynamic => {
self.rb_vels.linvel = linvel;
self.vels.linvel = linvel;
if wake_up {
self.wake_up(true)
}
}
RigidBodyType::KinematicVelocityBased => {
self.rb_vels.linvel = linvel;
self.vels.linvel = linvel;
}
RigidBodyType::Fixed | RigidBodyType::KinematicPositionBased => {}
}
@@ -542,16 +510,16 @@ impl RigidBody {
/// put to sleep because it did not move for a while.
#[cfg(feature = "dim2")]
pub fn set_angvel(&mut self, angvel: Real, wake_up: bool) {
if self.rb_vels.angvel != angvel {
match self.rb_type {
if self.vels.angvel != angvel {
match self.body_type {
RigidBodyType::Dynamic => {
self.rb_vels.angvel = angvel;
self.vels.angvel = angvel;
if wake_up {
self.wake_up(true)
}
}
RigidBodyType::KinematicVelocityBased => {
self.rb_vels.angvel = angvel;
self.vels.angvel = angvel;
}
RigidBodyType::Fixed | RigidBodyType::KinematicPositionBased => {}
}
@@ -564,16 +532,16 @@ impl RigidBody {
/// put to sleep because it did not move for a while.
#[cfg(feature = "dim3")]
pub fn set_angvel(&mut self, angvel: Vector<Real>, wake_up: bool) {
if self.rb_vels.angvel != angvel {
match self.rb_type {
if self.vels.angvel != angvel {
match self.body_type {
RigidBodyType::Dynamic => {
self.rb_vels.angvel = angvel;
self.vels.angvel = angvel;
if wake_up {
self.wake_up(true)
}
}
RigidBodyType::KinematicVelocityBased => {
self.rb_vels.angvel = angvel;
self.vels.angvel = angvel;
}
RigidBodyType::Fixed | RigidBodyType::KinematicPositionBased => {}
}
@@ -583,24 +551,24 @@ impl RigidBody {
/// The world-space position of this rigid-body.
#[inline]
pub fn position(&self) -> &Isometry<Real> {
&self.rb_pos.position
&self.pos.position
}
/// The translational part of this rigid-body's position.
#[inline]
pub fn translation(&self) -> &Vector<Real> {
&self.rb_pos.position.translation.vector
&self.pos.position.translation.vector
}
/// Sets the translational part of this rigid-body's position.
#[inline]
pub fn set_translation(&mut self, translation: Vector<Real>, wake_up: bool) {
if self.rb_pos.position.translation.vector != translation
|| self.rb_pos.next_position.translation.vector != translation
if self.pos.position.translation.vector != translation
|| self.pos.next_position.translation.vector != translation
{
self.changes.insert(RigidBodyChanges::POSITION);
self.rb_pos.position.translation.vector = translation;
self.rb_pos.next_position.translation.vector = translation;
self.pos.position.translation.vector = translation;
self.pos.next_position.translation.vector = translation;
// TODO: Do we really need to check that the body isn't dynamic?
if wake_up && self.is_dynamic() {
@@ -612,7 +580,7 @@ impl RigidBody {
/// The rotational part of this rigid-body's position.
#[inline]
pub fn rotation(&self) -> &Rotation<Real> {
&self.rb_pos.position.rotation
&self.pos.position.rotation
}
/// Sets the rotational part of this rigid-body's position.
@@ -620,12 +588,10 @@ impl RigidBody {
pub fn set_rotation(&mut self, rotation: AngVector<Real>, wake_up: bool) {
let rotation = Rotation::new(rotation);
if self.rb_pos.position.rotation != rotation
|| self.rb_pos.next_position.rotation != rotation
{
if self.pos.position.rotation != rotation || self.pos.next_position.rotation != rotation {
self.changes.insert(RigidBodyChanges::POSITION);
self.rb_pos.position.rotation = rotation;
self.rb_pos.next_position.rotation = rotation;
self.pos.position.rotation = rotation;
self.pos.next_position.rotation = rotation;
// TODO: Do we really need to check that the body isn't dynamic?
if wake_up && self.is_dynamic() {
@@ -644,10 +610,10 @@ impl RigidBody {
/// If `wake_up` is `true` then the rigid-body will be woken up if it was
/// put to sleep because it did not move for a while.
pub fn set_position(&mut self, pos: Isometry<Real>, wake_up: bool) {
if self.rb_pos.position != pos || self.rb_pos.next_position != pos {
if self.pos.position != pos || self.pos.next_position != pos {
self.changes.insert(RigidBodyChanges::POSITION);
self.rb_pos.position = pos;
self.rb_pos.next_position = pos;
self.pos.position = pos;
self.pos.next_position = pos;
// TODO: Do we really need to check that the body isn't dynamic?
if wake_up && self.is_dynamic() {
@@ -659,38 +625,33 @@ impl RigidBody {
/// If this rigid body is kinematic, sets its future translation after the next timestep integration.
pub fn set_next_kinematic_rotation(&mut self, rotation: AngVector<Real>) {
if self.is_kinematic() {
self.rb_pos.next_position.rotation = Rotation::new(rotation);
self.pos.next_position.rotation = Rotation::new(rotation);
}
}
/// If this rigid body is kinematic, sets its future orientation after the next timestep integration.
pub fn set_next_kinematic_translation(&mut self, translation: Vector<Real>) {
if self.is_kinematic() {
self.rb_pos.next_position.translation = translation.into();
self.pos.next_position.translation = translation.into();
}
}
/// If this rigid body is kinematic, sets its future position after the next timestep integration.
pub fn set_next_kinematic_position(&mut self, pos: Isometry<Real>) {
if self.is_kinematic() {
self.rb_pos.next_position = pos;
self.pos.next_position = pos;
}
}
/// Predicts the next position of this rigid-body, by integrating its velocity and forces
/// by a time of `dt`.
pub fn predict_position_using_velocity_and_forces(&self, dt: Real) -> Isometry<Real> {
self.rb_pos.integrate_forces_and_velocities(
dt,
&self.rb_forces,
&self.rb_vels,
&self.rb_mprops,
)
self.pos
.integrate_forces_and_velocities(dt, &self.forces, &self.vels, &self.mprops)
}
pub(crate) fn update_world_mass_properties(&mut self) {
self.rb_mprops
.update_world_mass_properties(&self.rb_pos.position);
self.mprops.update_world_mass_properties(&self.pos.position);
}
}
@@ -698,8 +659,8 @@ impl RigidBody {
impl RigidBody {
/// Resets to zero all the constant (linear) forces manually applied to this rigid-body.
pub fn reset_forces(&mut self, wake_up: bool) {
if !self.rb_forces.user_force.is_zero() {
self.rb_forces.user_force = na::zero();
if !self.forces.user_force.is_zero() {
self.forces.user_force = na::zero();
if wake_up {
self.wake_up(true);
@@ -709,8 +670,8 @@ impl RigidBody {
/// Resets to zero all the constant torques manually applied to this rigid-body.
pub fn reset_torques(&mut self, wake_up: bool) {
if !self.rb_forces.user_torque.is_zero() {
self.rb_forces.user_torque = na::zero();
if !self.forces.user_torque.is_zero() {
self.forces.user_torque = na::zero();
if wake_up {
self.wake_up(true);
@@ -723,8 +684,8 @@ impl RigidBody {
/// This does nothing on non-dynamic bodies.
pub fn add_force(&mut self, force: Vector<Real>, wake_up: bool) {
if !force.is_zero() {
if self.rb_type == RigidBodyType::Dynamic {
self.rb_forces.user_force += force;
if self.body_type == RigidBodyType::Dynamic {
self.forces.user_force += force;
if wake_up {
self.wake_up(true);
@@ -739,8 +700,8 @@ impl RigidBody {
#[cfg(feature = "dim2")]
pub fn add_torque(&mut self, torque: Real, wake_up: bool) {
if !torque.is_zero() {
if self.rb_type == RigidBodyType::Dynamic {
self.rb_forces.user_torque += torque;
if self.body_type == RigidBodyType::Dynamic {
self.forces.user_torque += torque;
if wake_up {
self.wake_up(true);
@@ -755,8 +716,8 @@ impl RigidBody {
#[cfg(feature = "dim3")]
pub fn add_torque(&mut self, torque: Vector<Real>, wake_up: bool) {
if !torque.is_zero() {
if self.rb_type == RigidBodyType::Dynamic {
self.rb_forces.user_torque += torque;
if self.body_type == RigidBodyType::Dynamic {
self.forces.user_torque += torque;
if wake_up {
self.wake_up(true);
@@ -770,9 +731,9 @@ impl RigidBody {
/// This does nothing on non-dynamic bodies.
pub fn add_force_at_point(&mut self, force: Vector<Real>, point: Point<Real>, wake_up: bool) {
if !force.is_zero() {
if self.rb_type == RigidBodyType::Dynamic {
self.rb_forces.user_force += force;
self.rb_forces.user_torque += (point - self.rb_mprops.world_com).gcross(force);
if self.body_type == RigidBodyType::Dynamic {
self.forces.user_force += force;
self.forces.user_torque += (point - self.mprops.world_com).gcross(force);
if wake_up {
self.wake_up(true);
@@ -788,8 +749,8 @@ impl RigidBody {
/// The impulse is applied right away, changing the linear velocity.
/// This does nothing on non-dynamic bodies.
pub fn apply_impulse(&mut self, impulse: Vector<Real>, wake_up: bool) {
if !impulse.is_zero() && self.rb_type == RigidBodyType::Dynamic {
self.rb_vels.linvel += impulse.component_mul(&self.rb_mprops.effective_inv_mass);
if !impulse.is_zero() && self.body_type == RigidBodyType::Dynamic {
self.vels.linvel += impulse.component_mul(&self.mprops.effective_inv_mass);
if wake_up {
self.wake_up(true);
@@ -802,9 +763,9 @@ impl RigidBody {
/// This does nothing on non-dynamic bodies.
#[cfg(feature = "dim2")]
pub fn apply_torque_impulse(&mut self, torque_impulse: Real, wake_up: bool) {
if !torque_impulse.is_zero() && self.rb_type == RigidBodyType::Dynamic {
self.rb_vels.angvel += self.rb_mprops.effective_world_inv_inertia_sqrt
* (self.rb_mprops.effective_world_inv_inertia_sqrt * torque_impulse);
if !torque_impulse.is_zero() && self.body_type == RigidBodyType::Dynamic {
self.vels.angvel += self.mprops.effective_world_inv_inertia_sqrt
* (self.mprops.effective_world_inv_inertia_sqrt * torque_impulse);
if wake_up {
self.wake_up(true);
@@ -817,9 +778,9 @@ impl RigidBody {
/// This does nothing on non-dynamic bodies.
#[cfg(feature = "dim3")]
pub fn apply_torque_impulse(&mut self, torque_impulse: Vector<Real>, wake_up: bool) {
if !torque_impulse.is_zero() && self.rb_type == RigidBodyType::Dynamic {
self.rb_vels.angvel += self.rb_mprops.effective_world_inv_inertia_sqrt
* (self.rb_mprops.effective_world_inv_inertia_sqrt * torque_impulse);
if !torque_impulse.is_zero() && self.body_type == RigidBodyType::Dynamic {
self.vels.angvel += self.mprops.effective_world_inv_inertia_sqrt
* (self.mprops.effective_world_inv_inertia_sqrt * torque_impulse);
if wake_up {
self.wake_up(true);
@@ -836,7 +797,7 @@ impl RigidBody {
point: Point<Real>,
wake_up: bool,
) {
let torque_impulse = (point - self.rb_mprops.world_com).gcross(impulse);
let torque_impulse = (point - self.mprops.world_com).gcross(impulse);
self.apply_impulse(impulse, wake_up);
self.apply_torque_impulse(torque_impulse, wake_up);
}
@@ -845,28 +806,27 @@ impl RigidBody {
impl RigidBody {
/// The velocity of the given world-space point on this rigid-body.
pub fn velocity_at_point(&self, point: &Point<Real>) -> Vector<Real> {
self.rb_vels
.velocity_at_point(point, &self.rb_mprops.world_com)
self.vels.velocity_at_point(point, &self.mprops.world_com)
}
/// The kinetic energy of this body.
pub fn kinetic_energy(&self) -> Real {
self.rb_vels.kinetic_energy(&self.rb_mprops)
self.vels.kinetic_energy(&self.mprops)
}
/// The potential energy of this body in a gravity field.
pub fn gravitational_potential_energy(&self, dt: Real, gravity: Vector<Real>) -> Real {
let world_com = self
.rb_mprops
.mprops
.local_mprops
.world_com(&self.rb_pos.position)
.world_com(&self.pos.position)
.coords;
// Project position back along velocity vector one half-step (leap-frog)
// to sync up the potential energy with the kinetic energy:
let world_com = world_com - self.rb_vels.linvel * (dt / 2.0);
let world_com = world_com - self.vels.linvel * (dt / 2.0);
-self.mass() * self.rb_forces.gravity_scale * gravity.dot(&world_com)
-self.mass() * self.forces.gravity_scale * gravity.dot(&world_com)
}
}
@@ -886,7 +846,7 @@ pub struct RigidBodyBuilder {
pub linear_damping: Real,
/// Damping factor for gradually slowing down the angular motion of the rigid-body, `0.0` by default.
pub angular_damping: Real,
rb_type: RigidBodyType,
body_type: RigidBodyType,
mprops_flags: LockedAxes,
/// The additional mass properties of the rigid-body being built. See [`RigidBodyBuilder::additional_mass_properties`] for more information.
pub additional_mass_properties: MassProperties,
@@ -906,7 +866,7 @@ pub struct RigidBodyBuilder {
impl RigidBodyBuilder {
/// Initialize a new builder for a rigid body which is either fixed, dynamic, or kinematic.
pub fn new(rb_type: RigidBodyType) -> Self {
pub fn new(body_type: RigidBodyType) -> Self {
Self {
position: Isometry::identity(),
linvel: Vector::zeros(),
@@ -914,7 +874,7 @@ impl RigidBodyBuilder {
gravity_scale: 1.0,
linear_damping: 0.0,
angular_damping: 0.0,
rb_type,
body_type,
mprops_flags: LockedAxes::empty(),
additional_mass_properties: MassProperties::zero(),
can_sleep: true,
@@ -1191,23 +1151,23 @@ impl RigidBodyBuilder {
/// Build a new rigid-body with the parameters configured with this builder.
pub fn build(&self) -> RigidBody {
let mut rb = RigidBody::new();
rb.rb_pos.next_position = self.position; // FIXME: compute the correct value?
rb.rb_pos.position = self.position;
rb.rb_vels.linvel = self.linvel;
rb.rb_vels.angvel = self.angvel;
rb.rb_type = self.rb_type;
rb.pos.next_position = self.position; // FIXME: compute the correct value?
rb.pos.position = self.position;
rb.vels.linvel = self.linvel;
rb.vels.angvel = self.angvel;
rb.body_type = self.body_type;
rb.user_data = self.user_data;
if self.additional_mass_properties != MassProperties::default() {
rb.rb_mprops.additional_local_mprops = Some(Box::new(self.additional_mass_properties));
rb.rb_mprops.local_mprops = self.additional_mass_properties;
rb.mprops.additional_local_mprops = Some(Box::new(self.additional_mass_properties));
rb.mprops.local_mprops = self.additional_mass_properties;
}
rb.rb_mprops.flags = self.mprops_flags;
rb.rb_damping.linear_damping = self.linear_damping;
rb.rb_damping.angular_damping = self.angular_damping;
rb.rb_forces.gravity_scale = self.gravity_scale;
rb.rb_dominance = RigidBodyDominance(self.dominance_group);
rb.mprops.flags = self.mprops_flags;
rb.damping.linear_damping = self.linear_damping;
rb.damping.angular_damping = self.angular_damping;
rb.forces.gravity_scale = self.gravity_scale;
rb.dominance = RigidBodyDominance(self.dominance_group);
rb.enable_ccd(self.ccd_enabled);
if self.can_sleep && self.sleeping {
@@ -1215,8 +1175,8 @@ impl RigidBodyBuilder {
}
if !self.can_sleep {
rb.rb_activation.linear_threshold = -1.0;
rb.rb_activation.angular_threshold = -1.0;
rb.activation.linear_threshold = -1.0;
rb.activation.angular_threshold = -1.0;
}
rb

35
src/dynamics/rigid_body_components.rs
View File

@@ -1,8 +1,7 @@
use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
use crate::dynamics::MassProperties;
use crate::geometry::{
ColliderChanges, ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition,
ColliderShape,
ColliderSet, ColliderShape,
};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector,
@@ -295,16 +294,12 @@ impl RigidBodyMassProps {
}
/// Recompute the mass-properties of this rigid-bodies based on its currentely attached colliders.
pub fn recompute_mass_properties_from_colliders<Colliders>(
pub fn recompute_mass_properties_from_colliders(
&mut self,
colliders: &Colliders,
colliders: &ColliderSet,
attached_colliders: &RigidBodyColliders,
position: &Isometry<Real>,
) where
Colliders: ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderMassProps>
+ ComponentSet<ColliderShape>,
{
) {
self.local_mprops = self
.additional_local_mprops
.as_ref()
@@ -312,16 +307,16 @@ impl RigidBodyMassProps {
.unwrap_or_else(MassProperties::default);
for handle in &attached_colliders.0 {
let co_parent: Option<&ColliderParent> = colliders.get(handle.0);
if let Some(co_parent) = co_parent {
let (co_mprops, co_shape): (&ColliderMassProps, &ColliderShape) =
colliders.index_bundle(handle.0);
let to_add = co_mprops
.mass_properties(&**co_shape)
if let Some(co) = colliders.get(handle) {
if let Some(co_parent) = co.parent {
let to_add = co
.mprops
.mass_properties(&**co.shape)
.transform_by(&co_parent.pos_wrt_parent);
self.local_mprops += to_add;
}
}
}
self.update_world_mass_properties(position);
}
@@ -892,16 +887,12 @@ impl RigidBodyColliders {
}
/// Update the positions of all the colliders attached to this rigid-body.
pub fn update_positions<Colliders>(
pub fn update_positions(
&self,
colliders: &mut Colliders,
colliders: &mut ColliderSet,
modified_colliders: &mut Vec<ColliderHandle>,
parent_pos: &Isometry<Real>,
) where
Colliders: ComponentSetMut<ColliderPosition>
+ ComponentSetMut<ColliderChanges>
+ ComponentSetOption<ColliderParent>,
{
) {
for handle in &self.0 {
// NOTE: the ColliderParent component must exist if we enter this method.
let co_parent: &ColliderParent = colliders

68
src/dynamics/rigid_body_set.rs
View File

@@ -1,11 +1,6 @@
use crate::data::{Arena, ComponentSet, ComponentSetMut, ComponentSetOption};
use crate::data::Arena;
use crate::dynamics::{
ImpulseJointSet, RigidBody, RigidBodyCcd, RigidBodyChanges, RigidBodyDamping, RigidBodyForces,
RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::dynamics::{
IslandManager, MultibodyJointSet, RigidBodyActivation, RigidBodyColliders, RigidBodyDominance,
RigidBodyHandle, RigidBodyType,
ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBody, RigidBodyChanges, RigidBodyHandle,
};
use crate::geometry::ColliderSet;
use std::ops::{Index, IndexMut};
@@ -39,59 +34,6 @@ pub struct RigidBodySet {
pub(crate) modified_bodies: Vec<RigidBodyHandle>,
}
macro_rules! impl_field_component_set(
($T: ty, $field: ident) => {
impl ComponentSetOption<$T> for RigidBodySet {
fn get(&self, handle: crate::data::Index) -> Option<&$T> {
self.get(RigidBodyHandle(handle)).map(|b| &b.$field)
}
}
impl ComponentSet<$T> for RigidBodySet {
fn size_hint(&self) -> usize {
self.len()
}
#[inline(always)]
fn for_each(&self, mut f: impl FnMut(crate::data::Index, &$T)) {
for (handle, body) in self.bodies.iter() {
f(handle, &body.$field)
}
}
}
impl ComponentSetMut<$T> for RigidBodySet {
fn set_internal(&mut self, handle: crate::data::Index, val: $T) {
if let Some(rb) = self.get_mut_internal(RigidBodyHandle(handle)) {
rb.$field = val;
}
}
#[inline(always)]
fn map_mut_internal<Result>(
&mut self,
handle: crate::data::Index,
f: impl FnOnce(&mut $T) -> Result,
) -> Option<Result> {
self.get_mut_internal(RigidBodyHandle(handle)).map(|rb| f(&mut rb.$field))
}
}
}
);
impl_field_component_set!(RigidBodyPosition, rb_pos);
impl_field_component_set!(RigidBodyMassProps, rb_mprops);
impl_field_component_set!(RigidBodyVelocity, rb_vels);
impl_field_component_set!(RigidBodyDamping, rb_damping);
impl_field_component_set!(RigidBodyForces, rb_forces);
impl_field_component_set!(RigidBodyCcd, rb_ccd);
impl_field_component_set!(RigidBodyIds, rb_ids);
impl_field_component_set!(RigidBodyType, rb_type);
impl_field_component_set!(RigidBodyActivation, rb_activation);
impl_field_component_set!(RigidBodyColliders, rb_colliders);
impl_field_component_set!(RigidBodyDominance, rb_dominance);
impl_field_component_set!(RigidBodyChanges, changes);
impl RigidBodySet {
/// Create a new empty set of rigid bodies.
pub fn new() -> Self {
@@ -147,7 +89,7 @@ impl RigidBodySet {
/*
* Update active sets.
*/
islands.rigid_body_removed(handle, &rb.rb_ids, self);
islands.rigid_body_removed(handle, &rb.ids, self);
/*
* Remove colliders attached to this rigid-body.
@@ -233,6 +175,10 @@ impl RigidBodySet {
self.bodies.get_mut(handle.0)
}
pub(crate) fn index_mut_internal(&mut self, handle: RigidBodyHandle) -> &mut RigidBody {
&mut self.bodies[handle.0]
}
// Just a very long name instead of `.get_mut` to make sure
// this is really the method we wanted to use instead of `get_mut_internal`.
pub(crate) fn get_mut_internal_with_modification_tracking(

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

34
src/geometry/broad_phase_multi_sap/broad_phase.rs
View File

@@ -3,15 +3,14 @@ use super::{
};
use crate::geometry::broad_phase_multi_sap::SAPProxyIndex;
use crate::geometry::{
ColliderBroadPhaseData, ColliderChanges, ColliderHandle, ColliderPosition, ColliderShape,
ColliderBroadPhaseData, ColliderChanges, ColliderHandle, ColliderPosition, ColliderSet,
ColliderShape,
};
use crate::math::Real;
use crate::utils::IndexMut2;
use parry::bounding_volume::BoundingVolume;
use parry::utils::hashmap::HashMap;
use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
/// A broad-phase combining a Hierarchical Grid and Sweep-and-Prune.
///
/// The basic Sweep-and-Prune (SAP) algorithm has one significant flaws:
@@ -435,19 +434,14 @@ impl BroadPhase {
}
/// Updates the broad-phase, taking into account the new collider positions.
pub fn update<Colliders>(
pub fn update(
&mut self,
prediction_distance: Real,
colliders: &mut Colliders,
colliders: &mut ColliderSet,
modified_colliders: &[ColliderHandle],
removed_colliders: &[ColliderHandle],
events: &mut Vec<BroadPhasePairEvent>,
) where
Colliders: ComponentSetMut<ColliderBroadPhaseData>
+ ComponentSet<ColliderChanges>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>,
{
) {
// Phase 1: pre-delete the collisions that have been deleted.
self.handle_removed_colliders(removed_colliders);
@@ -457,30 +451,22 @@ impl BroadPhase {
for handle in modified_colliders {
// NOTE: we use `get` because the collider may no longer
// exist if it has been removed.
let co_changes: Option<&ColliderChanges> = colliders.get(handle.0);
if let Some(co_changes) = co_changes {
let (co_bf_data, co_pos, co_shape): (
&ColliderBroadPhaseData,
&ColliderPosition,
&ColliderShape,
) = colliders.index_bundle(handle.0);
if !co_changes.needs_broad_phase_update() {
if let Some(co) = colliders.get(*handle) {
if !co.changes.needs_broad_phase_update() {
continue;
}
let mut new_proxy_id = co_bf_data.proxy_index;
let mut new_proxy_id = co.bf_data.proxy_index;
if self.handle_modified_collider(
prediction_distance,
*handle,
&mut new_proxy_id,
(co_pos, co_shape, co_changes),
(&co.pos, &co.shape, &co.changes),
) {
need_region_propagation = true;
}
if co_bf_data.proxy_index != new_proxy_id {
if co.bf_data.proxy_index != new_proxy_id {
self.colliders_proxy_ids.insert(*handle, new_proxy_id);
// Make sure we have the new proxy index in case

183
src/geometry/collider.rs
View File

@@ -17,118 +17,118 @@ use parry::shape::Shape;
///
/// To build a new collider, use the `ColliderBuilder` structure.
pub struct Collider {
pub(crate) co_type: ColliderType,
pub(crate) co_shape: ColliderShape,
pub(crate) co_mprops: ColliderMassProps,
pub(crate) co_changes: ColliderChanges,
pub(crate) co_parent: Option<ColliderParent>,
pub(crate) co_pos: ColliderPosition,
pub(crate) co_material: ColliderMaterial,
pub(crate) co_flags: ColliderFlags,
pub(crate) co_bf_data: ColliderBroadPhaseData,
pub(crate) coll_type: ColliderType,
pub(crate) shape: ColliderShape,
pub(crate) mprops: ColliderMassProps,
pub(crate) changes: ColliderChanges,
pub(crate) parent: Option<ColliderParent>,
pub(crate) pos: ColliderPosition,
pub(crate) material: ColliderMaterial,
pub(crate) flags: ColliderFlags,
pub(crate) bf_data: ColliderBroadPhaseData,
/// User-defined data associated to this collider.
pub user_data: u128,
}
impl Collider {
pub(crate) fn reset_internal_references(&mut self) {
self.co_bf_data.proxy_index = crate::INVALID_U32;
self.co_changes = ColliderChanges::all();
self.bf_data.proxy_index = crate::INVALID_U32;
self.changes = ColliderChanges::all();
}
/// The rigid body this collider is attached to.
pub fn parent(&self) -> Option<RigidBodyHandle> {
self.co_parent.map(|parent| parent.handle)
self.parent.map(|parent| parent.handle)
}
/// Is this collider a sensor?
pub fn is_sensor(&self) -> bool {
self.co_type.is_sensor()
self.coll_type.is_sensor()
}
/// The physics hooks enabled for this collider.
pub fn active_hooks(&self) -> ActiveHooks {
self.co_flags.active_hooks
self.flags.active_hooks
}
/// Sets the physics hooks enabled for this collider.
pub fn set_active_hooks(&mut self, active_hooks: ActiveHooks) {
self.co_flags.active_hooks = active_hooks;
self.flags.active_hooks = active_hooks;
}
/// The events enabled for this collider.
pub fn active_events(&self) -> ActiveEvents {
self.co_flags.active_events
self.flags.active_events
}
/// Sets the events enabled for this collider.
pub fn set_active_events(&mut self, active_events: ActiveEvents) {
self.co_flags.active_events = active_events;
self.flags.active_events = active_events;
}
/// The collision types enabled for this collider.
pub fn active_collision_types(&self) -> ActiveCollisionTypes {
self.co_flags.active_collision_types
self.flags.active_collision_types
}
/// Sets the collision types enabled for this collider.
pub fn set_active_collision_types(&mut self, active_collision_types: ActiveCollisionTypes) {
self.co_flags.active_collision_types = active_collision_types;
self.flags.active_collision_types = active_collision_types;
}
/// The friction coefficient of this collider.
pub fn friction(&self) -> Real {
self.co_material.friction
self.material.friction
}
/// Sets the friction coefficient of this collider.
pub fn set_friction(&mut self, coefficient: Real) {
self.co_material.friction = coefficient
self.material.friction = coefficient
}
/// The combine rule used by this collider to combine its friction
/// coefficient with the friction coefficient of the other collider it
/// is in contact with.
pub fn friction_combine_rule(&self) -> CoefficientCombineRule {
self.co_material.friction_combine_rule
self.material.friction_combine_rule
}
/// Sets the combine rule used by this collider to combine its friction
/// coefficient with the friction coefficient of the other collider it
/// is in contact with.
pub fn set_friction_combine_rule(&mut self, rule: CoefficientCombineRule) {
self.co_material.friction_combine_rule = rule;
self.material.friction_combine_rule = rule;
}
/// The restitution coefficient of this collider.
pub fn restitution(&self) -> Real {
self.co_material.restitution
self.material.restitution
}
/// Sets the restitution coefficient of this collider.
pub fn set_restitution(&mut self, coefficient: Real) {
self.co_material.restitution = coefficient
self.material.restitution = coefficient
}
/// The combine rule used by this collider to combine its restitution
/// coefficient with the restitution coefficient of the other collider it
/// is in contact with.
pub fn restitution_combine_rule(&self) -> CoefficientCombineRule {
self.co_material.restitution_combine_rule
self.material.restitution_combine_rule
}
/// Sets the combine rule used by this collider to combine its restitution
/// coefficient with the restitution coefficient of the other collider it
/// is in contact with.
pub fn set_restitution_combine_rule(&mut self, rule: CoefficientCombineRule) {
self.co_material.restitution_combine_rule = rule;
self.material.restitution_combine_rule = rule;
}
/// Sets whether or not this is a sensor collider.
pub fn set_sensor(&mut self, is_sensor: bool) {
if is_sensor != self.is_sensor() {
self.co_changes.insert(ColliderChanges::TYPE);
self.co_type = if is_sensor {
self.changes.insert(ColliderChanges::TYPE);
self.coll_type = if is_sensor {
ColliderType::Sensor
} else {
ColliderType::Solid
@@ -138,55 +138,55 @@ impl Collider {
/// Sets the translational part of this collider's position.
pub fn set_translation(&mut self, translation: Vector<Real>) {
self.co_changes.insert(ColliderChanges::POSITION);
self.co_pos.0.translation.vector = translation;
self.changes.insert(ColliderChanges::POSITION);
self.pos.0.translation.vector = translation;
}
/// Sets the rotational part of this collider's position.
pub fn set_rotation(&mut self, rotation: AngVector<Real>) {
self.co_changes.insert(ColliderChanges::POSITION);
self.co_pos.0.rotation = Rotation::new(rotation);
self.changes.insert(ColliderChanges::POSITION);
self.pos.0.rotation = Rotation::new(rotation);
}
/// Sets the position of this collider.
pub fn set_position(&mut self, position: Isometry<Real>) {
self.co_changes.insert(ColliderChanges::POSITION);
self.co_pos.0 = position;
self.changes.insert(ColliderChanges::POSITION);
self.pos.0 = position;
}
/// The world-space position of this collider.
pub fn position(&self) -> &Isometry<Real> {
&self.co_pos
&self.pos
}
/// The translational part of this collider's position.
pub fn translation(&self) -> &Vector<Real> {
&self.co_pos.0.translation.vector
&self.pos.0.translation.vector
}
/// The rotational part of this collider's position.
pub fn rotation(&self) -> &Rotation<Real> {
&self.co_pos.0.rotation
&self.pos.0.rotation
}
/// The position of this collider wrt the body it is attached to.
pub fn position_wrt_parent(&self) -> Option<&Isometry<Real>> {
self.co_parent.as_ref().map(|p| &p.pos_wrt_parent)
self.parent.as_ref().map(|p| &p.pos_wrt_parent)
}
/// Sets the translational part of this collider's translation relative to its parent rigid-body.
pub fn set_translation_wrt_parent(&mut self, translation: Vector<Real>) {
if let Some(co_parent) = self.co_parent.as_mut() {
self.co_changes.insert(ColliderChanges::PARENT);
co_parent.pos_wrt_parent.translation.vector = translation;
if let Some(parent) = self.parent.as_mut() {
self.changes.insert(ColliderChanges::PARENT);
parent.pos_wrt_parent.translation.vector = translation;
}
}
/// Sets the rotational part of this collider's rotaiton relative to its parent rigid-body.
pub fn set_rotation_wrt_parent(&mut self, rotation: AngVector<Real>) {
if let Some(co_parent) = self.co_parent.as_mut() {
self.co_changes.insert(ColliderChanges::PARENT);
co_parent.pos_wrt_parent.rotation = Rotation::new(rotation);
if let Some(parent) = self.parent.as_mut() {
self.changes.insert(ColliderChanges::PARENT);
parent.pos_wrt_parent.rotation = Rotation::new(rotation);
}
}
@@ -194,46 +194,46 @@ impl Collider {
///
/// Does nothing if the collider is not attached to a rigid-body.
pub fn set_position_wrt_parent(&mut self, pos_wrt_parent: Isometry<Real>) {
if let Some(co_parent) = self.co_parent.as_mut() {
self.co_changes.insert(ColliderChanges::PARENT);
co_parent.pos_wrt_parent = pos_wrt_parent;
if let Some(parent) = self.parent.as_mut() {
self.changes.insert(ColliderChanges::PARENT);
parent.pos_wrt_parent = pos_wrt_parent;
}
}
/// The collision groups used by this collider.
pub fn collision_groups(&self) -> InteractionGroups {
self.co_flags.collision_groups
self.flags.collision_groups
}
/// Sets the collision groups of this collider.
pub fn set_collision_groups(&mut self, groups: InteractionGroups) {
if self.co_flags.collision_groups != groups {
self.co_changes.insert(ColliderChanges::GROUPS);
self.co_flags.collision_groups = groups;
if self.flags.collision_groups != groups {
self.changes.insert(ColliderChanges::GROUPS);
self.flags.collision_groups = groups;
}
}
/// The solver groups used by this collider.
pub fn solver_groups(&self) -> InteractionGroups {
self.co_flags.solver_groups
self.flags.solver_groups
}
/// Sets the solver groups of this collider.
pub fn set_solver_groups(&mut self, groups: InteractionGroups) {
if self.co_flags.solver_groups != groups {
self.co_changes.insert(ColliderChanges::GROUPS);
self.co_flags.solver_groups = groups;
if self.flags.solver_groups != groups {
self.changes.insert(ColliderChanges::GROUPS);
self.flags.solver_groups = groups;
}
}
/// The material (friction and restitution properties) of this collider.
pub fn material(&self) -> &ColliderMaterial {
&self.co_material
&self.material
}
/// The density of this collider, if set.
pub fn density(&self) -> Option<Real> {
match &self.co_mprops {
match &self.mprops {
ColliderMassProps::Density(density) => Some(*density),
ColliderMassProps::MassProperties(_) => None,
}
@@ -241,7 +241,7 @@ impl Collider {
/// The geometric shape of this collider.
pub fn shape(&self) -> &dyn Shape {
self.co_shape.as_ref()
self.shape.as_ref()
}
/// A mutable reference to the geometric shape of this collider.
@@ -250,37 +250,36 @@ impl Collider {
/// cloned first so that `self` contains a unique copy of that
/// shape that you can modify.
pub fn shape_mut(&mut self) -> &mut dyn Shape {
self.co_changes.insert(ColliderChanges::SHAPE);
self.co_shape.make_mut()
self.changes.insert(ColliderChanges::SHAPE);
self.shape.make_mut()
}
/// Sets the shape of this collider.
pub fn set_shape(&mut self, shape: SharedShape) {
self.co_changes.insert(ColliderChanges::SHAPE);
self.co_shape = shape;
self.changes.insert(ColliderChanges::SHAPE);
self.shape = shape;
}
/// Retrieve the SharedShape. Also see the `shape()` function
pub fn shared_shape(&self) -> &SharedShape {
&self.co_shape
&self.shape
}
/// Compute the axis-aligned bounding box of this collider.
pub fn compute_aabb(&self) -> AABB {
self.co_shape.compute_aabb(&self.co_pos)
self.shape.compute_aabb(&self.pos)
}
/// Compute the axis-aligned bounding box of this collider moving from its current position
/// to the given `next_position`
pub fn compute_swept_aabb(&self, next_position: &Isometry<Real>) -> AABB {
self.co_shape
.compute_swept_aabb(&self.co_pos, next_position)
self.shape.compute_swept_aabb(&self.pos, next_position)
}
/// Compute the local-space mass properties of this collider.
pub fn mass_properties(&self) -> MassProperties {
match &self.co_mprops {
ColliderMassProps::Density(density) => self.co_shape.mass_properties(*density),
match &self.mprops {
ColliderMassProps::Density(density) => self.shape.mass_properties(*density),
ColliderMassProps::MassProperties(mass_properties) => **mass_properties,
}
}
@@ -726,18 +725,17 @@ impl ColliderBuilder {
/// Builds a new collider attached to the given rigid-body.
pub fn build(&self) -> Collider {
let (co_changes, co_pos, co_bf_data, co_shape, co_type, co_material, co_flags, co_mprops) =
self.components();
let (changes, pos, bf_data, shape, coll_type, material, flags, mprops) = self.components();
Collider {
co_shape,
co_mprops,
co_material,
co_parent: None,
co_changes,
co_pos,
co_bf_data,
co_flags,
co_type,
shape,
mprops,
material,
parent: None,
changes,
pos,
bf_data,
flags,
coll_type,
user_data: self.user_data,
}
}
@@ -763,39 +761,32 @@ impl ColliderBuilder {
ColliderMassProps::Density(density)
};
let co_shape = self.shape.clone();
let co_mprops = mass_info;
let co_material = ColliderMaterial {
let shape = self.shape.clone();
let mprops = mass_info;
let material = ColliderMaterial {
friction: self.friction,
restitution: self.restitution,
friction_combine_rule: self.friction_combine_rule,
restitution_combine_rule: self.restitution_combine_rule,
};
let co_flags = ColliderFlags {
let flags = ColliderFlags {
collision_groups: self.collision_groups,
solver_groups: self.solver_groups,
active_collision_types: self.active_collision_types,
active_hooks: self.active_hooks,
active_events: self.active_events,
};
let co_changes = ColliderChanges::all();
let co_pos = ColliderPosition(self.position);
let co_bf_data = ColliderBroadPhaseData::default();
let co_type = if self.is_sensor {
let changes = ColliderChanges::all();
let pos = ColliderPosition(self.position);
let bf_data = ColliderBroadPhaseData::default();
let coll_type = if self.is_sensor {
ColliderType::Sensor
} else {
ColliderType::Solid
};
(
co_changes,
co_pos,
co_bf_data,
co_shape,
co_type,
co_material,
co_flags,
co_mprops,
changes, pos, bf_data, shape, coll_type, material, flags, mprops,
)
}
}

64
src/geometry/collider_set.rs
View File

@@ -1,11 +1,6 @@
use crate::data::arena::Arena;
use crate::data::{ComponentSet, ComponentSetMut, ComponentSetOption};
use crate::dynamics::{IslandManager, RigidBodyHandle, RigidBodySet};
use crate::geometry::{
Collider, ColliderBroadPhaseData, ColliderFlags, ColliderMassProps, ColliderMaterial,
ColliderParent, ColliderPosition, ColliderShape, ColliderType,
};
use crate::geometry::{ColliderChanges, ColliderHandle};
use crate::geometry::{Collider, ColliderChanges, ColliderHandle, ColliderParent};
use crate::math::Isometry;
use std::ops::{Index, IndexMut};
@@ -18,63 +13,6 @@ pub struct ColliderSet {
pub(crate) removed_colliders: Vec<ColliderHandle>,
}
macro_rules! impl_field_component_set(
($T: ty, $field: ident) => {
impl ComponentSetOption<$T> for ColliderSet {
fn get(&self, handle: crate::data::Index) -> Option<&$T> {
self.get(ColliderHandle(handle)).map(|b| &b.$field)
}
}
impl ComponentSet<$T> for ColliderSet {
fn size_hint(&self) -> usize {
self.len()
}
#[inline(always)]
fn for_each(&self, mut f: impl FnMut(crate::data::Index, &$T)) {
for (handle, body) in self.colliders.iter() {
f(handle, &body.$field)
}
}
}
impl ComponentSetMut<$T> for ColliderSet {
fn set_internal(&mut self, handle: crate::data::Index, val: $T) {
if let Some(rb) = self.get_mut_internal(ColliderHandle(handle)) {
rb.$field = val;
}
}
#[inline(always)]
fn map_mut_internal<Result>(
&mut self,
handle: crate::data::Index,
f: impl FnOnce(&mut $T) -> Result,
) -> Option<Result> {
self.get_mut_internal(ColliderHandle(handle)).map(|rb| f(&mut rb.$field))
}
}
}
);
impl_field_component_set!(ColliderType, co_type);
impl_field_component_set!(ColliderShape, co_shape);
impl_field_component_set!(ColliderMassProps, co_mprops);
impl_field_component_set!(ColliderChanges, co_changes);
impl_field_component_set!(ColliderPosition, co_pos);
impl_field_component_set!(ColliderMaterial, co_material);
impl_field_component_set!(ColliderFlags, co_flags);
impl_field_component_set!(ColliderBroadPhaseData, co_bf_data);
impl ComponentSetOption<ColliderParent> for ColliderSet {
#[inline(always)]
fn get(&self, handle: crate::data::Index) -> Option<&ColliderParent> {
self.get(ColliderHandle(handle))
.and_then(|b| b.co_parent.as_ref())
}
}
impl ColliderSet {
/// Create a new empty set of colliders.
pub fn new() -> Self {

142
src/geometry/narrow_phase.rs
View File

@@ -1,16 +1,16 @@
#[cfg(feature = "parallel")]
use rayon::prelude::*;
use crate::data::{BundleSet, Coarena, ComponentSet, ComponentSetMut, ComponentSetOption};
use crate::dynamics::CoefficientCombineRule;
use crate::data::Coarena;
use crate::dynamics::{
IslandManager, RigidBodyActivation, RigidBodyDominance, RigidBodyIds, RigidBodyType,
CoefficientCombineRule, IslandManager, RigidBodyActivation, RigidBodyDominance, RigidBodyIds,
RigidBodySet, RigidBodyType,
};
use crate::geometry::{
BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderMaterial,
ColliderPair, ColliderParent, ColliderPosition, ColliderShape, ColliderType, CollisionEvent,
ContactData, ContactManifold, ContactManifoldData, ContactPair, InteractionGraph,
IntersectionPair, SolverContact, SolverFlags,
ColliderPair, ColliderParent, ColliderPosition, ColliderSet, ColliderShape, ColliderType,
CollisionEvent, ContactData, ContactManifold, ContactManifoldData, ContactPair,
InteractionGraph, IntersectionPair, SolverContact, SolverFlags,
};
use crate::math::{Real, Vector};
use crate::pipeline::{
@@ -250,23 +250,15 @@ impl NarrowPhase {
// }
/// Maintain the narrow-phase internal state by taking collider removal into account.
pub fn handle_user_changes<Bodies, Colliders>(
pub fn handle_user_changes(
&mut self,
mut islands: Option<&mut IslandManager>,
modified_colliders: &[ColliderHandle],
removed_colliders: &[ColliderHandle],
colliders: &mut Colliders,
bodies: &mut Bodies,
colliders: &mut ColliderSet,
bodies: &mut RigidBodySet,
events: &dyn EventHandler,
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
Colliders: ComponentSet<ColliderChanges>
+ ComponentSet<ColliderType>
+ ComponentSet<ColliderFlags>
+ ComponentSetOption<ColliderParent>,
{
) {
// TODO: avoid these hash-maps.
// They are necessary to handle the swap-remove done internally
// by the contact/intersection graphs when a node is removed.
@@ -305,22 +297,17 @@ impl NarrowPhase {
self.handle_modified_colliders(islands, modified_colliders, colliders, bodies, events);
}
pub(crate) fn remove_collider<Bodies, Colliders>(
pub(crate) fn remove_collider(
&mut self,
intersection_graph_id: ColliderGraphIndex,
contact_graph_id: ColliderGraphIndex,
mut islands: Option<&mut IslandManager>,
colliders: &mut Colliders,
bodies: &mut Bodies,
colliders: &mut ColliderSet,
bodies: &mut RigidBodySet,
prox_id_remap: &mut HashMap<ColliderHandle, ColliderGraphIndex>,
contact_id_remap: &mut HashMap<ColliderHandle, ColliderGraphIndex>,
events: &dyn EventHandler,
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
Colliders: ComponentSetOption<ColliderParent>,
{
) {
// Wake up every body in contact with the deleted collider and generate Stopped collision events.
if let Some(islands) = islands.as_deref_mut() {
for (a, b, pair) in self.contact_graph.interactions_with(contact_graph_id) {
@@ -379,22 +366,14 @@ impl NarrowPhase {
}
}
pub(crate) fn handle_modified_colliders<Bodies, Colliders>(
pub(crate) fn handle_modified_colliders(
&mut self,
mut islands: Option<&mut IslandManager>,
modified_colliders: &[ColliderHandle],
colliders: &Colliders,
bodies: &mut Bodies,
colliders: &ColliderSet,
bodies: &mut RigidBodySet,
events: &dyn EventHandler,
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
Colliders: ComponentSet<ColliderChanges>
+ ComponentSet<ColliderType>
+ ComponentSet<ColliderFlags>
+ ComponentSetOption<ColliderParent>,
{
) {
let mut pairs_to_remove = vec![];
for handle in modified_colliders {
@@ -496,22 +475,15 @@ impl NarrowPhase {
}
}
fn remove_pair<Bodies, Colliders>(
fn remove_pair(
&mut self,
islands: Option<&mut IslandManager>,
colliders: &Colliders,
bodies: &mut Bodies,
colliders: &ColliderSet,
bodies: &mut RigidBodySet,
pair: &ColliderPair,
events: &dyn EventHandler,
mode: PairRemovalMode,
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
Colliders: ComponentSet<ColliderType>
+ ComponentSet<ColliderFlags>
+ ComponentSetOption<ColliderParent>,
{
) {
let co_type1: Option<&ColliderType> = colliders.get(pair.collider1.0);
let co_type2: Option<&ColliderType> = colliders.get(pair.collider2.0);
@@ -582,10 +554,7 @@ impl NarrowPhase {
}
}
fn add_pair<Colliders>(&mut self, colliders: &Colliders, pair: &ColliderPair)
where
Colliders: ComponentSet<ColliderType> + ComponentSetOption<ColliderParent>,
{
fn add_pair(&mut self, colliders: &ColliderSet, pair: &ColliderPair) {
let co_type1: Option<&ColliderType> = colliders.get(pair.collider1.0);
let co_type2: Option<&ColliderType> = colliders.get(pair.collider2.0);
@@ -666,21 +635,14 @@ impl NarrowPhase {
}
}
pub(crate) fn register_pairs<Bodies, Colliders>(
pub(crate) fn register_pairs(
&mut self,
mut islands: Option<&mut IslandManager>,
colliders: &Colliders,
bodies: &mut Bodies,
colliders: &ColliderSet,
bodies: &mut RigidBodySet,
broad_phase_events: &[BroadPhasePairEvent],
events: &dyn EventHandler,
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSetMut<RigidBodyIds>
+ ComponentSet<RigidBodyType>,
Colliders: ComponentSet<ColliderType>
+ ComponentSet<ColliderFlags>
+ ComponentSetOption<ColliderParent>,
{
) {
for event in broad_phase_events {
match event {
BroadPhasePairEvent::AddPair(pair) => {
@@ -700,24 +662,14 @@ impl NarrowPhase {
}
}
pub(crate) fn compute_intersections<Bodies, Colliders>(
pub(crate) fn compute_intersections(
&mut self,
bodies: &Bodies,
colliders: &Colliders,
bodies: &RigidBodySet,
colliders: &ColliderSet,
modified_colliders: &[ColliderHandle],
hooks: &dyn PhysicsHooks<Bodies, Colliders>,
hooks: &dyn PhysicsHooks,
events: &dyn EventHandler,
) where
Bodies: ComponentSet<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyDominance>,
Colliders: ComponentSet<ColliderChanges>
+ ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderShape>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderMaterial>
+ ComponentSet<ColliderFlags>,
{
) {
if modified_colliders.is_empty() {
return;
}
@@ -824,25 +776,15 @@ impl NarrowPhase {
});
}
pub(crate) fn compute_contacts<Bodies, Colliders>(
pub(crate) fn compute_contacts(
&mut self,
prediction_distance: Real,
bodies: &Bodies,
colliders: &Colliders,
bodies: &RigidBodySet,
colliders: &ColliderSet,
modified_colliders: &[ColliderHandle],
hooks: &dyn PhysicsHooks<Bodies, Colliders>,
hooks: &dyn PhysicsHooks,
events: &dyn EventHandler,
) where
Bodies: ComponentSet<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyDominance>,
Colliders: ComponentSet<ColliderChanges>
+ ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderShape>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderMaterial>
+ ComponentSet<ColliderFlags>,
{
) {
if modified_colliders.is_empty() {
return;
}
@@ -1057,17 +999,13 @@ impl NarrowPhase {
/// Retrieve all the interactions with at least one contact point, happening between two active bodies.
// NOTE: this is very similar to the code from ImpulseJointSet::select_active_interactions.
pub(crate) fn select_active_contacts<'a, Bodies>(
pub(crate) fn select_active_contacts<'a>(
&'a mut self,
islands: &IslandManager,
bodies: &Bodies,
bodies: &RigidBodySet,
out_manifolds: &mut Vec<&'a mut ContactManifold>,
out: &mut Vec<Vec<ContactManifoldIndex>>,
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyActivation>,
{
) {
for out_island in &mut out[..islands.num_islands()] {
out_island.clear();
}

66
src/pipeline/collision_pipeline.rs
View File

@@ -1,14 +1,8 @@
//! Physics pipeline structures.
use crate::data::{ComponentSet, ComponentSetMut, ComponentSetOption};
use crate::dynamics::{
RigidBodyActivation, RigidBodyChanges, RigidBodyColliders, RigidBodyDominance, RigidBodyHandle,
RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
};
use crate::dynamics::RigidBodyHandle;
use crate::geometry::{
BroadPhase, BroadPhasePairEvent, ColliderBroadPhaseData, ColliderChanges, ColliderFlags,
ColliderHandle, ColliderMassProps, ColliderMaterial, ColliderPair, ColliderParent,
ColliderPosition, ColliderShape, ColliderType, NarrowPhase,
BroadPhase, BroadPhasePairEvent, ColliderChanges, ColliderHandle, ColliderPair, NarrowPhase,
};
use crate::math::Real;
use crate::pipeline::{EventHandler, PhysicsHooks};
@@ -48,32 +42,19 @@ impl CollisionPipeline {
}
}
fn detect_collisions<Bodies, Colliders>(
fn detect_collisions(
&mut self,
prediction_distance: Real,
broad_phase: &mut BroadPhase,
narrow_phase: &mut NarrowPhase,
bodies: &mut Bodies,
colliders: &mut Colliders,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
modified_colliders: &[ColliderHandle],
removed_colliders: &[ColliderHandle],
hooks: &dyn PhysicsHooks<Bodies, Colliders>,
hooks: &dyn PhysicsHooks,
events: &dyn EventHandler,
handle_user_changes: bool,
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>
+ ComponentSet<RigidBodyDominance>,
Colliders: ComponentSetMut<ColliderBroadPhaseData>
+ ComponentSet<ColliderChanges>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>
+ ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderType>
+ ComponentSet<ColliderMaterial>
+ ComponentSet<ColliderFlags>,
{
) {
// Update broad-phase.
self.broad_phase_events.clear();
self.broadphase_collider_pairs.clear();
@@ -112,7 +93,7 @@ impl CollisionPipeline {
fn clear_modified_colliders(
&mut self,
colliders: &mut impl ComponentSetMut<ColliderChanges>,
colliders: &mut ColliderSet,
modified_colliders: &mut Vec<ColliderHandle>,
) {
for handle in modified_colliders.drain(..) {
@@ -129,7 +110,7 @@ impl CollisionPipeline {
narrow_phase: &mut NarrowPhase,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
hooks: &dyn PhysicsHooks<RigidBodySet, ColliderSet>,
hooks: &dyn PhysicsHooks,
events: &dyn EventHandler,
) {
let mut modified_bodies = bodies.take_modified();
@@ -151,38 +132,19 @@ impl CollisionPipeline {
}
/// Executes one step of the collision detection.
pub fn step_generic<Bodies, Colliders>(
pub fn step_generic(
&mut self,
prediction_distance: Real,
broad_phase: &mut BroadPhase,
narrow_phase: &mut NarrowPhase,
bodies: &mut Bodies,
colliders: &mut Colliders,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
modified_bodies: &mut Vec<RigidBodyHandle>,
modified_colliders: &mut Vec<ColliderHandle>,
removed_colliders: &mut Vec<ColliderHandle>,
hooks: &dyn PhysicsHooks<Bodies, Colliders>,
hooks: &dyn PhysicsHooks,
events: &dyn EventHandler,
) where
Bodies: ComponentSetMut<RigidBodyPosition>
+ ComponentSetMut<RigidBodyVelocity>
+ ComponentSetMut<RigidBodyIds>
+ ComponentSetMut<RigidBodyActivation>
+ ComponentSetMut<RigidBodyChanges>
+ ComponentSetMut<RigidBodyMassProps>
+ ComponentSet<RigidBodyColliders>
+ ComponentSet<RigidBodyDominance>
+ ComponentSet<RigidBodyType>,
Colliders: ComponentSetMut<ColliderBroadPhaseData>
+ ComponentSetMut<ColliderChanges>
+ ComponentSetMut<ColliderPosition>
+ ComponentSet<ColliderShape>
+ ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderType>
+ ComponentSet<ColliderMaterial>
+ ComponentSet<ColliderFlags>
+ ComponentSet<ColliderMassProps>,
{
) {
super::user_changes::handle_user_changes_to_colliders(
bodies,
colliders,

53
src/pipeline/physics_hooks.rs
View File

@@ -1,14 +1,14 @@
use crate::dynamics::RigidBodyHandle;
use crate::geometry::{ColliderHandle, ContactManifold, SolverContact, SolverFlags};
use crate::dynamics::{RigidBodyHandle, RigidBodySet};
use crate::geometry::{ColliderHandle, ColliderSet, ContactManifold, SolverContact, SolverFlags};
use crate::math::{Real, Vector};
use na::ComplexField;
/// Context given to custom collision filters to filter-out collisions.
pub struct PairFilterContext<'a, Bodies, Colliders> {
pub struct PairFilterContext<'a> {
/// The set of rigid-bodies.
pub bodies: &'a Bodies,
pub bodies: &'a RigidBodySet,
/// The set of colliders.
pub colliders: &'a Colliders,
pub colliders: &'a ColliderSet,
/// The handle of the first collider involved in the potential collision.
pub collider1: ColliderHandle,
/// The handle of the first collider involved in the potential collision.
@@ -20,11 +20,11 @@ pub struct PairFilterContext<'a, Bodies, Colliders> {
}
/// Context given to custom contact modifiers to modify the contacts seen by the constraints solver.
pub struct ContactModificationContext<'a, Bodies, Colliders> {
pub struct ContactModificationContext<'a> {
/// The set of rigid-bodies.
pub bodies: &'a Bodies,
pub bodies: &'a RigidBodySet,
/// The set of colliders.
pub colliders: &'a Colliders,
pub colliders: &'a ColliderSet,
/// The handle of the first collider involved in the potential collision.
pub collider1: ColliderHandle,
/// The handle of the first collider involved in the potential collision.
@@ -45,7 +45,7 @@ pub struct ContactModificationContext<'a, Bodies, Colliders> {
pub user_data: &'a mut u32,
}
impl<'a, Bodies, Colliders> ContactModificationContext<'a, Bodies, Colliders> {
impl<'a> ContactModificationContext<'a> {
/// Helper function to update `self` to emulate a oneway-platform.
///
/// The "oneway" behavior will only allow contacts between two colliders
@@ -139,28 +139,24 @@ impl Default for ActiveHooks {
// not having Send+Sync isn't a problem.
/// User-defined functions called by the physics engines during one timestep in order to customize its behavior.
#[cfg(target_arch = "wasm32")]
pub trait PhysicsHooks<Bodies, Colliders> {
pub trait PhysicsHooks {
/// Applies the contact pair filter.
fn filter_contact_pair(
&self,
_context: &PairFilterContext<Bodies, Colliders>,
) -> Option<SolverFlags> {
fn filter_contact_pair(&self, _context: &PairFilterContext) -> Option<SolverFlags> {
None
}
/// Applies the intersection pair filter.
fn filter_intersection_pair(&self, _context: &PairFilterContext<Bodies, Colliders>) -> bool {
fn filter_intersection_pair(&self, _context: &PairFilterContext) -> bool {
false
}
/// Modifies the set of contacts seen by the constraints solver.
fn modify_solver_contacts(&self, _context: &mut ContactModificationContext<Bodies, Colliders>) {
}
fn modify_solver_contacts(&self, _context: &mut ContactModificationContext) {}
}
/// User-defined functions called by the physics engines during one timestep in order to customize its behavior.
#[cfg(not(target_arch = "wasm32"))]
pub trait PhysicsHooks<Bodies, Colliders>: Send + Sync {
pub trait PhysicsHooks: Send + Sync {
/// Applies the contact pair filter.
///
/// Note that this method will only be called if at least one of the colliders
@@ -185,10 +181,7 @@ pub trait PhysicsHooks<Bodies, Colliders>: Send + Sync {
/// will be taken into account by the constraints solver. If this returns
/// `Some(SolverFlags::empty())` then the constraints solver will ignore these
/// contacts.
fn filter_contact_pair(
&self,
_context: &PairFilterContext<Bodies, Colliders>,
) -> Option<SolverFlags> {
fn filter_contact_pair(&self, _context: &PairFilterContext) -> Option<SolverFlags> {
Some(SolverFlags::COMPUTE_IMPULSES)
}
@@ -212,7 +205,7 @@ pub trait PhysicsHooks<Bodies, Colliders>: Send + Sync {
/// not compute any intersection information for it.
/// If this return `true` then the narrow-phase will compute intersection
/// information for this pair.
fn filter_intersection_pair(&self, _context: &PairFilterContext<Bodies, Colliders>) -> bool {
fn filter_intersection_pair(&self, _context: &PairFilterContext) -> bool {
true
}
@@ -241,21 +234,17 @@ pub trait PhysicsHooks<Bodies, Colliders>: Send + Sync {
/// as 0 and can be modified in `context.user_data`.
///
/// The world-space contact normal can be modified in `context.normal`.
fn modify_solver_contacts(&self, _context: &mut ContactModificationContext<Bodies, Colliders>) {
}
fn modify_solver_contacts(&self, _context: &mut ContactModificationContext) {}
}
impl<Bodies, Colliders> PhysicsHooks<Bodies, Colliders> for () {
fn filter_contact_pair(
&self,
_context: &PairFilterContext<Bodies, Colliders>,
) -> Option<SolverFlags> {
impl PhysicsHooks for () {
fn filter_contact_pair(&self, _context: &PairFilterContext) -> Option<SolverFlags> {
Some(SolverFlags::default())
}
fn filter_intersection_pair(&self, _: &PairFilterContext<Bodies, Colliders>) -> bool {
fn filter_intersection_pair(&self, _: &PairFilterContext) -> bool {
true
}
fn modify_solver_contacts(&self, _: &mut ContactModificationContext<Bodies, Colliders>) {}
fn modify_solver_contacts(&self, _: &mut ContactModificationContext) {}
}

139
src/pipeline/physics_pipeline.rs
View File

@@ -1,21 +1,18 @@
//! Physics pipeline structures.
use crate::counters::Counters;
use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
#[cfg(not(feature = "parallel"))]
use crate::dynamics::IslandSolver;
use crate::dynamics::{
CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet,
RigidBodyActivation, RigidBodyCcd, RigidBodyChanges, RigidBodyColliders, RigidBodyDamping,
RigidBodyDominance, RigidBodyForces, RigidBodyHandle, RigidBodyIds, RigidBodyMassProps,
RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
RigidBodyColliders, RigidBodyForces, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition,
RigidBodyType, RigidBodyVelocity,
};
#[cfg(feature = "parallel")]
use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
use crate::geometry::{
BroadPhase, BroadPhasePairEvent, ColliderBroadPhaseData, ColliderChanges, ColliderFlags,
ColliderHandle, ColliderMaterial, ColliderPair, ColliderParent, ColliderPosition,
ColliderShape, ColliderType, ContactManifoldIndex, NarrowPhase, ColliderMassProps
BroadPhase, BroadPhasePairEvent, ColliderChanges, ColliderHandle, ColliderPair,
ContactManifoldIndex, NarrowPhase,
};
use crate::math::{Real, Vector};
use crate::pipeline::{EventHandler, PhysicsHooks};
@@ -71,7 +68,7 @@ impl PhysicsPipeline {
fn clear_modified_colliders(
&mut self,
colliders: &mut impl ComponentSetMut<ColliderChanges>,
colliders: &mut ColliderSet,
modified_colliders: &mut Vec<ColliderHandle>,
) {
for handle in modified_colliders.drain(..) {
@@ -79,33 +76,20 @@ impl PhysicsPipeline {
}
}
fn detect_collisions<Bodies, Colliders>(
fn detect_collisions(
&mut self,
integration_parameters: &IntegrationParameters,
islands: &mut IslandManager,
broad_phase: &mut BroadPhase,
narrow_phase: &mut NarrowPhase,
bodies: &mut Bodies,
colliders: &mut Colliders,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
modified_colliders: &[ColliderHandle],
removed_colliders: &[ColliderHandle],
hooks: &dyn PhysicsHooks<Bodies, Colliders>,
hooks: &dyn PhysicsHooks,
events: &dyn EventHandler,
handle_user_changes: bool,
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>
+ ComponentSet<RigidBodyDominance>,
Colliders: ComponentSetMut<ColliderBroadPhaseData>
+ ComponentSet<ColliderChanges>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>
+ ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderType>
+ ComponentSet<ColliderMaterial>
+ ComponentSet<ColliderFlags>,
{
) {
self.counters.stages.collision_detection_time.resume();
self.counters.cd.broad_phase_time.resume();
@@ -155,28 +139,17 @@ impl PhysicsPipeline {
self.counters.stages.collision_detection_time.pause();
}
fn build_islands_and_solve_velocity_constraints<Bodies, Colliders>(
fn build_islands_and_solve_velocity_constraints(
&mut self,
gravity: &Vector<Real>,
integration_parameters: &IntegrationParameters,
islands: &mut IslandManager,
narrow_phase: &mut NarrowPhase,
bodies: &mut Bodies,
colliders: &mut Colliders,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
) where
Bodies: ComponentSetMut<RigidBodyPosition>
+ ComponentSetMut<RigidBodyVelocity>
+ ComponentSetMut<RigidBodyMassProps>
+ ComponentSetMut<RigidBodyForces>
+ ComponentSetMut<RigidBodyIds>
+ ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyDamping>
+ ComponentSet<RigidBodyColliders>
+ ComponentSet<RigidBodyType>,
Colliders: ComponentSetOption<ColliderParent>,
{
) {
self.counters.stages.island_construction_time.resume();
islands.update_active_set_with_contacts(
integration_parameters.dt,
@@ -285,7 +258,7 @@ impl PhysicsPipeline {
.par_iter_mut()
.enumerate()
.for_each(|(island_id, solver)| {
let bodies: &mut Bodies =
let bodies: &mut RigidBodySet =
unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
let manifolds: &mut Vec<&mut ContactManifold> =
unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) };
@@ -313,28 +286,16 @@ impl PhysicsPipeline {
self.counters.stages.solver_time.pause();
}
fn run_ccd_motion_clamping<Bodies, Colliders>(
fn run_ccd_motion_clamping(
&mut self,
integration_parameters: &IntegrationParameters,
islands: &IslandManager,
bodies: &mut Bodies,
colliders: &mut Colliders,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
narrow_phase: &NarrowPhase,
ccd_solver: &mut CCDSolver,
events: &dyn EventHandler,
) where
Bodies: ComponentSetMut<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyCcd>
+ ComponentSet<RigidBodyColliders>
+ ComponentSet<RigidBodyForces>
+ ComponentSet<RigidBodyMassProps>,
Colliders: ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>
+ ComponentSet<ColliderType>
+ ComponentSet<ColliderFlags>,
{
) {
self.counters.ccd.toi_computation_time.start();
// Handle CCD
let impacts = ccd_solver.predict_impacts_at_next_positions(
@@ -349,22 +310,13 @@ impl PhysicsPipeline {
self.counters.ccd.toi_computation_time.pause();
}
fn advance_to_final_positions<Bodies, Colliders>(
fn advance_to_final_positions(
&mut self,
islands: &IslandManager,
bodies: &mut Bodies,
colliders: &mut Colliders,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
modified_colliders: &mut Vec<ColliderHandle>,
) where
Bodies: ComponentSetMut<RigidBodyVelocity>
+ ComponentSetMut<RigidBodyForces>
+ ComponentSetMut<RigidBodyPosition>
+ ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyColliders>,
Colliders: ComponentSetMut<ColliderPosition>
+ ComponentSetMut<ColliderChanges>
+ ComponentSetOption<ColliderParent>,
{
) {
// Set the rigid-bodies and kinematic bodies to their final position.
for handle in islands.iter_active_bodies() {
bodies.map_mut_internal(handle.0, |poss: &mut RigidBodyPosition| {
@@ -377,17 +329,12 @@ impl PhysicsPipeline {
}
}
fn interpolate_kinematic_velocities<Bodies>(
fn interpolate_kinematic_velocities(
&mut self,
integration_parameters: &IntegrationParameters,
islands: &IslandManager,
bodies: &mut Bodies,
) where
Bodies: ComponentSetMut<RigidBodyVelocity>
+ ComponentSetMut<RigidBodyPosition>
+ ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyMassProps>,
{
bodies: &mut RigidBodySet,
) {
// Update kinematic bodies velocities.
// TODO: what is the best place for this? It should at least be
// located before the island computation because we test the velocity
@@ -440,7 +387,7 @@ impl PhysicsPipeline {
impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
ccd_solver: &mut CCDSolver,
hooks: &dyn PhysicsHooks<RigidBodySet, ColliderSet>,
hooks: &dyn PhysicsHooks,
events: &dyn EventHandler,
) {
let mut modified_bodies = bodies.take_modified();
@@ -467,46 +414,24 @@ impl PhysicsPipeline {
}
/// Executes one timestep of the physics simulation.
pub fn step_generic<Bodies, Colliders>(
pub fn step_generic(
&mut self,
gravity: &Vector<Real>,
integration_parameters: &IntegrationParameters,
islands: &mut IslandManager,
broad_phase: &mut BroadPhase,
narrow_phase: &mut NarrowPhase,
bodies: &mut Bodies,
colliders: &mut Colliders,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
modified_bodies: &mut Vec<RigidBodyHandle>,
modified_colliders: &mut Vec<ColliderHandle>,
removed_colliders: &mut Vec<ColliderHandle>,
impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
ccd_solver: &mut CCDSolver,
hooks: &dyn PhysicsHooks<Bodies, Colliders>,
hooks: &dyn PhysicsHooks,
events: &dyn EventHandler,
) where
Bodies: ComponentSetMut<RigidBodyPosition>
+ ComponentSetMut<RigidBodyVelocity>
+ ComponentSetMut<RigidBodyMassProps>
+ ComponentSetMut<RigidBodyIds>
+ ComponentSetMut<RigidBodyForces>
+ ComponentSetMut<RigidBodyActivation>
+ ComponentSetMut<RigidBodyChanges>
+ ComponentSetMut<RigidBodyCcd>
+ ComponentSet<RigidBodyColliders>
+ ComponentSet<RigidBodyDamping>
+ ComponentSet<RigidBodyDominance>
+ ComponentSet<RigidBodyType>,
Colliders: ComponentSetMut<ColliderBroadPhaseData>
+ ComponentSetMut<ColliderChanges>
+ ComponentSetMut<ColliderPosition>
+ ComponentSet<ColliderShape>
+ ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderType>
+ ComponentSet<ColliderMaterial>
+ ComponentSet<ColliderFlags>
+ ComponentSet<ColliderMassProps>,
{
) {
self.counters.reset();
self.counters.step_started();

229
src/pipeline/query_pipeline.rs
View File

@@ -1,4 +1,3 @@
use crate::data::{BundleSet, ComponentSet, ComponentSetOption};
use crate::dynamics::{
IslandManager, RigidBodyColliders, RigidBodyForces, RigidBodyMassProps, RigidBodyPosition,
RigidBodyVelocity,
@@ -40,9 +39,9 @@ pub struct QueryPipeline {
dilation_factor: Real,
}
struct QueryPipelineAsCompositeShape<'a, Colliders> {
struct QueryPipelineAsCompositeShape<'a> {
query_pipeline: &'a QueryPipeline,
colliders: &'a Colliders,
colliders: &'a ColliderSet,
query_groups: InteractionGroups,
filter: Option<&'a dyn Fn(ColliderHandle) -> bool>,
}
@@ -63,12 +62,7 @@ pub enum QueryPipelineMode {
},
}
impl<'a, Colliders> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a, Colliders>
where
// TODO ECS: make everything optional but the shape?
Colliders:
ComponentSet<ColliderFlags> + ComponentSet<ColliderPosition> + ComponentSet<ColliderShape>,
{
impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> {
type PartShape = dyn Shape;
type PartId = ColliderHandle;
@@ -77,15 +71,11 @@ where
shape_id: Self::PartId,
mut f: impl FnMut(Option<&Isometry<Real>>, &Self::PartShape),
) {
let co_flags: Option<&ColliderFlags> = self.colliders.get(shape_id.0);
if let Some(co_flags) = co_flags {
if co_flags.collision_groups.test(self.query_groups)
if let Some(co) = self.colliders.get(shape_id) {
if co.flags.collision_groups.test(self.query_groups)
&& self.filter.map(|f| f(shape_id)).unwrap_or(true)
{
let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) =
self.colliders.index_bundle(shape_id.0);
f(Some(co_pos), &**co_shape)
f(Some(co.pos), &**co.shape)
}
}
}
@@ -115,12 +105,12 @@ impl QueryPipeline {
Self::with_query_dispatcher(DefaultQueryDispatcher)
}
fn as_composite_shape<'a, Colliders>(
fn as_composite_shape<'a>(
&'a self,
colliders: &'a Colliders,
colliders: &'a ColliderSet,
query_groups: InteractionGroups,
filter: Option<&'a dyn Fn(ColliderHandle) -> bool>,
) -> QueryPipelineAsCompositeShape<'a, Colliders> {
) -> QueryPipelineAsCompositeShape<'a> {
QueryPipelineAsCompositeShape {
query_pipeline: self,
colliders,
@@ -162,21 +152,12 @@ impl QueryPipeline {
}
/// Update the acceleration structure on the query pipeline.
pub fn update_generic<Bodies, Colliders>(
pub fn update_generic(
&mut self,
islands: &IslandManager,
bodies: &Bodies,
colliders: &Colliders,
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyColliders>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyForces>,
Colliders: ComponentSet<ColliderShape>
+ ComponentSet<ColliderPosition>
+ ComponentSetOption<ColliderParent>,
{
bodies: &RigidBodySet,
colliders: &ColliderSet,
) {
self.update_with_mode(
islands,
bodies,
@@ -186,40 +167,22 @@ impl QueryPipeline {
}
/// Update the acceleration structure on the query pipeline.
pub fn update_with_mode<Bodies, Colliders>(
pub fn update_with_mode(
&mut self,
islands: &IslandManager,
bodies: &Bodies,
colliders: &Colliders,
bodies: &RigidBodySet,
colliders: &ColliderSet,
mode: QueryPipelineMode,
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyColliders>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyForces>,
Colliders: ComponentSet<ColliderShape>
+ ComponentSet<ColliderPosition>
+ ComponentSetOption<ColliderParent>,
{
struct DataGenerator<'a, Bs, Cs> {
bodies: &'a Bs,
colliders: &'a Cs,
) {
struct DataGenerator<'a> {
bodies: &'a RigidBodySet,
colliders: &'a ColliderSet,
mode: QueryPipelineMode,
}
impl<'a, Bs, Cs> QBVHDataGenerator<ColliderHandle> for DataGenerator<'a, Bs, Cs>
where
Bs: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyForces>,
Cs: ComponentSet<ColliderShape>
+ ComponentSet<ColliderPosition>
+ ComponentSetOption<ColliderParent>,
{
impl<'a> QBVHDataGenerator<ColliderHandle> for DataGenerator<'a> {
fn size_hint(&self) -> usize {
ComponentSet::<ColliderShape>::size_hint(self.colliders)
self.colliders.len()
}
#[inline(always)]
@@ -313,16 +276,13 @@ impl QueryPipeline {
QueryPipelineMode::SweepTestWithNextPosition => {
self.qbvh.update(
|handle| {
let co_parent: Option<&ColliderParent> = colliders.get(handle.0);
let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) =
colliders.index_bundle(handle.0);
if let Some(co_parent) = co_parent {
let rb_pos: &RigidBodyPosition = bodies.index(co_parent.handle.0);
let next_position = rb_pos.next_position * co_parent.pos_wrt_parent;
co_shape.compute_swept_aabb(&co_pos, &next_position)
let co = &colliders[handle];
if let Some(parent) = co.parent {
let rb_pos: &RigidBodyPosition = bodies.index(co.parent.handle.0);
let next_position = rb_pos.next_position * co.parent.pos_wrt_parent;
co.shape.compute_swept_aabb(&co.pos, &next_position)
} else {
co_shape.compute_aabb(&co_pos)
co.shape.compute_aabb(&co.pos)
}
},
self.dilation_factor,
@@ -331,25 +291,17 @@ impl QueryPipeline {
QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
self.qbvh.update(
|handle| {
let co_parent: Option<&ColliderParent> = colliders.get(handle.0);
let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) =
colliders.index_bundle(handle.0);
let co = &colliders[handle];
if let Some(parent) = co.parent {
let rb = &bodies[parent.handle];
let predicted_pos = rb
.pos
.integrate_forces_and_velocities(dt, rb.forces, rb.vels, rb.mprops);
if let Some(co_parent) = co_parent {
let (rb_pos, vels, forces, mprops): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyForces,
&RigidBodyMassProps,
) = bodies.index_bundle(co_parent.handle.0);
let predicted_pos =
rb_pos.integrate_forces_and_velocities(dt, forces, vels, mprops);
let next_position = predicted_pos * co_parent.pos_wrt_parent;
co_shape.compute_swept_aabb(&co_pos, &next_position)
let next_position = predicted_pos * parent.pos_wrt_parent;
co.shape.compute_swept_aabb(&co.pos, &next_position)
} else {
co_shape.compute_aabb(&co_pos)
co.shape.compute_aabb(&co.pos)
}
},
self.dilation_factor,
@@ -373,20 +325,15 @@ impl QueryPipeline {
/// * `filter`: a more fine-grained filter. A collider is taken into account by this query if
/// its `contact_group` is compatible with the `query_groups`, and if this `filter`
/// is either `None` or returns `true`.
pub fn cast_ray<Colliders>(
pub fn cast_ray(
&self,
colliders: &Colliders,
colliders: &ColliderSet,
ray: &Ray,
max_toi: Real,
solid: bool,
query_groups: InteractionGroups,
filter: Option<&dyn Fn(ColliderHandle) -> bool>,
) -> Option<(ColliderHandle, Real)>
where
Colliders: ComponentSet<ColliderFlags>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>,
{
) -> Option<(ColliderHandle, Real)> {
let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
let mut visitor =
RayCompositeShapeToiBestFirstVisitor::new(&pipeline_shape, ray, max_toi, solid);
@@ -409,20 +356,15 @@ impl QueryPipeline {
/// * `filter`: a more fine-grained filter. A collider is taken into account by this query if
/// its `contact_group` is compatible with the `query_groups`, and if this `filter`
/// is either `None` or returns `true`.
pub fn cast_ray_and_get_normal<Colliders>(
pub fn cast_ray_and_get_normal(
&self,
colliders: &Colliders,
colliders: &ColliderSet,
ray: &Ray,
max_toi: Real,
solid: bool,
query_groups: InteractionGroups,
filter: Option<&dyn Fn(ColliderHandle) -> bool>,
) -> Option<(ColliderHandle, RayIntersection)>
where
Colliders: ComponentSet<ColliderFlags>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>,
{
) -> Option<(ColliderHandle, RayIntersection)> {
let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
let mut visitor = RayCompositeShapeToiAndNormalBestFirstVisitor::new(
&pipeline_shape,
@@ -452,20 +394,16 @@ impl QueryPipeline {
/// * `callback`: function executed on each collider for which a ray intersection has been found.
/// There is no guarantees on the order the results will be yielded. If this callback returns `false`,
/// this method will exit early, ignore any further raycast.
pub fn intersections_with_ray<'a, Colliders>(
pub fn intersections_with_ray<'a, ColliderSet>(
&self,
colliders: &'a Colliders,
colliders: &'a ColliderSet,
ray: &Ray,
max_toi: Real,
solid: bool,
query_groups: InteractionGroups,
filter: Option<&dyn Fn(ColliderHandle) -> bool>,
mut callback: impl FnMut(ColliderHandle, RayIntersection) -> bool,
) where
Colliders: ComponentSet<ColliderFlags>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>,
{
) {
let mut leaf_callback = &mut |handle: &ColliderHandle| {
let co_shape: Option<&ColliderShape> = colliders.get(handle.0);
if let Some(co_shape) = co_shape {
@@ -499,19 +437,14 @@ impl QueryPipeline {
/// * `filter` - a more fine-grained filter. A collider is taken into account by this query if
/// its `contact_group` is compatible with the `query_groups`, and if this `filter`
/// is either `None` or returns `true`.
pub fn intersection_with_shape<Colliders>(
pub fn intersection_with_shape(
&self,
colliders: &Colliders,
colliders: &ColliderSet,
shape_pos: &Isometry<Real>,
shape: &dyn Shape,
query_groups: InteractionGroups,
filter: Option<&dyn Fn(ColliderHandle) -> bool>,
) -> Option<ColliderHandle>
where
Colliders: ComponentSet<ColliderFlags>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>,
{
) -> Option<ColliderHandle> {
let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
let mut visitor = IntersectionCompositeShapeShapeBestFirstVisitor::new(
&*self.query_dispatcher,
@@ -540,19 +473,14 @@ impl QueryPipeline {
/// * `filter` - a more fine-grained filter. A collider is taken into account by this query if
/// its `contact_group` is compatible with the `query_groups`, and if this `filter`
/// is either `None` or returns `true`.
pub fn project_point<Colliders>(
pub fn project_point(
&self,
colliders: &Colliders,
colliders: &ColliderSet,
point: &Point<Real>,
solid: bool,
query_groups: InteractionGroups,
filter: Option<&dyn Fn(ColliderHandle) -> bool>,
) -> Option<(ColliderHandle, PointProjection)>
where
Colliders: ComponentSet<ColliderFlags>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>,
{
) -> Option<(ColliderHandle, PointProjection)> {
let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
let mut visitor =
PointCompositeShapeProjBestFirstVisitor::new(&pipeline_shape, point, solid);
@@ -574,18 +502,14 @@ impl QueryPipeline {
/// is either `None` or returns `true`.
/// * `callback` - A function called with each collider with a shape
/// containing the `point`.
pub fn intersections_with_point<'a, Colliders>(
pub fn intersections_with_point<'a, ColliderSet>(
&self,
colliders: &'a Colliders,
colliders: &'a ColliderSet,
point: &Point<Real>,
query_groups: InteractionGroups,
filter: Option<&dyn Fn(ColliderHandle) -> bool>,
mut callback: impl FnMut(ColliderHandle) -> bool,
) where
Colliders: ComponentSet<ColliderFlags>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>,
{
) {
let mut leaf_callback = &mut |handle: &ColliderHandle| {
let co_shape: Option<&ColliderShape> = colliders.get(handle.0);
@@ -626,18 +550,13 @@ impl QueryPipeline {
/// * `filter` - a more fine-grained filter. A collider is taken into account by this query if
/// its `contact_group` is compatible with the `query_groups`, and if this `filter`
/// is either `None` or returns `true`.
pub fn project_point_and_get_feature<Colliders>(
pub fn project_point_and_get_feature(
&self,
colliders: &Colliders,
colliders: &ColliderSet,
point: &Point<Real>,
query_groups: InteractionGroups,
filter: Option<&dyn Fn(ColliderHandle) -> bool>,
) -> Option<(ColliderHandle, PointProjection, FeatureId)>
where
Colliders: ComponentSet<ColliderFlags>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>,
{
) -> Option<(ColliderHandle, PointProjection, FeatureId)> {
let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
let mut visitor =
PointCompositeShapeProjWithFeatureBestFirstVisitor::new(&pipeline_shape, point, false);
@@ -674,21 +593,16 @@ impl QueryPipeline {
/// * `filter` - a more fine-grained filter. A collider is taken into account by this query if
/// its `contact_group` is compatible with the `query_groups`, and if this `filter`
/// is either `None` or returns `true`.
pub fn cast_shape<'a, Colliders>(
pub fn cast_shape<'a>(
&self,
colliders: &'a Colliders,
colliders: &'a ColliderSet,
shape_pos: &Isometry<Real>,
shape_vel: &Vector<Real>,
shape: &dyn Shape,
max_toi: Real,
query_groups: InteractionGroups,
filter: Option<&dyn Fn(ColliderHandle) -> bool>,
) -> Option<(ColliderHandle, TOI)>
where
Colliders: ComponentSet<ColliderFlags>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>,
{
) -> Option<(ColliderHandle, TOI)> {
let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
let mut visitor = TOICompositeShapeShapeBestFirstVisitor::new(
&*self.query_dispatcher,
@@ -724,9 +638,9 @@ impl QueryPipeline {
/// * `filter` - a more fine-grained filter. A collider is taken into account by this query if
/// its `contact_group` is compatible with the `query_groups`, and if this `filter`
/// is either `None` or returns `true`.
pub fn nonlinear_cast_shape<Colliders>(
pub fn nonlinear_cast_shape(
&self,
colliders: &Colliders,
colliders: &ColliderSet,
shape_motion: &NonlinearRigidMotion,
shape: &dyn Shape,
start_time: Real,
@@ -734,12 +648,7 @@ impl QueryPipeline {
stop_at_penetration: bool,
query_groups: InteractionGroups,
filter: Option<&dyn Fn(ColliderHandle) -> bool>,
) -> Option<(ColliderHandle, TOI)>
where
Colliders: ComponentSet<ColliderFlags>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>,
{
) -> Option<(ColliderHandle, TOI)> {
let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter);
let pipeline_motion = NonlinearRigidMotion::identity();
let mut visitor = NonlinearTOICompositeShapeShapeBestFirstVisitor::new(
@@ -768,19 +677,15 @@ impl QueryPipeline {
/// its `contact_group` is compatible with the `query_groups`, and if this `filter`
/// is either `None` or returns `true`.
/// * `callback` - A function called with the handles of each collider intersecting the `shape`.
pub fn intersections_with_shape<'a, Colliders>(
pub fn intersections_with_shape<'a, ColliderSet>(
&self,
colliders: &'a Colliders,
colliders: &'a ColliderSet,
shape_pos: &Isometry<Real>,
shape: &dyn Shape,
query_groups: InteractionGroups,
filter: Option<&dyn Fn(ColliderHandle) -> bool>,
mut callback: impl FnMut(ColliderHandle) -> bool,
) where
Colliders: ComponentSet<ColliderFlags>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>,
{
) {
let dispatcher = &*self.query_dispatcher;
let inv_shape_pos = shape_pos.inverse();

98
src/pipeline/user_changes.rs
View File

@@ -1,28 +1,15 @@
use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
use crate::dynamics::{
IslandManager, RigidBodyActivation, RigidBodyChanges, RigidBodyColliders, RigidBodyHandle,
RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
};
use crate::geometry::{
ColliderChanges, ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition,
ColliderShape,
IslandManager, RigidBodyActivation, RigidBodyChanges, RigidBodyHandle, RigidBodyIds,
RigidBodyPosition, RigidBodySet, RigidBodyType,
};
use crate::geometry::{ColliderChanges, ColliderHandle, ColliderPosition, ColliderSet};
use parry::utils::hashmap::HashMap;
pub(crate) fn handle_user_changes_to_colliders<Bodies, Colliders>(
bodies: &mut Bodies,
colliders: &mut Colliders,
pub(crate) fn handle_user_changes_to_colliders(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
modified_colliders: &[ColliderHandle],
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyColliders>
+ ComponentSetMut<RigidBodyMassProps>,
Colliders: ComponentSetMut<ColliderChanges>
+ ComponentSetMut<ColliderPosition>
+ ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderShape>
+ ComponentSet<ColliderMassProps>,
{
) {
// TODO: avoid this hashmap? We could perhaps add a new flag to RigidBodyChanges to
// indicated that the mass properties need to be recomputed?
let mut mprops_to_update = HashMap::default();
@@ -30,25 +17,20 @@ pub(crate) fn handle_user_changes_to_colliders<Bodies, Colliders>(
for handle in modified_colliders {
// NOTE: we use `get` because the collider may no longer
// exist if it has been removed.
let co_changes: Option<ColliderChanges> = colliders.get(handle.0).copied();
if let Some(co_changes) = co_changes {
if co_changes.contains(ColliderChanges::PARENT) {
let co_parent: Option<&ColliderParent> = colliders.get(handle.0);
if let Some(co_parent) = co_parent {
if let Some(co) = colliders.get(*handle) {
if co.changes.contains(ColliderChanges::PARENT) {
if let Some(co_parent) = co.parent {
let parent_pos: &RigidBodyPosition = bodies.index(co_parent.handle.0);
let new_pos = parent_pos.position * co_parent.pos_wrt_parent;
let new_changes = co_changes | ColliderChanges::POSITION;
let new_changes = co.changes | ColliderChanges::POSITION;
colliders.set_internal(handle.0, ColliderPosition(new_pos));
colliders.set_internal(handle.0, new_changes);
}
}
if co_changes.contains(ColliderChanges::SHAPE) {
let co_parent: Option<&ColliderParent> = colliders.get(handle.0);
if let Some(co_parent) = co_parent {
if co.changes.contains(ColliderChanges::SHAPE) {
if let Some(co_parent) = co.parent {
mprops_to_update.insert(co_parent.handle, ());
}
}
@@ -56,11 +38,10 @@ pub(crate) fn handle_user_changes_to_colliders<Bodies, Colliders>(
}
for (to_update, _) in mprops_to_update {
let (rb_pos, rb_colliders): (&RigidBodyPosition, &RigidBodyColliders) =
bodies.index_bundle(to_update.0);
let position = rb_pos.position;
let rb = &bodies[to_update];
let position = rb.position();
// FIXME: remove the clone once we remove the ComponentSets.
let attached_colliders = rb_colliders.clone();
let attached_colliders = rb.colliders().clone();
bodies.map_mut_internal(to_update.0, |rb_mprops| {
rb_mprops.recompute_mass_properties_from_colliders(
@@ -72,23 +53,13 @@ pub(crate) fn handle_user_changes_to_colliders<Bodies, Colliders>(
}
}
pub(crate) fn handle_user_changes_to_rigid_bodies<Bodies, Colliders>(
pub(crate) fn handle_user_changes_to_rigid_bodies(
mut islands: Option<&mut IslandManager>,
bodies: &mut Bodies,
colliders: &mut Colliders,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
modified_bodies: &[RigidBodyHandle],
modified_colliders: &mut Vec<ColliderHandle>,
) where
Bodies: ComponentSetMut<RigidBodyChanges>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>
+ ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyColliders>
+ ComponentSet<RigidBodyPosition>,
Colliders: ComponentSetMut<ColliderPosition>
+ ComponentSetMut<ColliderChanges>
+ ComponentSetOption<ColliderParent>,
{
) {
enum FinalAction {
UpdateActiveKinematicSetId,
UpdateActiveDynamicSetId,
@@ -96,28 +67,23 @@ pub(crate) fn handle_user_changes_to_rigid_bodies<Bodies, Colliders>(
for handle in modified_bodies {
let mut final_action = None;
let changes: Option<&RigidBodyChanges> = bodies.get(handle.0);
if changes.is_none() {
if !bodies.contains(*handle) {
// The body no longer exists.
continue;
}
let mut changes = *changes.unwrap();
let mut ids: RigidBodyIds = *bodies.index(handle.0);
let mut activation: RigidBodyActivation = *bodies.index(handle.0);
let (status, rb_colliders, poss): (
&RigidBodyType,
&RigidBodyColliders,
&RigidBodyPosition,
) = bodies.index_bundle(handle.0);
let rb = &bodies[handle];
let mut changes = rb.changes;
let mut ids: RigidBodyIds = rb.ids;
let mut activation: RigidBodyActivation = rb.activation;
{
// The body's status changed. We need to make sure
// it is on the correct active set.
if let Some(islands) = islands.as_deref_mut() {
if changes.contains(RigidBodyChanges::TYPE) {
match status {
match rb.status {
RigidBodyType::Dynamic => {
// Remove from the active kinematic set if it was there.
if islands.active_kinematic_set.get(ids.active_set_id) == Some(handle) {
@@ -161,9 +127,10 @@ pub(crate) fn handle_user_changes_to_rigid_bodies<Bodies, Colliders>(
if changes.contains(RigidBodyChanges::POSITION)
|| changes.contains(RigidBodyChanges::COLLIDERS)
{
rb_colliders.update_positions(colliders, modified_colliders, &poss.position);
rb.colliders
.update_positions(colliders, modified_colliders, &rb.pos.position);
if status.is_kinematic()
if rb.is_kinematic()
&& islands.active_kinematic_set.get(ids.active_set_id) != Some(handle)
{
ids.active_set_id = islands.active_kinematic_set.len();
@@ -175,7 +142,7 @@ pub(crate) fn handle_user_changes_to_rigid_bodies<Bodies, Colliders>(
// sleeping and if it is not already inside of the active set.
if changes.contains(RigidBodyChanges::SLEEP)
&& !activation.sleeping // May happen if the body was put to sleep manually.
&& status.is_dynamic() // Only dynamic bodies are in the active dynamic set.
&& rb.is_dynamic() // Only dynamic bodies are in the active dynamic set.
&& islands.active_dynamic_set.get(ids.active_set_id) != Some(handle)
{
ids.active_set_id = islands.active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates.
@@ -186,14 +153,15 @@ pub(crate) fn handle_user_changes_to_rigid_bodies<Bodies, Colliders>(
if changes.contains(RigidBodyChanges::POSITION)
|| changes.contains(RigidBodyChanges::COLLIDERS)
{
rb_colliders.update_positions(colliders, modified_colliders, &poss.position);
rb.colliders
.update_positions(colliders, modified_colliders, &rb.pos.position);
}
}
if changes.contains(RigidBodyChanges::DOMINANCE)
|| changes.contains(RigidBodyChanges::TYPE)
{
for handle in rb_colliders.0.iter() {
for handle in rb.colliders.0.iter() {
colliders.map_mut_internal(handle.0, |co_changes: &mut ColliderChanges| {
if !co_changes.contains(ColliderChanges::MODIFIED) {
modified_colliders.push(*handle);

2
src_testbed/ui.rs
View File

@@ -268,7 +268,7 @@ fn serialization_string(timestep_id: usize, physics: &PhysicsState) -> String {
Hashes at frame: {}
|_ Broad phase [{:.1}KB]: {}
|_ Narrow phase [{:.1}KB]: {}
|_ Bodies [{:.1}KB]: {}
|_ &RigidBodySet [{:.1}KB]: {}
|_ Colliders [{:.1}KB]: {}
|_ Joints [{:.1}KB]: {}"#,
serialization_time,