use crate::dynamics::MassProperties;
use crate::geometry::{
Collider, ColliderHandle, ColliderSet, InteractionGraph, RigidBodyGraphIndex,
};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector,
};
use crate::utils::{self, WCross, WDot};
use num::Zero;
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub enum BodyStatus {
Dynamic,
Static,
Kinematic,
}
bitflags::bitflags! {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub(crate) struct RigidBodyFlags: u8 {
const TRANSLATION_LOCKED = 1 << 0;
const ROTATION_LOCKED_X = 1 << 1;
const ROTATION_LOCKED_Y = 1 << 2;
const ROTATION_LOCKED_Z = 1 << 3;
}
}
bitflags::bitflags! {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub(crate) struct RigidBodyChanges: u32 {
const MODIFIED = 1 << 0;
const POSITION = 1 << 1;
const SLEEP = 1 << 2;
const COLLIDERS = 1 << 3;
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Debug, Clone)]
pub struct RigidBody {
pub(crate) position: Isometry<Real>,
pub(crate) predicted_position: Isometry<Real>,
pub(crate) mass_properties: MassProperties,
pub world_com: Point<Real>,
pub effective_inv_mass: Real,
pub effective_world_inv_inertia_sqrt: AngularInertia<Real>,
pub(crate) linvel: Vector<Real>,
pub(crate) angvel: AngVector<Real>,
pub linear_damping: Real,
pub angular_damping: Real,
pub(crate) linacc: Vector<Real>,
pub(crate) angacc: AngVector<Real>,
pub(crate) colliders: Vec<ColliderHandle>,
pub(crate) gravity_scale: Real,
pub activation: ActivationStatus,
pub(crate) joint_graph_index: RigidBodyGraphIndex,
pub(crate) active_island_id: usize,
pub(crate) active_set_id: usize,
pub(crate) active_set_offset: usize,
pub(crate) active_set_timestamp: u32,
flags: RigidBodyFlags,
pub(crate) changes: RigidBodyChanges,
pub body_status: BodyStatus,
pub user_data: u128,
}
impl RigidBody {
fn new() -> Self {
Self {
position: Isometry::identity(),
predicted_position: Isometry::identity(),
mass_properties: MassProperties::zero(),
world_com: Point::origin(),
effective_inv_mass: 0.0,
effective_world_inv_inertia_sqrt: AngularInertia::zero(),
linvel: Vector::zeros(),
angvel: na::zero(),
linacc: Vector::zeros(),
angacc: na::zero(),
gravity_scale: 1.0,
linear_damping: 0.0,
angular_damping: 0.0,
colliders: Vec::new(),
activation: ActivationStatus::new_active(),
joint_graph_index: InteractionGraph::<(), ()>::invalid_graph_index(),
active_island_id: 0,
active_set_id: 0,
active_set_offset: 0,
active_set_timestamp: 0,
flags: RigidBodyFlags::empty(),
changes: RigidBodyChanges::all(),
body_status: BodyStatus::Dynamic,
user_data: 0,
}
}
pub(crate) fn reset_internal_references(&mut self) {
self.colliders = Vec::new();
self.joint_graph_index = InteractionGraph::<(), ()>::invalid_graph_index();
self.active_island_id = 0;
self.active_set_id = 0;
self.active_set_offset = 0;
self.active_set_timestamp = 0;
}
pub(crate) fn integrate_accelerations(&mut self, dt: Real, gravity: Vector<Real>) {
if self.effective_inv_mass != 0.0 {
self.linvel += (gravity * self.gravity_scale + self.linacc) * dt;
self.linacc = na::zero();
}
self.angvel += self.angacc * dt;
self.angacc = na::zero();
}
#[inline]
pub fn mass_properties(&self) -> &MassProperties {
&self.mass_properties
}
#[inline]
pub fn set_mass_properties(&mut self, props: MassProperties, wake_up: bool) {
if self.is_dynamic() && wake_up {
self.wake_up(true);
}
self.mass_properties = props;
}
pub fn colliders(&self) -> &[ColliderHandle] {
&self.colliders[..]
}
pub fn is_dynamic(&self) -> bool {
self.body_status == BodyStatus::Dynamic
}
pub fn is_kinematic(&self) -> bool {
self.body_status == BodyStatus::Kinematic
}
pub fn is_static(&self) -> bool {
self.body_status == BodyStatus::Static
}
pub fn mass(&self) -> Real {
utils::inv(self.mass_properties.inv_mass)
}
pub fn predicted_position(&self) -> &Isometry<Real> {
&self.predicted_position
}
pub fn gravity_scale(&self) -> Real {
self.gravity_scale
}
pub fn set_gravity_scale(&mut self, scale: Real, wake_up: bool) {
if wake_up && self.activation.sleeping {
self.changes.insert(RigidBodyChanges::SLEEP);
self.activation.sleeping = false;
}
self.gravity_scale = scale;
}
pub(crate) fn add_collider(&mut self, handle: ColliderHandle, coll: &Collider) {
self.changes.set(
RigidBodyChanges::MODIFIED | RigidBodyChanges::COLLIDERS,
true,
);
let mass_properties = coll
.mass_properties()
.transform_by(coll.position_wrt_parent());
self.colliders.push(handle);
self.mass_properties += mass_properties;
self.update_world_mass_properties();
}
pub(crate) fn update_colliders_positions(&mut self, colliders: &mut ColliderSet) {
for handle in &self.colliders {
let collider = &mut colliders[*handle];
collider.position = self.position * collider.delta;
collider.predicted_position = self.predicted_position * collider.delta;
}
}
pub(crate) fn remove_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
if let Some(i) = self.colliders.iter().position(|e| *e == handle) {
self.changes.set(RigidBodyChanges::COLLIDERS, true);
self.colliders.swap_remove(i);
let mass_properties = coll
.mass_properties()
.transform_by(coll.position_wrt_parent());
self.mass_properties -= mass_properties;
self.update_world_mass_properties();
}
}
pub fn sleep(&mut self) {
self.activation.energy = 0.0;
self.activation.sleeping = true;
self.linvel = na::zero();
self.angvel = na::zero();
}
pub fn wake_up(&mut self, strong: bool) {
if self.activation.sleeping {
self.changes.insert(RigidBodyChanges::SLEEP);
self.activation.sleeping = false;
}
if (strong || self.activation.energy == 0.0) && self.is_dynamic() {
self.activation.energy = self.activation.threshold.abs() * 2.0;
}
}
pub(crate) fn update_energy(&mut self) {
let mix_factor = 0.01;
let new_energy = (1.0 - mix_factor) * self.activation.energy
+ mix_factor * (self.linvel.norm_squared() + self.angvel.gdot(self.angvel));
self.activation.energy = new_energy.min(self.activation.threshold.abs() * 4.0);
}
pub fn is_sleeping(&self) -> bool {
self.activation.sleeping
}
pub fn is_moving(&self) -> bool {
!self.linvel.is_zero() || !self.angvel.is_zero()
}
fn integrate_velocity(&self, dt: Real) -> Isometry<Real> {
let com = &self.position * self.mass_properties.local_com;
let shift = Translation::from(com.coords);
shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse()
}
pub(crate) fn integrate(&mut self, dt: Real) {
self.linvel *= 1.0 / (1.0 + dt * self.linear_damping);
self.angvel *= 1.0 / (1.0 + dt * self.angular_damping);
self.position = self.integrate_velocity(dt) * self.position;
}
pub fn linvel(&self) -> &Vector<Real> {
&self.linvel
}
#[cfg(feature = "dim2")]
pub fn angvel(&self) -> Real {
self.angvel
}
#[cfg(feature = "dim3")]
pub fn angvel(&self) -> &Vector<Real> {
&self.angvel
}
pub fn set_linvel(&mut self, linvel: Vector<Real>, wake_up: bool) {
self.linvel = linvel;
if self.is_dynamic() && wake_up {
self.wake_up(true)
}
}
#[cfg(feature = "dim2")]
pub fn set_angvel(&mut self, angvel: Real, wake_up: bool) {
self.angvel = angvel;
if self.is_dynamic() && wake_up {
self.wake_up(true)
}
}
#[cfg(feature = "dim3")]
pub fn set_angvel(&mut self, angvel: Vector<Real>, wake_up: bool) {
self.angvel = angvel;
if self.is_dynamic() && wake_up {
self.wake_up(true)
}
}
pub fn position(&self) -> &Isometry<Real> {
&self.position
}
pub fn set_position(&mut self, pos: Isometry<Real>, wake_up: bool) {
self.changes.insert(RigidBodyChanges::POSITION);
self.set_position_internal(pos);
if wake_up && self.is_dynamic() {
self.wake_up(true)
}
}
pub(crate) fn set_position_internal(&mut self, pos: Isometry<Real>) {
self.position = pos;
if self.is_static() || self.is_kinematic() {
self.predicted_position = pos;
}
}
pub fn set_next_kinematic_position(&mut self, pos: Isometry<Real>) {
if self.is_kinematic() {
self.predicted_position = pos;
}
}
pub(crate) fn compute_velocity_from_predicted_position(&mut self, inv_dt: Real) {
let dpos = self.predicted_position * self.position.inverse();
#[cfg(feature = "dim2")]
{
self.angvel = dpos.rotation.angle() * inv_dt;
}
#[cfg(feature = "dim3")]
{
self.angvel = dpos.rotation.scaled_axis() * inv_dt;
}
self.linvel = dpos.translation.vector * inv_dt;
}
pub(crate) fn update_predicted_position(&mut self, dt: Real) {
self.predicted_position = self.integrate_velocity(dt) * self.position;
}
pub(crate) fn update_world_mass_properties(&mut self) {
self.world_com = self.mass_properties.world_com(&self.position);
self.effective_inv_mass = self.mass_properties.inv_mass;
self.effective_world_inv_inertia_sqrt = self
.mass_properties
.world_inv_inertia_sqrt(&self.position.rotation);
if self.flags.contains(RigidBodyFlags::TRANSLATION_LOCKED) {
self.effective_inv_mass = 0.0;
}
#[cfg(feature = "dim2")]
{
if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Z) {
self.effective_world_inv_inertia_sqrt = 0.0;
}
}
#[cfg(feature = "dim3")]
{
if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_X) {
self.effective_world_inv_inertia_sqrt.m11 = 0.0;
self.effective_world_inv_inertia_sqrt.m12 = 0.0;
self.effective_world_inv_inertia_sqrt.m13 = 0.0;
}
if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Y) {
self.effective_world_inv_inertia_sqrt.m22 = 0.0;
self.effective_world_inv_inertia_sqrt.m12 = 0.0;
self.effective_world_inv_inertia_sqrt.m23 = 0.0;
}
if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Z) {
self.effective_world_inv_inertia_sqrt.m33 = 0.0;
self.effective_world_inv_inertia_sqrt.m13 = 0.0;
self.effective_world_inv_inertia_sqrt.m23 = 0.0;
}
}
}
pub fn apply_force(&mut self, force: Vector<Real>, wake_up: bool) {
if self.body_status == BodyStatus::Dynamic {
self.linacc += force * self.effective_inv_mass;
if wake_up {
self.wake_up(true);
}
}
}
pub fn apply_impulse(&mut self, impulse: Vector<Real>, wake_up: bool) {
if self.body_status == BodyStatus::Dynamic {
self.linvel += impulse * self.effective_inv_mass;
if wake_up {
self.wake_up(true);
}
}
}
#[cfg(feature = "dim2")]
pub fn apply_torque(&mut self, torque: Real, wake_up: bool) {
if self.body_status == BodyStatus::Dynamic {
self.angacc += self.effective_world_inv_inertia_sqrt
* (self.effective_world_inv_inertia_sqrt * torque);
if wake_up {
self.wake_up(true);
}
}
}
#[cfg(feature = "dim3")]
pub fn apply_torque(&mut self, torque: Vector<Real>, wake_up: bool) {
if self.body_status == BodyStatus::Dynamic {
self.angacc += self.effective_world_inv_inertia_sqrt
* (self.effective_world_inv_inertia_sqrt * torque);
if wake_up {
self.wake_up(true);
}
}
}
#[cfg(feature = "dim2")]
pub fn apply_torque_impulse(&mut self, torque_impulse: Real, wake_up: bool) {
if self.body_status == BodyStatus::Dynamic {
self.angvel += self.effective_world_inv_inertia_sqrt
* (self.effective_world_inv_inertia_sqrt * torque_impulse);
if wake_up {
self.wake_up(true);
}
}
}
#[cfg(feature = "dim3")]
pub fn apply_torque_impulse(&mut self, torque_impulse: Vector<Real>, wake_up: bool) {
if self.body_status == BodyStatus::Dynamic {
self.angvel += self.effective_world_inv_inertia_sqrt
* (self.effective_world_inv_inertia_sqrt * torque_impulse);
if wake_up {
self.wake_up(true);
}
}
}
pub fn apply_force_at_point(&mut self, force: Vector<Real>, point: Point<Real>, wake_up: bool) {
let torque = (point - self.world_com).gcross(force);
self.apply_force(force, wake_up);
self.apply_torque(torque, wake_up);
}
pub fn apply_impulse_at_point(
&mut self,
impulse: Vector<Real>,
point: Point<Real>,
wake_up: bool,
) {
let torque_impulse = (point - self.world_com).gcross(impulse);
self.apply_impulse(impulse, wake_up);
self.apply_torque_impulse(torque_impulse, wake_up);
}
pub fn velocity_at_point(&self, point: &Point<Real>) -> Vector<Real> {
let dpt = point - self.world_com;
self.linvel + self.angvel.gcross(dpt)
}
}
pub struct RigidBodyBuilder {
position: Isometry<Real>,
linvel: Vector<Real>,
angvel: AngVector<Real>,
gravity_scale: Real,
linear_damping: Real,
angular_damping: Real,
body_status: BodyStatus,
flags: RigidBodyFlags,
mass_properties: MassProperties,
can_sleep: bool,
sleeping: bool,
user_data: u128,
}
impl RigidBodyBuilder {
pub fn new(body_status: BodyStatus) -> Self {
Self {
position: Isometry::identity(),
linvel: Vector::zeros(),
angvel: na::zero(),
gravity_scale: 1.0,
linear_damping: 0.0,
angular_damping: 0.0,
body_status,
flags: RigidBodyFlags::empty(),
mass_properties: MassProperties::zero(),
can_sleep: true,
sleeping: false,
user_data: 0,
}
}
pub fn new_static() -> Self {
Self::new(BodyStatus::Static)
}
pub fn new_kinematic() -> Self {
Self::new(BodyStatus::Kinematic)
}
pub fn new_dynamic() -> Self {
Self::new(BodyStatus::Dynamic)
}
pub fn gravity_scale(mut self, x: Real) -> Self {
self.gravity_scale = x;
self
}
#[cfg(feature = "dim2")]
pub fn translation(mut self, x: Real, y: Real) -> Self {
self.position.translation.x = x;
self.position.translation.y = y;
self
}
#[cfg(feature = "dim3")]
pub fn translation(mut self, x: Real, y: Real, z: Real) -> Self {
self.position.translation.x = x;
self.position.translation.y = y;
self.position.translation.z = z;
self
}
pub fn rotation(mut self, angle: AngVector<Real>) -> Self {
self.position.rotation = Rotation::new(angle);
self
}
pub fn position(mut self, pos: Isometry<Real>) -> Self {
self.position = pos;
self
}
pub fn user_data(mut self, data: u128) -> Self {
self.user_data = data;
self
}
pub fn mass_properties(mut self, props: MassProperties) -> Self {
self.mass_properties = props;
self
}
pub fn lock_translations(mut self) -> Self {
self.flags.set(RigidBodyFlags::TRANSLATION_LOCKED, true);
self
}
pub fn lock_rotations(mut self) -> Self {
self.flags.set(RigidBodyFlags::ROTATION_LOCKED_X, true);
self.flags.set(RigidBodyFlags::ROTATION_LOCKED_Y, true);
self.flags.set(RigidBodyFlags::ROTATION_LOCKED_Z, true);
self
}
#[cfg(feature = "dim3")]
pub fn restrict_rotations(
mut self,
allow_rotations_x: bool,
allow_rotations_y: bool,
allow_rotations_z: bool,
) -> Self {
self.flags
.set(RigidBodyFlags::ROTATION_LOCKED_X, !allow_rotations_x);
self.flags
.set(RigidBodyFlags::ROTATION_LOCKED_Y, !allow_rotations_y);
self.flags
.set(RigidBodyFlags::ROTATION_LOCKED_Z, !allow_rotations_z);
self
}
pub fn mass(mut self, mass: Real) -> Self {
self.mass_properties.inv_mass = utils::inv(mass);
self
}
#[cfg(feature = "dim2")]
pub fn principal_angular_inertia(mut self, inertia: Real) -> Self {
self.mass_properties.inv_principal_inertia_sqrt = utils::inv(inertia);
self
}
#[cfg(feature = "dim2")]
#[deprecated(note = "renamed to `principal_angular_inertia`.")]
pub fn principal_inertia(self, inertia: Real) -> Self {
self.principal_angular_inertia(inertia)
}
#[cfg(feature = "dim3")]
pub fn principal_angular_inertia(mut self, inertia: AngVector<Real>) -> Self {
self.mass_properties.inv_principal_inertia_sqrt = inertia.map(utils::inv);
self
}
#[cfg(feature = "dim3")]
#[deprecated(note = "renamed to `principal_angular_inertia`.")]
pub fn principal_inertia(self, inertia: AngVector<Real>) -> Self {
self.principal_angular_inertia(inertia)
}
pub fn linear_damping(mut self, factor: Real) -> Self {
self.linear_damping = factor;
self
}
pub fn angular_damping(mut self, factor: Real) -> Self {
self.angular_damping = factor;
self
}
#[cfg(feature = "dim2")]
pub fn linvel(mut self, x: Real, y: Real) -> Self {
self.linvel = Vector::new(x, y);
self
}
#[cfg(feature = "dim3")]
pub fn linvel(mut self, x: Real, y: Real, z: Real) -> Self {
self.linvel = Vector::new(x, y, z);
self
}
pub fn angvel(mut self, angvel: AngVector<Real>) -> Self {
self.angvel = angvel;
self
}
pub fn can_sleep(mut self, can_sleep: bool) -> Self {
self.can_sleep = can_sleep;
self
}
pub fn sleeping(mut self, sleeping: bool) -> Self {
self.sleeping = sleeping;
self
}
pub fn build(&self) -> RigidBody {
let mut rb = RigidBody::new();
rb.predicted_position = self.position;
rb.set_position_internal(self.position);
rb.linvel = self.linvel;
rb.angvel = self.angvel;
rb.body_status = self.body_status;
rb.user_data = self.user_data;
rb.mass_properties = self.mass_properties;
rb.linear_damping = self.linear_damping;
rb.angular_damping = self.angular_damping;
rb.gravity_scale = self.gravity_scale;
rb.flags = self.flags;
if self.can_sleep && self.sleeping {
rb.sleep();
}
if !self.can_sleep {
rb.activation.threshold = -1.0;
}
rb
}
}
#[derive(Copy, Clone, Debug)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct ActivationStatus {
pub threshold: Real,
pub energy: Real,
pub sleeping: bool,
}
impl ActivationStatus {
pub fn default_threshold() -> Real {
0.01
}
pub fn new_active() -> Self {
ActivationStatus {
threshold: Self::default_threshold(),
energy: Self::default_threshold() * 4.0,
sleeping: false,
}
}
pub fn new_inactive() -> Self {
ActivationStatus {
threshold: Self::default_threshold(),
energy: 0.0,
sleeping: true,
}
}
#[inline]
pub fn is_active(&self) -> bool {
self.energy != 0.0
}
}