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

10
Cargo.toml
View File

@@ -8,11 +8,11 @@ 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" }
#nalgebra = { path = "../nalgebra" }
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" }
#nalgebra = { git = "https://github.com/dimforge/nalgebra", branch = "dev" }

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

89
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 update_mass_props {
bodies.map_mut_internal(link.rigid_body.0, |mprops: &mut RigidBodyMassProps| {
mprops.update_world_mass_properties(&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 {
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

39
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,14 +307,14 @@ 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)
.transform_by(&co_parent.pos_wrt_parent);
self.local_mprops += to_add;
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;
}
}
}
@@ -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,