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

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