use crate::dynamics::MassProperties;
use crate::geometry::{Collider, ColliderHandle, InteractionGraph, RigidBodyGraphIndex};
use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Translation, Vector};
use crate::utils::{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,
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Debug)]
pub struct RigidBody {
pub position: Isometry<f32>,
pub(crate) predicted_position: Isometry<f32>,
pub mass_properties: MassProperties,
pub world_com: Point<f32>,
pub world_inv_inertia_sqrt: AngularInertia<f32>,
pub linvel: Vector<f32>,
pub angvel: AngVector<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,
pub body_status: BodyStatus,
pub user_data: u128,
}
impl Clone for RigidBody {
fn clone(&self) -> Self {
Self {
colliders: Vec::new(),
joint_graph_index: RigidBodyGraphIndex::new(crate::INVALID_U32),
active_island_id: crate::INVALID_USIZE,
active_set_id: crate::INVALID_USIZE,
active_set_offset: crate::INVALID_USIZE,
active_set_timestamp: crate::INVALID_U32,
..*self
}
}
}
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(),
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,
body_status: BodyStatus::Dynamic,
user_data: 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();
}
}
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 {
crate::utils::inv(self.mass_properties.inv_mass)
}
pub fn predicted_position(&self) -> &Isometry<f32> {
&self.predicted_position
}
pub(crate) fn add_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
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 remove_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
if let Some(i) = self.colliders.iter().position(|e| *e == handle) {
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) {
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.position = self.integrate_velocity(dt) * self.position;
}
pub fn set_position(&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>) {
if self.body_status == BodyStatus::Dynamic {
self.linacc += force * self.mass_properties.inv_mass;
}
}
pub fn apply_impulse(&mut self, impulse: Vector<f32>) {
if self.body_status == BodyStatus::Dynamic {
self.linvel += impulse * self.mass_properties.inv_mass;
}
}
#[cfg(feature = "dim2")]
pub fn apply_torque(&mut self, torque: f32) {
if self.body_status == BodyStatus::Dynamic {
self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque);
}
}
#[cfg(feature = "dim3")]
pub fn apply_torque(&mut self, torque: Vector<f32>) {
if self.body_status == BodyStatus::Dynamic {
self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque);
}
}
#[cfg(feature = "dim2")]
pub fn apply_torque_impulse(&mut self, torque_impulse: f32) {
if self.body_status == BodyStatus::Dynamic {
self.angvel +=
self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse);
}
}
#[cfg(feature = "dim3")]
pub fn apply_torque_impulse(&mut self, torque_impulse: Vector<f32>) {
if self.body_status == BodyStatus::Dynamic {
self.angvel +=
self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse);
}
}
pub fn apply_force_at_point(&mut self, force: Vector<f32>, point: Point<f32>) {
let torque = (point - self.world_com).gcross(force);
self.apply_force(force);
self.apply_torque(torque);
}
pub fn apply_impulse_at_point(&mut self, impulse: Vector<f32>, point: Point<f32>) {
let torque_impulse = (point - self.world_com).gcross(impulse);
self.apply_impulse(impulse);
self.apply_torque_impulse(torque_impulse);
}
}
pub struct RigidBodyBuilder {
position: Isometry<f32>,
linvel: Vector<f32>,
angvel: AngVector<f32>,
body_status: BodyStatus,
can_sleep: bool,
user_data: u128,
}
impl RigidBodyBuilder {
pub fn new(body_status: BodyStatus) -> Self {
Self {
position: Isometry::identity(),
linvel: Vector::zeros(),
angvel: na::zero(),
body_status,
can_sleep: true,
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
}
#[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 build(&self) -> RigidBody {
let mut rb = RigidBody::new();
rb.predicted_position = self.position;
rb.set_position(self.position);
rb.linvel = self.linvel;
rb.angvel = self.angvel;
rb.body_status = self.body_status;
rb.user_data = self.user_data;
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
}
}