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

Rigid-body: don’t clear forces at end of timestep + don’t wake-up a rigid-body if the modified property is equal to the old value.

Browse Source
This commit is contained in:
Sébastien Crozet
2022-03-13 15:29:22 +01:00
committed by Sébastien Crozet
parent 1535db87c7
commit 8e07d8799f
3 changed files with 299 additions and 223 deletions
Download Patch File Download Diff File Expand all files Collapse all files

435
src/dynamics/rigid_body.rs
View File

@@ -1,7 +1,7 @@
use crate::dynamics::{ use crate::dynamics::{
MassProperties, RigidBodyActivation, RigidBodyCcd, RigidBodyChanges, RigidBodyColliders, LockedAxes, MassProperties, RigidBodyActivation, RigidBodyCcd, RigidBodyChanges,
RigidBodyDamping, RigidBodyDominance, RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyColliders, RigidBodyDamping, RigidBodyDominance, RigidBodyForces, RigidBodyIds,
RigidBodyMassPropsFlags, RigidBodyPosition, RigidBodyType, RigidBodyVelocity, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
}; };
use crate::geometry::{ use crate::geometry::{
Collider, ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition, ColliderShape, Collider, ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition, ColliderShape,
@@ -111,6 +111,10 @@ impl RigidBody {
if status != self.rb_type { if status != self.rb_type {
self.changes.insert(RigidBodyChanges::TYPE); self.changes.insert(RigidBodyChanges::TYPE);
self.rb_type = status; self.rb_type = status;
if status == RigidBodyType::Static {
self.rb_vels = RigidBodyVelocity::zero();
}
} }
} }
@@ -129,23 +133,35 @@ impl RigidBody {
self.rb_dominance.effective_group(&self.rb_type) self.rb_dominance.effective_group(&self.rb_type)
} }
#[inline]
pub fn set_locked_axes(&mut self, locked_axes: LockedAxes, wake_up: bool) {
if locked_axes != self.rb_mprops.flags {
if self.is_dynamic() && wake_up {
self.wake_up(true);
}
self.rb_mprops.flags = locked_axes;
self.update_world_mass_properties();
}
}
#[inline] #[inline]
/// Locks or unlocks all the rotations of this rigid-body. /// Locks or unlocks all the rotations of this rigid-body.
pub fn lock_rotations(&mut self, locked: bool, wake_up: bool) { pub fn lock_rotations(&mut self, locked: bool, wake_up: bool) {
if self.is_dynamic() { if !self.rb_mprops.flags.contains(LockedAxes::ROTATION_LOCKED) {
if wake_up { if self.is_dynamic() && wake_up {
self.wake_up(true); self.wake_up(true);
} }
self.rb_mprops self.rb_mprops
.flags .flags
.set(RigidBodyMassPropsFlags::ROTATION_LOCKED_X, locked); .set(LockedAxes::ROTATION_LOCKED_X, locked);
self.rb_mprops self.rb_mprops
.flags .flags
.set(RigidBodyMassPropsFlags::ROTATION_LOCKED_Y, locked); .set(LockedAxes::ROTATION_LOCKED_Y, locked);
self.rb_mprops self.rb_mprops
.flags .flags
.set(RigidBodyMassPropsFlags::ROTATION_LOCKED_Z, locked); .set(LockedAxes::ROTATION_LOCKED_Z, locked);
self.update_world_mass_properties(); self.update_world_mass_properties();
} }
} }
@@ -159,23 +175,23 @@ impl RigidBody {
allow_rotations_z: bool, allow_rotations_z: bool,
wake_up: bool, wake_up: bool,
) { ) {
if self.is_dynamic() { if self.rb_mprops.flags.contains(LockedAxes::ROTATION_LOCKED_X) != !allow_rotations_x
if wake_up { || 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.is_dynamic() && wake_up {
self.wake_up(true); self.wake_up(true);
} }
self.rb_mprops.flags.set( self.rb_mprops
RigidBodyMassPropsFlags::ROTATION_LOCKED_X, .flags
!allow_rotations_x, .set(LockedAxes::ROTATION_LOCKED_X, !allow_rotations_x);
); self.rb_mprops
self.rb_mprops.flags.set( .flags
RigidBodyMassPropsFlags::ROTATION_LOCKED_Y, .set(LockedAxes::ROTATION_LOCKED_Y, !allow_rotations_y);
!allow_rotations_y, self.rb_mprops
); .flags
self.rb_mprops.flags.set( .set(LockedAxes::ROTATION_LOCKED_Z, !allow_rotations_z);
RigidBodyMassPropsFlags::ROTATION_LOCKED_Z,
!allow_rotations_z,
);
self.update_world_mass_properties(); self.update_world_mass_properties();
} }
} }
@@ -183,14 +199,18 @@ impl RigidBody {
#[inline] #[inline]
/// Locks or unlocks all the rotations of this rigid-body. /// Locks or unlocks all the rotations of this rigid-body.
pub fn lock_translations(&mut self, locked: bool, wake_up: bool) { pub fn lock_translations(&mut self, locked: bool, wake_up: bool) {
if self.is_dynamic() { if !self
if wake_up { .rb_mprops
.flags
.contains(LockedAxes::TRANSLATION_LOCKED)
{
if self.is_dynamic() && wake_up {
self.wake_up(true); self.wake_up(true);
} }
self.rb_mprops self.rb_mprops
.flags .flags
.set(RigidBodyMassPropsFlags::TRANSLATION_LOCKED, locked); .set(LockedAxes::TRANSLATION_LOCKED, locked);
self.update_world_mass_properties(); self.update_world_mass_properties();
} }
} }
@@ -204,35 +224,65 @@ impl RigidBody {
#[cfg(feature = "dim3")] allow_translation_z: bool, #[cfg(feature = "dim3")] allow_translation_z: bool,
wake_up: bool, wake_up: bool,
) { ) {
if self.is_dynamic() { #[cfg(feature = "dim2")]
if wake_up { if self
self.wake_up(true); .rb_mprops
} .flags
.contains(LockedAxes::TRANSLATION_LOCKED_X)
self.rb_mprops.flags.set( == !allow_translation_x
RigidBodyMassPropsFlags::TRANSLATION_LOCKED_X, && self
!allow_translation_x, .rb_mprops
); .flags
self.rb_mprops.flags.set( .contains(LockedAxes::TRANSLATION_LOCKED_Y)
RigidBodyMassPropsFlags::TRANSLATION_LOCKED_Y, == !allow_translation_y
!allow_translation_y, {
); // Nothing to change.
#[cfg(feature = "dim3")] return;
self.rb_mprops.flags.set(
RigidBodyMassPropsFlags::TRANSLATION_LOCKED_Z,
!allow_translation_z,
);
self.update_world_mass_properties();
} }
#[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
{
// Nothing to change.
return;
}
if self.is_dynamic() && wake_up {
self.wake_up(true);
}
self.rb_mprops
.flags
.set(LockedAxes::TRANSLATION_LOCKED_X, !allow_translation_x);
self.rb_mprops
.flags
.set(LockedAxes::TRANSLATION_LOCKED_Y, !allow_translation_y);
#[cfg(feature = "dim3")]
self.rb_mprops
.flags
.set(LockedAxes::TRANSLATION_LOCKED_Z, !allow_translation_z);
self.update_world_mass_properties();
} }
/// Are the translations of this rigid-body locked? /// Are the translations of this rigid-body locked?
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
pub fn is_translation_locked(&self) -> bool { pub fn is_translation_locked(&self) -> bool {
self.rb_mprops.flags.contains( self.rb_mprops
RigidBodyMassPropsFlags::TRANSLATION_LOCKED_X .flags
| RigidBodyMassPropsFlags::TRANSLATION_LOCKED_Y, .contains(LockedAxes::TRANSLATION_LOCKED_X | LockedAxes::TRANSLATION_LOCKED_Y)
)
} }
/// Are the translations of this rigid-body locked? /// Are the translations of this rigid-body locked?
@@ -240,30 +290,22 @@ impl RigidBody {
pub fn is_translation_locked(&self) -> bool { pub fn is_translation_locked(&self) -> bool {
self.rb_mprops self.rb_mprops
.flags .flags
.contains(RigidBodyMassPropsFlags::TRANSLATION_LOCKED) .contains(LockedAxes::TRANSLATION_LOCKED)
} }
/// Are the rotations of this rigid-body locked? /// Are the rotations of this rigid-body locked?
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
pub fn is_rotation_locked(&self) -> bool { pub fn is_rotation_locked(&self) -> bool {
self.rb_mprops self.rb_mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z)
.flags
.contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_Z)
} }
/// Returns `true` for each rotational degrees of freedom locked on this rigid-body. /// Returns `true` for each rotational degrees of freedom locked on this rigid-body.
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
pub fn is_rotation_locked(&self) -> [bool; 3] { pub fn is_rotation_locked(&self) -> [bool; 3] {
[ [
self.rb_mprops self.rb_mprops.flags.contains(LockedAxes::ROTATION_LOCKED_X),
.flags self.rb_mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Y),
.contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_X), self.rb_mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z),
self.rb_mprops
.flags
.contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_Y),
self.rb_mprops
.flags
.contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_Z),
] ]
} }
@@ -300,12 +342,14 @@ impl RigidBody {
/// put to sleep because it did not move for a while. /// put to sleep because it did not move for a while.
#[inline] #[inline]
pub fn set_mass_properties(&mut self, props: MassProperties, wake_up: bool) { pub fn set_mass_properties(&mut self, props: MassProperties, wake_up: bool) {
if self.is_dynamic() && wake_up { if self.rb_mprops.local_mprops != props {
self.wake_up(true); if self.is_dynamic() && wake_up {
} self.wake_up(true);
}
self.rb_mprops.local_mprops = props; self.rb_mprops.local_mprops = props;
self.update_world_mass_properties(); self.update_world_mass_properties();
}
} }
/// The handles of colliders attached to this rigid body. /// The handles of colliders attached to this rigid body.
@@ -357,12 +401,14 @@ impl RigidBody {
/// Sets the gravity scale facter for this rigid-body. /// Sets the gravity scale facter for this rigid-body.
pub fn set_gravity_scale(&mut self, scale: Real, wake_up: bool) { pub fn set_gravity_scale(&mut self, scale: Real, wake_up: bool) {
if wake_up && self.rb_activation.sleeping { if self.rb_forces.gravity_scale != scale {
self.changes.insert(RigidBodyChanges::SLEEP); if wake_up && self.rb_activation.sleeping {
self.rb_activation.sleeping = false; self.changes.insert(RigidBodyChanges::SLEEP);
} self.rb_activation.sleeping = false;
}
self.rb_forces.gravity_scale = scale; self.rb_forces.gravity_scale = scale;
}
} }
/// The dominance group of this rigid-body. /// The dominance group of this rigid-body.
@@ -473,10 +519,19 @@ impl RigidBody {
/// If `wake_up` is `true` then the rigid-body will be woken up if it was /// 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. /// put to sleep because it did not move for a while.
pub fn set_linvel(&mut self, linvel: Vector<Real>, wake_up: bool) { pub fn set_linvel(&mut self, linvel: Vector<Real>, wake_up: bool) {
self.rb_vels.linvel = linvel; if self.rb_vels.linvel != linvel {
match self.rb_type {
if self.is_dynamic() && wake_up { RigidBodyType::Dynamic => {
self.wake_up(true) self.rb_vels.linvel = linvel;
if wake_up {
self.wake_up(true)
}
}
RigidBodyType::KinematicVelocityBased => {
self.rb_vels.linvel = linvel;
}
RigidBodyType::Static | RigidBodyType::KinematicPositionBased => {}
}
} }
} }
@@ -486,10 +541,19 @@ impl RigidBody {
/// put to sleep because it did not move for a while. /// put to sleep because it did not move for a while.
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
pub fn set_angvel(&mut self, angvel: Real, wake_up: bool) { pub fn set_angvel(&mut self, angvel: Real, wake_up: bool) {
self.rb_vels.angvel = angvel; if self.rb_vels.angvel != angvel {
match self.rb_type {
if self.is_dynamic() && wake_up { RigidBodyType::Dynamic => {
self.wake_up(true) self.rb_vels.angvel = angvel;
if wake_up {
self.wake_up(true)
}
}
RigidBodyType::KinematicVelocityBased => {
self.rb_vels.angvel = angvel;
}
RigidBodyType::Static | RigidBodyType::KinematicPositionBased => {}
}
} }
} }
@@ -499,10 +563,19 @@ impl RigidBody {
/// put to sleep because it did not move for a while. /// put to sleep because it did not move for a while.
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
pub fn set_angvel(&mut self, angvel: Vector<Real>, wake_up: bool) { pub fn set_angvel(&mut self, angvel: Vector<Real>, wake_up: bool) {
self.rb_vels.angvel = angvel; if self.rb_vels.angvel != angvel {
match self.rb_type {
if self.is_dynamic() && wake_up { RigidBodyType::Dynamic => {
self.wake_up(true) self.rb_vels.angvel = angvel;
if wake_up {
self.wake_up(true)
}
}
RigidBodyType::KinematicVelocityBased => {
self.rb_vels.angvel = angvel;
}
RigidBodyType::Static | RigidBodyType::KinematicPositionBased => {}
}
} }
} }
@@ -521,13 +594,17 @@ impl RigidBody {
/// Sets the translational part of this rigid-body's position. /// Sets the translational part of this rigid-body's position.
#[inline] #[inline]
pub fn set_translation(&mut self, translation: Vector<Real>, wake_up: bool) { pub fn set_translation(&mut self, translation: Vector<Real>, wake_up: bool) {
self.changes.insert(RigidBodyChanges::POSITION); if self.rb_pos.position.translation.vector != translation
self.rb_pos.position.translation.vector = translation; || self.rb_pos.next_position.translation.vector != translation
self.rb_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;
// TODO: Do we really need to check that the body isn't dynamic? // TODO: Do we really need to check that the body isn't dynamic?
if wake_up && self.is_dynamic() { if wake_up && self.is_dynamic() {
self.wake_up(true) self.wake_up(true)
}
} }
} }
@@ -540,14 +617,19 @@ impl RigidBody {
/// Sets the rotational part of this rigid-body's position. /// Sets the rotational part of this rigid-body's position.
#[inline] #[inline]
pub fn set_rotation(&mut self, rotation: AngVector<Real>, wake_up: bool) { pub fn set_rotation(&mut self, rotation: AngVector<Real>, wake_up: bool) {
self.changes.insert(RigidBodyChanges::POSITION);
let rotation = Rotation::new(rotation); let rotation = Rotation::new(rotation);
self.rb_pos.position.rotation = rotation;
self.rb_pos.next_position.rotation = rotation;
// TODO: Do we really need to check that the body isn't dynamic? if self.rb_pos.position.rotation != rotation
if wake_up && self.is_dynamic() { || self.rb_pos.next_position.rotation != rotation
self.wake_up(true) {
self.changes.insert(RigidBodyChanges::POSITION);
self.rb_pos.position.rotation = rotation;
self.rb_pos.next_position.rotation = rotation;
// TODO: Do we really need to check that the body isn't dynamic?
if wake_up && self.is_dynamic() {
self.wake_up(true)
}
} }
} }
@@ -561,13 +643,15 @@ impl RigidBody {
/// If `wake_up` is `true` then the rigid-body will be woken up if it was /// 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. /// put to sleep because it did not move for a while.
pub fn set_position(&mut self, pos: Isometry<Real>, wake_up: bool) { pub fn set_position(&mut self, pos: Isometry<Real>, wake_up: bool) {
self.changes.insert(RigidBodyChanges::POSITION); if self.rb_pos.position != pos || self.rb_pos.next_position != pos {
self.rb_pos.position = pos; self.changes.insert(RigidBodyChanges::POSITION);
self.rb_pos.next_position = pos; self.rb_pos.position = pos;
self.rb_pos.next_position = pos;
// TODO: Do we really need to check that the body isn't dynamic? // TODO: Do we really need to check that the body isn't dynamic?
if wake_up && self.is_dynamic() { if wake_up && self.is_dynamic() {
self.wake_up(true) self.wake_up(true)
}
} }
} }
@@ -611,12 +695,10 @@ impl RigidBody {
/// ## Applying forces and torques /// ## Applying forces and torques
impl RigidBody { impl RigidBody {
/// Applies a force at the center-of-mass of this rigid-body. /// Resets to zero all the constant (linear) forces applied to this rigid-body.
/// The force will be applied in the next simulation step. pub fn reset_force(&mut self, wake_up: bool) {
/// This does nothing on non-dynamic bodies. if !self.rb_forces.user_force.is_zero() {
pub fn apply_force(&mut self, force: Vector<Real>, wake_up: bool) { self.rb_forces.user_force = na::zero();
if self.rb_type == RigidBodyType::Dynamic {
self.rb_forces.force += force;
if wake_up { if wake_up {
self.wake_up(true); self.wake_up(true);
@@ -624,44 +706,76 @@ impl RigidBody {
} }
} }
/// Applies a torque at the center-of-mass of this rigid-body. /// Resets to zero all the constant torques applied to this rigid-body.
/// The torque will be applied in the next simulation step. pub fn reset_torque(&mut self, wake_up: bool) {
if !self.rb_forces.user_torque.is_zero() {
self.rb_forces.user_torque = na::zero();
if wake_up {
self.wake_up(true);
}
}
}
/// Adds to this rigid-body a constant force applied at its center-of-mass.ç
///
/// 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 wake_up {
self.wake_up(true);
}
}
}
}
/// Adds to this rigid-body a constant torque at its center-of-mass.
///
/// This does nothing on non-dynamic bodies. /// This does nothing on non-dynamic bodies.
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
pub fn apply_torque(&mut self, torque: Real, wake_up: bool) { pub fn add_torque(&mut self, torque: Real, wake_up: bool) {
if self.rb_type == RigidBodyType::Dynamic { if !torque.is_zero() {
self.rb_forces.torque += torque; if self.rb_type == RigidBodyType::Dynamic {
self.rb_forces.user_torque += torque;
if wake_up { if wake_up {
self.wake_up(true); self.wake_up(true);
}
} }
} }
} }
/// Applies a torque at the center-of-mass of this rigid-body. /// Adds to this rigid-body a constant torque at its center-of-mass.
/// The torque will be applied in the next simulation step. ///
/// This does nothing on non-dynamic bodies. /// This does nothing on non-dynamic bodies.
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
pub fn apply_torque(&mut self, torque: Vector<Real>, wake_up: bool) { pub fn add_torque(&mut self, torque: Vector<Real>, wake_up: bool) {
if self.rb_type == RigidBodyType::Dynamic { if !torque.is_zero() {
self.rb_forces.torque += torque; if self.rb_type == RigidBodyType::Dynamic {
self.rb_forces.user_torque += torque;
if wake_up { if wake_up {
self.wake_up(true); self.wake_up(true);
}
} }
} }
} }
/// Applies a force at the given world-space point of this rigid-body. /// Adds to this rigid-body a constant force at the given world-space point of this rigid-body.
/// The force will be applied in the next simulation step. ///
/// This does nothing on non-dynamic bodies. /// This does nothing on non-dynamic bodies.
pub fn apply_force_at_point(&mut self, force: Vector<Real>, point: Point<Real>, wake_up: bool) { pub fn add_force_at_point(&mut self, force: Vector<Real>, point: Point<Real>, wake_up: bool) {
if self.rb_type == RigidBodyType::Dynamic { if !force.is_zero() {
self.rb_forces.force += force; if self.rb_type == RigidBodyType::Dynamic {
self.rb_forces.torque += (point - self.rb_mprops.world_com).gcross(force); self.rb_forces.user_force += force;
self.rb_forces.user_torque += (point - self.rb_mprops.world_com).gcross(force);
if wake_up { if wake_up {
self.wake_up(true); self.wake_up(true);
}
} }
} }
} }
@@ -673,7 +787,7 @@ impl RigidBody {
/// The impulse is applied right away, changing the linear velocity. /// The impulse is applied right away, changing the linear velocity.
/// This does nothing on non-dynamic bodies. /// This does nothing on non-dynamic bodies.
pub fn apply_impulse(&mut self, impulse: Vector<Real>, wake_up: bool) { pub fn apply_impulse(&mut self, impulse: Vector<Real>, wake_up: bool) {
if self.rb_type == RigidBodyType::Dynamic { if !impulse.is_zero() && self.rb_type == RigidBodyType::Dynamic {
self.rb_vels.linvel += impulse.component_mul(&self.rb_mprops.effective_inv_mass); self.rb_vels.linvel += impulse.component_mul(&self.rb_mprops.effective_inv_mass);
if wake_up { if wake_up {
@@ -687,7 +801,7 @@ impl RigidBody {
/// This does nothing on non-dynamic bodies. /// This does nothing on non-dynamic bodies.
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
pub fn apply_torque_impulse(&mut self, torque_impulse: Real, wake_up: bool) { pub fn apply_torque_impulse(&mut self, torque_impulse: Real, wake_up: bool) {
if self.rb_type == RigidBodyType::Dynamic { if !torque_impulse.is_zero() && self.rb_type == RigidBodyType::Dynamic {
self.rb_vels.angvel += self.rb_mprops.effective_world_inv_inertia_sqrt self.rb_vels.angvel += self.rb_mprops.effective_world_inv_inertia_sqrt
* (self.rb_mprops.effective_world_inv_inertia_sqrt * torque_impulse); * (self.rb_mprops.effective_world_inv_inertia_sqrt * torque_impulse);
@@ -702,7 +816,7 @@ impl RigidBody {
/// This does nothing on non-dynamic bodies. /// This does nothing on non-dynamic bodies.
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
pub fn apply_torque_impulse(&mut self, torque_impulse: Vector<Real>, wake_up: bool) { pub fn apply_torque_impulse(&mut self, torque_impulse: Vector<Real>, wake_up: bool) {
if self.rb_type == RigidBodyType::Dynamic { if !torque_impulse.is_zero() && self.rb_type == RigidBodyType::Dynamic {
self.rb_vels.angvel += self.rb_mprops.effective_world_inv_inertia_sqrt self.rb_vels.angvel += self.rb_mprops.effective_world_inv_inertia_sqrt
* (self.rb_mprops.effective_world_inv_inertia_sqrt * torque_impulse); * (self.rb_mprops.effective_world_inv_inertia_sqrt * torque_impulse);
@@ -772,7 +886,7 @@ pub struct RigidBodyBuilder {
/// Damping factor for gradually slowing down the angular motion of the rigid-body, `0.0` by default. /// Damping factor for gradually slowing down the angular motion of the rigid-body, `0.0` by default.
pub angular_damping: Real, pub angular_damping: Real,
rb_type: RigidBodyType, rb_type: RigidBodyType,
mprops_flags: RigidBodyMassPropsFlags, mprops_flags: LockedAxes,
/// The additional mass properties of the rigid-body being built. See [`RigidBodyBuilder::additional_mass_properties`] for more information. /// The additional mass properties of the rigid-body being built. See [`RigidBodyBuilder::additional_mass_properties`] for more information.
pub mass_properties: MassProperties, pub mass_properties: MassProperties,
/// Whether or not the rigid-body to be created can sleep if it reaches a dynamic equilibrium. /// Whether or not the rigid-body to be created can sleep if it reaches a dynamic equilibrium.
@@ -800,7 +914,7 @@ impl RigidBodyBuilder {
linear_damping: 0.0, linear_damping: 0.0,
angular_damping: 0.0, angular_damping: 0.0,
rb_type, rb_type,
mprops_flags: RigidBodyMassPropsFlags::empty(), mprops_flags: LockedAxes::empty(),
mass_properties: MassProperties::zero(), mass_properties: MassProperties::zero(),
can_sleep: true, can_sleep: true,
sleeping: false, sleeping: false,
@@ -881,10 +995,14 @@ impl RigidBodyBuilder {
self self
} }
pub fn locked_axes(mut self, locked_axes: LockedAxes) -> Self {
self.mprops_flags = locked_axes;
self
}
/// Prevents this rigid-body from translating because of forces. /// Prevents this rigid-body from translating because of forces.
pub fn lock_translations(mut self) -> Self { pub fn lock_translations(mut self) -> Self {
self.mprops_flags self.mprops_flags.set(LockedAxes::TRANSLATION_LOCKED, true);
.set(RigidBodyMassPropsFlags::TRANSLATION_LOCKED, true);
self self
} }
@@ -895,30 +1013,21 @@ impl RigidBodyBuilder {
allow_translations_y: bool, allow_translations_y: bool,
#[cfg(feature = "dim3")] allow_translations_z: bool, #[cfg(feature = "dim3")] allow_translations_z: bool,
) -> Self { ) -> Self {
self.mprops_flags.set( self.mprops_flags
RigidBodyMassPropsFlags::TRANSLATION_LOCKED_X, .set(LockedAxes::TRANSLATION_LOCKED_X, !allow_translations_x);
!allow_translations_x, self.mprops_flags
); .set(LockedAxes::TRANSLATION_LOCKED_Y, !allow_translations_y);
self.mprops_flags.set(
RigidBodyMassPropsFlags::TRANSLATION_LOCKED_Y,
!allow_translations_y,
);
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
self.mprops_flags.set( self.mprops_flags
RigidBodyMassPropsFlags::TRANSLATION_LOCKED_Z, .set(LockedAxes::TRANSLATION_LOCKED_Z, !allow_translations_z);
!allow_translations_z,
);
self self
} }
/// Prevents this rigid-body from rotating because of forces. /// Prevents this rigid-body from rotating because of forces.
pub fn lock_rotations(mut self) -> Self { pub fn lock_rotations(mut self) -> Self {
self.mprops_flags self.mprops_flags.set(LockedAxes::ROTATION_LOCKED_X, true);
.set(RigidBodyMassPropsFlags::ROTATION_LOCKED_X, true); self.mprops_flags.set(LockedAxes::ROTATION_LOCKED_Y, true);
self.mprops_flags self.mprops_flags.set(LockedAxes::ROTATION_LOCKED_Z, true);
.set(RigidBodyMassPropsFlags::ROTATION_LOCKED_Y, true);
self.mprops_flags
.set(RigidBodyMassPropsFlags::ROTATION_LOCKED_Z, true);
self self
} }
@@ -930,18 +1039,12 @@ impl RigidBodyBuilder {
allow_rotations_y: bool, allow_rotations_y: bool,
allow_rotations_z: bool, allow_rotations_z: bool,
) -> Self { ) -> Self {
self.mprops_flags.set( self.mprops_flags
RigidBodyMassPropsFlags::ROTATION_LOCKED_X, .set(LockedAxes::ROTATION_LOCKED_X, !allow_rotations_x);
!allow_rotations_x, self.mprops_flags
); .set(LockedAxes::ROTATION_LOCKED_Y, !allow_rotations_y);
self.mprops_flags.set( self.mprops_flags
RigidBodyMassPropsFlags::ROTATION_LOCKED_Y, .set(LockedAxes::ROTATION_LOCKED_Z, !allow_rotations_z);
!allow_rotations_y,
);
self.mprops_flags.set(
RigidBodyMassPropsFlags::ROTATION_LOCKED_Z,
!allow_rotations_z,
);
self self
} }

65
src/dynamics/rigid_body_components.rs
View File

@@ -202,7 +202,8 @@ where
bitflags::bitflags! { bitflags::bitflags! {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// Flags affecting the behavior of the constraints solver for a given contact manifold. /// Flags affecting the behavior of the constraints solver for a given contact manifold.
pub struct RigidBodyMassPropsFlags: u8 { // FIXME: rename this to LockedAxes
pub struct LockedAxes: u8 {
/// Flag indicating that the rigid-body cannot translate along the `X` axis. /// Flag indicating that the rigid-body cannot translate along the `X` axis.
const TRANSLATION_LOCKED_X = 1 << 0; const TRANSLATION_LOCKED_X = 1 << 0;
/// Flag indicating that the rigid-body cannot translate along the `Y` axis. /// Flag indicating that the rigid-body cannot translate along the `Y` axis.
@@ -228,7 +229,7 @@ bitflags::bitflags! {
/// The mass properties of this rigid-bodies. /// The mass properties of this rigid-bodies.
pub struct RigidBodyMassProps { pub struct RigidBodyMassProps {
/// Flags for locking rotation and translation. /// Flags for locking rotation and translation.
pub flags: RigidBodyMassPropsFlags, pub flags: LockedAxes,
/// The local mass properties of the rigid-body. /// The local mass properties of the rigid-body.
pub local_mprops: MassProperties, pub local_mprops: MassProperties,
/// The world-space center of mass of the rigid-body. /// The world-space center of mass of the rigid-body.
@@ -243,7 +244,7 @@ pub struct RigidBodyMassProps {
impl Default for RigidBodyMassProps { impl Default for RigidBodyMassProps {
fn default() -> Self { fn default() -> Self {
Self { Self {
flags: RigidBodyMassPropsFlags::empty(), flags: LockedAxes::empty(),
local_mprops: MassProperties::zero(), local_mprops: MassProperties::zero(),
world_com: Point::origin(), world_com: Point::origin(),
effective_inv_mass: Vector::zero(), effective_inv_mass: Vector::zero(),
@@ -252,8 +253,8 @@ impl Default for RigidBodyMassProps {
} }
} }
impl From<RigidBodyMassPropsFlags> for RigidBodyMassProps { impl From<LockedAxes> for RigidBodyMassProps {
fn from(flags: RigidBodyMassPropsFlags) -> Self { fn from(flags: LockedAxes) -> Self {
Self { Self {
flags, flags,
..Self::default() ..Self::default()
@@ -299,60 +300,39 @@ impl RigidBodyMassProps {
self.local_mprops.world_inv_inertia_sqrt(&position.rotation); self.local_mprops.world_inv_inertia_sqrt(&position.rotation);
// Take into account translation/rotation locking. // Take into account translation/rotation locking.
if self if self.flags.contains(LockedAxes::TRANSLATION_LOCKED_X) {
.flags
.contains(RigidBodyMassPropsFlags::TRANSLATION_LOCKED_X)
{
self.effective_inv_mass.x = 0.0; self.effective_inv_mass.x = 0.0;
} }
if self if self.flags.contains(LockedAxes::TRANSLATION_LOCKED_Y) {
.flags
.contains(RigidBodyMassPropsFlags::TRANSLATION_LOCKED_Y)
{
self.effective_inv_mass.y = 0.0; self.effective_inv_mass.y = 0.0;
} }
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
if self if self.flags.contains(LockedAxes::TRANSLATION_LOCKED_Z) {
.flags
.contains(RigidBodyMassPropsFlags::TRANSLATION_LOCKED_Z)
{
self.effective_inv_mass.z = 0.0; self.effective_inv_mass.z = 0.0;
} }
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
{ {
if self if self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) {
.flags
.contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_Z)
{
self.effective_world_inv_inertia_sqrt = 0.0; self.effective_world_inv_inertia_sqrt = 0.0;
} }
} }
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
{ {
if self if self.flags.contains(LockedAxes::ROTATION_LOCKED_X) {
.flags
.contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_X)
{
self.effective_world_inv_inertia_sqrt.m11 = 0.0; self.effective_world_inv_inertia_sqrt.m11 = 0.0;
self.effective_world_inv_inertia_sqrt.m12 = 0.0; self.effective_world_inv_inertia_sqrt.m12 = 0.0;
self.effective_world_inv_inertia_sqrt.m13 = 0.0; self.effective_world_inv_inertia_sqrt.m13 = 0.0;
} }
if self if self.flags.contains(LockedAxes::ROTATION_LOCKED_Y) {
.flags
.contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_Y)
{
self.effective_world_inv_inertia_sqrt.m22 = 0.0; self.effective_world_inv_inertia_sqrt.m22 = 0.0;
self.effective_world_inv_inertia_sqrt.m12 = 0.0; self.effective_world_inv_inertia_sqrt.m12 = 0.0;
self.effective_world_inv_inertia_sqrt.m23 = 0.0; self.effective_world_inv_inertia_sqrt.m23 = 0.0;
} }
if self if self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) {
.flags
.contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_Z)
{
self.effective_world_inv_inertia_sqrt.m33 = 0.0; self.effective_world_inv_inertia_sqrt.m33 = 0.0;
self.effective_world_inv_inertia_sqrt.m13 = 0.0; self.effective_world_inv_inertia_sqrt.m13 = 0.0;
self.effective_world_inv_inertia_sqrt.m23 = 0.0; self.effective_world_inv_inertia_sqrt.m23 = 0.0;
@@ -659,6 +639,10 @@ pub struct RigidBodyForces {
/// Gravity is multiplied by this scaling factor before it's /// Gravity is multiplied by this scaling factor before it's
/// applied to this rigid-body. /// applied to this rigid-body.
pub gravity_scale: Real, pub gravity_scale: Real,
// Forces applied by the user.
pub user_force: Vector<Real>,
// Torque applied by the user.
pub user_torque: AngVector<Real>,
} }
impl Default for RigidBodyForces { impl Default for RigidBodyForces {
@@ -667,6 +651,8 @@ impl Default for RigidBodyForces {
force: na::zero(), force: na::zero(),
torque: na::zero(), torque: na::zero(),
gravity_scale: 1.0, gravity_scale: 1.0,
user_force: na::zero(),
user_torque: na::zero(),
} }
} }
} }
@@ -692,8 +678,13 @@ impl RigidBodyForces {
/// Adds to `self` the gravitational force that would result in a gravitational acceleration /// Adds to `self` the gravitational force that would result in a gravitational acceleration
/// equal to `gravity`. /// equal to `gravity`.
pub fn add_gravity_acceleration(&mut self, gravity: &Vector<Real>, mass: &Vector<Real>) { pub fn compute_effective_force_and_torque(
self.force += gravity.component_mul(&mass) * self.gravity_scale; &mut self,
gravity: &Vector<Real>,
mass: &Vector<Real>,
) {
self.force = self.user_force + gravity.component_mul(&mass) * self.gravity_scale;
self.torque = self.user_torque;
} }
/// Applies a force at the given world-space point of the rigid-body with the given mass properties. /// Applies a force at the given world-space point of the rigid-body with the given mass properties.
@@ -703,8 +694,8 @@ impl RigidBodyForces {
force: Vector<Real>, force: Vector<Real>,
point: Point<Real>, point: Point<Real>,
) { ) {
self.force += force; self.user_force += force;
self.torque += (point - rb_mprops.world_com).gcross(force); self.user_torque += (point - rb_mprops.world_com).gcross(force);
} }
} }

22
src/pipeline/physics_pipeline.rs
View File

@@ -224,7 +224,7 @@ impl PhysicsPipeline {
}) })
.unwrap(); .unwrap();
bodies.map_mut_internal(handle.0, |forces: &mut RigidBodyForces| { bodies.map_mut_internal(handle.0, |forces: &mut RigidBodyForces| {
forces.add_gravity_acceleration(&gravity, &effective_inv_mass) forces.compute_effective_force_and_torque(&gravity, &effective_inv_mass)
}); });
} }
@@ -355,7 +355,6 @@ impl PhysicsPipeline {
bodies: &mut Bodies, bodies: &mut Bodies,
colliders: &mut Colliders, colliders: &mut Colliders,
modified_colliders: &mut Vec<ColliderHandle>, modified_colliders: &mut Vec<ColliderHandle>,
clear_forces: bool,
) where ) where
Bodies: ComponentSetMut<RigidBodyVelocity> Bodies: ComponentSetMut<RigidBodyVelocity>
+ ComponentSetMut<RigidBodyForces> + ComponentSetMut<RigidBodyForces>
@@ -369,16 +368,6 @@ impl PhysicsPipeline {
// Set the rigid-bodies and kinematic bodies to their final position. // Set the rigid-bodies and kinematic bodies to their final position.
for handle in islands.iter_active_bodies() { for handle in islands.iter_active_bodies() {
let status: &RigidBodyType = bodies.index(handle.0); let status: &RigidBodyType = bodies.index(handle.0);
if status.is_kinematic() {
bodies.set_internal(handle.0, RigidBodyVelocity::zero());
}
if clear_forces {
bodies.map_mut_internal(handle.0, |f: &mut RigidBodyForces| {
f.torque = na::zero();
f.force = na::zero();
});
}
bodies.map_mut_internal(handle.0, |poss: &mut RigidBodyPosition| { bodies.map_mut_internal(handle.0, |poss: &mut RigidBodyPosition| {
poss.position = poss.next_position poss.position = poss.next_position
@@ -666,14 +655,7 @@ impl PhysicsPipeline {
} }
} }
let clear_forces = remaining_substeps == 0; self.advance_to_final_positions(islands, bodies, colliders, modified_colliders);
self.advance_to_final_positions(
islands,
bodies,
colliders,
modified_colliders,
clear_forces,
);
self.detect_collisions( self.detect_collisions(
&integration_parameters, &integration_parameters,