#![allow(missing_docs)]
use downcast_rs::Downcast;
use na::{self, DVectorSlice, DVectorSliceMut, Real};
use ncollide::shape::DeformationsType;
use crate::math::{Force, ForceType, Inertia, Isometry, Point, Vector, Velocity};
use crate::object::{BodyPartHandle, BodyHandle};
use crate::solver::{IntegrationParameters, ForceDirection};
#[derive(Copy, Clone, PartialEq, Eq, Hash, Debug)]
pub enum BodyStatus {
Disabled,
Static,
Dynamic,
Kinematic,
}
#[derive(Copy, Clone, Debug)]
pub struct ActivationStatus<N: Real> {
threshold: Option<N>,
energy: N,
}
impl<N: Real> ActivationStatus<N> {
pub fn default_threshold() -> N {
na::convert(0.01f64)
}
pub fn new_active() -> Self {
ActivationStatus {
threshold: Some(Self::default_threshold()),
energy: Self::default_threshold() * na::convert(4.0),
}
}
pub fn new_inactive() -> Self {
ActivationStatus {
threshold: Some(Self::default_threshold()),
energy: N::zero(),
}
}
#[inline]
pub fn is_active(&self) -> bool {
!self.energy.is_zero()
}
#[inline]
pub fn deactivation_threshold(&self) -> Option<N> {
self.threshold
}
#[inline]
pub fn set_deactivation_threshold(&mut self, threshold: Option<N>) {
self.threshold = threshold
}
#[inline]
pub fn energy(&self) -> N {
self.energy
}
#[inline]
pub fn set_energy(&mut self, energy: N) {
self.energy = energy
}
}
pub trait Body<N: Real>: Downcast + Send + Sync {
fn name(&self) -> &str;
fn set_name(&mut self, name: String);
fn is_ground(&self) -> bool {
false
}
fn update_activation_status(&mut self) {
if self.update_status().body_needs_wake_up() {
self.activate()
}
}
fn update_kinematics(&mut self);
fn update_dynamics(&mut self, dt: N);
fn update_acceleration(&mut self, gravity: &Vector<N>, params: &IntegrationParameters<N>);
fn clear_forces(&mut self);
fn clear_update_flags(&mut self);
fn update_status(&self) -> BodyUpdateStatus;
fn apply_displacement(&mut self, disp: &[N]);
fn handle(&self) -> BodyHandle;
fn status(&self) -> BodyStatus;
fn set_status(&mut self, status: BodyStatus);
fn activation_status(&self) -> &ActivationStatus<N>;
fn set_deactivation_threshold(&mut self, threshold: Option<N>);
fn ndofs(&self) -> usize;
fn generalized_acceleration(&self) -> DVectorSlice<N>;
fn generalized_velocity(&self) -> DVectorSlice<N>;
fn companion_id(&self) -> usize;
fn set_companion_id(&mut self, id: usize);
fn generalized_velocity_mut(&mut self) -> DVectorSliceMut<N>;
fn integrate(&mut self, params: &IntegrationParameters<N>);
fn activate_with_energy(&mut self, energy: N);
fn deactivate(&mut self);
fn part(&self, i: usize) -> Option<&BodyPart<N>>;
fn deformed_positions(&self) -> Option<(DeformationsType, &[N])>;
fn deformed_positions_mut(&mut self) -> Option<(DeformationsType, &mut [N])>;
fn fill_constraint_geometry(
&self,
part: &BodyPart<N>,
ndofs: usize,
center: &Point<N>,
dir: &ForceDirection<N>,
j_id: usize,
wj_id: usize,
jacobians: &mut [N],
inv_r: &mut N,
ext_vels: Option<&DVectorSlice<N>>,
out_vel: Option<&mut N>
);
fn world_point_at_material_point(&self, part: &BodyPart<N>, point: &Point<N>) -> Point<N>;
fn position_at_material_point(&self, part: &BodyPart<N>, point: &Point<N>) -> Isometry<N>;
fn material_point_at_world_point(&self, part: &BodyPart<N>, point: &Point<N>) -> Point<N>;
fn has_active_internal_constraints(&mut self) -> bool;
fn setup_internal_velocity_constraints(&mut self, ext_vels: &DVectorSlice<N>, params: &IntegrationParameters<N>);
fn warmstart_internal_velocity_constraints(&mut self, dvels: &mut DVectorSliceMut<N>);
fn step_solve_internal_velocity_constraints(&mut self, dvels: &mut DVectorSliceMut<N>);
#[inline]
fn step_solve_internal_position_constraints(&mut self, params: &IntegrationParameters<N>);
fn add_local_inertia_and_com(&mut self, _part_index: usize, _com: Point<N>, _inertia: Inertia<N>)
{}
#[inline]
fn status_dependent_ndofs(&self) -> usize {
if self.is_dynamic() {
self.ndofs()
} else {
0
}
}
#[inline]
fn status_dependent_body_part_velocity(&self, part: &BodyPart<N>) -> Velocity<N> {
if self.is_dynamic() {
part.velocity()
} else {
Velocity::zero()
}
}
#[inline]
fn is_active(&self) -> bool {
match self.status() {
BodyStatus::Dynamic => self.activation_status().is_active(),
BodyStatus::Kinematic => true,
BodyStatus::Static => false,
BodyStatus::Disabled => false,
}
}
#[inline]
fn is_dynamic(&self) -> bool {
self.status() == BodyStatus::Dynamic
}
#[inline]
fn is_kinematic(&self) -> bool {
self.status() == BodyStatus::Kinematic
}
#[inline]
fn is_static(&self) -> bool {
self.status() == BodyStatus::Static
}
#[inline]
fn activate(&mut self) {
if let Some(threshold) = self.activation_status().deactivation_threshold() {
self.activate_with_energy(threshold * na::convert(2.0));
}
}
#[inline]
fn gravity_enabled(&self) -> bool;
#[inline]
fn enable_gravity(&mut self, enabled: bool);
fn apply_force(&mut self, part_id: usize, force: &Force<N>, force_type: ForceType, auto_wake_up: bool);
fn apply_local_force(&mut self, part_id: usize, force: &Force<N>, force_type: ForceType, auto_wake_up: bool);
fn apply_force_at_point(&mut self, part_id: usize, force: &Vector<N>, point: &Point<N>, force_type: ForceType, auto_wake_up: bool);
fn apply_local_force_at_point(&mut self, part_id: usize, force: &Vector<N>, point: &Point<N>, force_type: ForceType, auto_wake_up: bool);
fn apply_force_at_local_point(&mut self, part_id: usize, force: &Vector<N>, point: &Point<N>, force_type: ForceType, auto_wake_up: bool);
fn apply_local_force_at_local_point(&mut self, part_id: usize, force: &Vector<N>, point: &Point<N>, force_type: ForceType, auto_wake_up: bool);
}
pub trait BodyPart<N: Real>: Downcast + Send + Sync {
fn is_ground(&self) -> bool {
false
}
fn part_handle(&self) -> BodyPartHandle;
fn center_of_mass(&self) -> Point<N>;
fn position(&self) -> Isometry<N>;
fn velocity(&self) -> Velocity<N>;
fn inertia(&self) -> Inertia<N>;
fn local_inertia(&self) -> Inertia<N>;
}
impl_downcast!(Body<N> where N: Real);
impl_downcast!(BodyPart<N> where N: Real);
bitflags! {
#[derive(Default)]
struct BodyUpdateStatusFlags: u8 {
const POSITION_CHANGED = 0b00000001;
const VELOCITY_CHANGED = 0b00000010;
const LOCAL_INERTIA_CHANGED = 0b000100;
const LOCAL_COM_CHANGED = 0b001000;
const DAMPING_CHANGED = 0b010000;
const STATUS_CHANGED = 0b100000;
}
}
macro_rules! bitflags_accessors(
($($get_name: ident, $set_name: ident, $variant: ident)*) => {$(
#[inline]
pub fn $get_name(&self) -> bool {
self.0.contains(BodyUpdateStatusFlags::$variant)
}
#[inline]
pub fn $set_name(&mut self, changed: bool) {
self.0.set(BodyUpdateStatusFlags::$variant, changed)
}
)*}
);
#[derive(Copy, Clone, Debug, Hash, PartialEq, Eq)]
pub struct BodyUpdateStatus(BodyUpdateStatusFlags);
impl BodyUpdateStatus {
#[inline]
pub fn all() -> Self {
BodyUpdateStatus(BodyUpdateStatusFlags::all())
}
#[inline]
pub fn empty() -> Self {
BodyUpdateStatus(BodyUpdateStatusFlags::default())
}
bitflags_accessors!(
position_changed, set_position_changed, POSITION_CHANGED
velocity_changed, set_velocity_changed, VELOCITY_CHANGED
local_inertia_changed, set_local_inertia_changed, LOCAL_INERTIA_CHANGED
local_com_changed, set_local_com_changed, LOCAL_COM_CHANGED
damping_changed, set_damping_changed, DAMPING_CHANGED
status_changed, set_status_changed, STATUS_CHANGED
);
#[inline]
pub fn inertia_needs_update(&self) -> bool {
self.0.intersects(
BodyUpdateStatusFlags::POSITION_CHANGED |
BodyUpdateStatusFlags::VELOCITY_CHANGED |
BodyUpdateStatusFlags::LOCAL_INERTIA_CHANGED |
BodyUpdateStatusFlags::LOCAL_COM_CHANGED |
BodyUpdateStatusFlags::DAMPING_CHANGED |
BodyUpdateStatusFlags::STATUS_CHANGED
)
}
#[inline]
pub fn colliders_need_update(&self) -> bool {
self.position_changed()
}
#[inline]
pub fn body_needs_wake_up(&self) -> bool {
self.0.intersects(
BodyUpdateStatusFlags::POSITION_CHANGED |
BodyUpdateStatusFlags::VELOCITY_CHANGED |
BodyUpdateStatusFlags::STATUS_CHANGED
)
}
#[inline]
pub fn clear(&mut self) {
self.0 = BodyUpdateStatusFlags::empty()
}
}