use crate::dynamics::MassProperties;
use crate::geometry::{
Collider, ColliderHandle, ColliderSet, InteractionGraph, RigidBodyGraphIndex,
};
use crate::math::{AngVector, AngularInertia, Isometry, Point, 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 IGNORE_COLLIDER_MASS = 1 << 0;
const IGNORE_COLLIDER_ANGULAR_INERTIA_X = 1 << 1;
const IGNORE_COLLIDER_ANGULAR_INERTIA_Y = 1 << 2;
const IGNORE_COLLIDER_ANGULAR_INERTIA_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<f32>,
pub(crate) predicted_position: Isometry<f32>,
pub(crate) mass_properties: MassProperties,
pub world_com: Point<f32>,
pub world_inv_inertia_sqrt: AngularInertia<f32>,
pub(crate) linvel: Vector<f32>,
pub(crate) angvel: AngVector<f32>,
pub linear_damping: f32,
pub angular_damping: f32,
pub(crate) linacc: Vector<f32>,
pub(crate) angacc: AngVector<f32>,
pub(crate) colliders: Vec<ColliderHandle>,
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(),
world_inv_inertia_sqrt: AngularInertia::zero(),
linvel: Vector::zeros(),
angvel: na::zero(),
linacc: Vector::zeros(),
angacc: na::zero(),
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: f32, gravity: Vector<f32>) {
if self.mass_properties.inv_mass != 0.0 {
self.linvel += (gravity + self.linacc) * dt;
self.angvel += self.angacc * dt;
self.linacc = na::zero();
self.angacc = na::zero();
}
}
#[inline]
pub fn mass_properties(&self) -> &MassProperties {
&self.mass_properties
}
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) -> f32 {
utils::inv(self.mass_properties.inv_mass)
}
pub fn predicted_position(&self) -> &Isometry<f32> {
&self.predicted_position
}
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 += Self::filter_collider_mass_props(mass_properties, self.flags);
self.update_world_mass_properties();
}
fn filter_collider_mass_props(
mut props: MassProperties,
flags: RigidBodyFlags,
) -> MassProperties {
if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_MASS) {
props.inv_mass = 0.0;
}
#[cfg(feature = "dim2")]
{
if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z) {
props.inv_principal_inertia_sqrt = 0.0;
}
}
#[cfg(feature = "dim3")]
{
if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_X) {
props.inv_principal_inertia_sqrt.x = 0.0;
}
if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Y) {
props.inv_principal_inertia_sqrt.y = 0.0;
}
if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z) {
props.inv_principal_inertia_sqrt.z = 0.0;
}
}
props
}
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 -= Self::filter_collider_mass_props(mass_properties, self.flags);
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: f32) -> Isometry<f32> {
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: f32) {
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<f32> {
&self.linvel
}
#[cfg(feature = "dim2")]
pub fn angvel(&self) -> f32 {
self.angvel
}
#[cfg(feature = "dim3")]
pub fn angvel(&self) -> &Vector<f32> {
&self.angvel
}
pub fn set_linvel(&mut self, linvel: Vector<f32>, 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: f32, 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<f32>, wake_up: bool) {
self.angvel = angvel;
if self.is_dynamic() && wake_up {
self.wake_up(true)
}
}
pub fn position(&self) -> &Isometry<f32> {
&self.position
}
pub fn set_position(&mut self, pos: Isometry<f32>, 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<f32>) {
self.position = pos;
if self.is_static() || self.is_kinematic() {
self.predicted_position = pos;
}
}
pub fn set_next_kinematic_position(&mut self, pos: Isometry<f32>) {
if self.is_kinematic() {
self.predicted_position = pos;
}
}
pub(crate) fn compute_velocity_from_predicted_position(&mut self, inv_dt: f32) {
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: f32) {
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.world_inv_inertia_sqrt = self
.mass_properties
.world_inv_inertia_sqrt(&self.position.rotation);
}
pub fn apply_force(&mut self, force: Vector<f32>, wake_up: bool) {
if self.body_status == BodyStatus::Dynamic {
self.linacc += force * self.mass_properties.inv_mass;
if wake_up {
self.wake_up(true);
}
}
}
pub fn apply_impulse(&mut self, impulse: Vector<f32>, wake_up: bool) {
if self.body_status == BodyStatus::Dynamic {
self.linvel += impulse * self.mass_properties.inv_mass;
if wake_up {
self.wake_up(true);
}
}
}
#[cfg(feature = "dim2")]
pub fn apply_torque(&mut self, torque: f32, wake_up: bool) {
if self.body_status == BodyStatus::Dynamic {
self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque);
if wake_up {
self.wake_up(true);
}
}
}
#[cfg(feature = "dim3")]
pub fn apply_torque(&mut self, torque: Vector<f32>, wake_up: bool) {
if self.body_status == BodyStatus::Dynamic {
self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque);
if wake_up {
self.wake_up(true);
}
}
}
#[cfg(feature = "dim2")]
pub fn apply_torque_impulse(&mut self, torque_impulse: f32, wake_up: bool) {
if self.body_status == BodyStatus::Dynamic {
self.angvel +=
self.world_inv_inertia_sqrt * (self.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<f32>, wake_up: bool) {
if self.body_status == BodyStatus::Dynamic {
self.angvel +=
self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse);
if wake_up {
self.wake_up(true);
}
}
}
pub fn apply_force_at_point(&mut self, force: Vector<f32>, point: Point<f32>, 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<f32>,
point: Point<f32>,
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<f32>) -> Vector<f32> {
let dpt = point - self.world_com;
self.linvel + self.angvel.gcross(dpt)
}
}
pub struct RigidBodyBuilder {
position: Isometry<f32>,
linvel: Vector<f32>,
angvel: AngVector<f32>,
linear_damping: f32,
angular_damping: f32,
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(),
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)
}
#[cfg(feature = "dim2")]
pub fn translation(mut self, x: f32, y: f32) -> Self {
self.position.translation.x = x;
self.position.translation.y = y;
self
}
#[cfg(feature = "dim3")]
pub fn translation(mut self, x: f32, y: f32, z: f32) -> Self {
self.position.translation.x = x;
self.position.translation.y = y;
self.position.translation.z = z;
self
}
pub fn rotation(mut self, angle: AngVector<f32>) -> Self {
self.position.rotation = Rotation::new(angle);
self
}
pub fn position(mut self, pos: Isometry<f32>) -> 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(self) -> Self {
self.mass(0.0, false)
}
pub fn lock_rotations(self) -> Self {
#[cfg(feature = "dim2")]
return self.principal_angular_inertia(0.0, false);
#[cfg(feature = "dim3")]
return self.principal_angular_inertia(Vector::zeros(), Vector::repeat(false));
}
pub fn mass(mut self, mass: f32, colliders_contribution_enabled: bool) -> Self {
self.mass_properties.inv_mass = utils::inv(mass);
self.flags.set(
RigidBodyFlags::IGNORE_COLLIDER_MASS,
!colliders_contribution_enabled,
);
self
}
#[cfg(feature = "dim2")]
pub fn principal_angular_inertia(
mut self,
inertia: f32,
colliders_contribution_enabled: bool,
) -> Self {
self.mass_properties.inv_principal_inertia_sqrt = utils::inv(inertia);
self.flags.set(
RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_X
| RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Y
| RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z,
!colliders_contribution_enabled,
);
self
}
#[cfg(feature = "dim2")]
#[deprecated(note = "renamed to `principal_angular_inertia`.")]
pub fn principal_inertia(self, inertia: f32, colliders_contribution_enabled: bool) -> Self {
self.principal_angular_inertia(inertia, colliders_contribution_enabled)
}
#[cfg(feature = "dim3")]
pub fn principal_angular_inertia(
mut self,
inertia: AngVector<f32>,
colliders_contribution_enabled: AngVector<bool>,
) -> Self {
self.mass_properties.inv_principal_inertia_sqrt = inertia.map(utils::inv);
self.flags.set(
RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_X,
!colliders_contribution_enabled.x,
);
self.flags.set(
RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Y,
!colliders_contribution_enabled.y,
);
self.flags.set(
RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z,
!colliders_contribution_enabled.z,
);
self
}
#[cfg(feature = "dim3")]
#[deprecated(note = "renamed to `principal_angular_inertia`.")]
pub fn principal_inertia(
self,
inertia: AngVector<f32>,
colliders_contribution_enabled: AngVector<bool>,
) -> Self {
self.principal_angular_inertia(inertia, colliders_contribution_enabled)
}
pub fn linear_damping(mut self, factor: f32) -> Self {
self.linear_damping = factor;
self
}
pub fn angular_damping(mut self, factor: f32) -> Self {
self.angular_damping = factor;
self
}
#[cfg(feature = "dim2")]
pub fn linvel(mut self, x: f32, y: f32) -> Self {
self.linvel = Vector::new(x, y);
self
}
#[cfg(feature = "dim3")]
pub fn linvel(mut self, x: f32, y: f32, z: f32) -> Self {
self.linvel = Vector::new(x, y, z);
self
}
pub fn angvel(mut self, angvel: AngVector<f32>) -> 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.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: f32,
pub energy: f32,
pub sleeping: bool,
}
impl ActivationStatus {
pub fn default_threshold() -> f32 {
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
}
}