#![allow(missing_docs)]
use downcast_rs::Downcast;
use na::{self, DVectorSlice, DVectorSliceMut, RealField};
use ncollide::interpolation::{
ConstantLinearVelocityRigidMotion, ConstantVelocityRigidMotion, RigidMotion,
};
use ncollide::shape::DeformationsType;
use crate::math::{Force, ForceType, Inertia, Isometry, Point, Vector, Velocity};
use crate::solver::{ForceDirection, IntegrationParameters};
pub enum BodyPartMotion<N: RealField> {
RigidLinear(ConstantLinearVelocityRigidMotion<N>),
RigidNonlinear(ConstantVelocityRigidMotion<N>),
Static(Isometry<N>),
}
impl<N: RealField> BodyPartMotion<N> {
pub(crate) fn is_static_or_linear(&self) -> bool {
match self {
BodyPartMotion::RigidLinear(_) | BodyPartMotion::Static(_) => true,
_ => false,
}
}
pub(crate) fn linvel(&self) -> Vector<N> {
match self {
BodyPartMotion::RigidLinear(m) => m.velocity,
BodyPartMotion::RigidNonlinear(m) => m.linvel,
BodyPartMotion::Static(_m) => Vector::zeros(),
}
}
}
impl<N: RealField> RigidMotion<N> for BodyPartMotion<N> {
fn position_at_time(&self, t: N) -> Isometry<N> {
match self {
BodyPartMotion::RigidLinear(m) => m.position_at_time(t),
BodyPartMotion::RigidNonlinear(m) => m.position_at_time(t),
BodyPartMotion::Static(m) => m.position_at_time(t),
}
}
}
#[derive(Copy, Clone, PartialEq, Eq, Hash, Debug)]
pub enum BodyStatus {
Disabled,
Static,
Dynamic,
Kinematic,
}
#[derive(Copy, Clone, Debug)]
pub struct ActivationStatus<N: RealField> {
threshold: Option<N>,
energy: N,
}
impl<N: RealField> 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: RealField>: Downcast + Send + Sync {
fn is_ground(&self) -> bool {
false
}
fn update_activation_status(&mut self) {
if self.update_status().body_needs_wake_up() {
self.activate()
}
}
fn advance(&mut self, _time_ratio: N) {}
fn validate_advancement(&mut self) {}
fn clamp_advancement(&mut self) {}
fn part_motion(&self, _part_id: usize, _time_origin: N) -> Option<BodyPartMotion<N>> {
None
}
fn step_started(&mut self) {}
fn update_kinematics(&mut self);
fn update_dynamics(&mut self, dt: N);
fn update_acceleration(&mut self, gravity: &Vector<N>, parameters: &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 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, parameters: &IntegrationParameters<N>);
fn activate_with_energy(&mut self, energy: N);
fn deactivate(&mut self);
fn num_parts(&self) -> usize;
fn part(&self, i: usize) -> Option<&dyn 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: &dyn 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: &dyn BodyPart<N>, point: &Point<N>) -> Point<N>;
fn position_at_material_point(&self, part: &dyn BodyPart<N>, point: &Point<N>) -> Isometry<N>;
fn material_point_at_world_point(&self, part: &dyn 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>,
parameters: &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>);
fn step_solve_internal_position_constraints(&mut self, parameters: &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: &dyn 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));
}
}
fn gravity_enabled(&self) -> bool;
fn enable_gravity(&mut self, enabled: bool);
fn velocity_at_point(&self, part_id: usize, point: &Point<N>) -> Velocity<N>;
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: RealField>: Downcast + Send + Sync {
fn is_ground(&self) -> bool {
false
}
fn center_of_mass(&self) -> Point<N>;
fn local_center_of_mass(&self) -> Point<N>;
fn position(&self) -> Isometry<N>;
fn safe_position(&self) -> Isometry<N> {
self.position()
}
fn velocity(&self) -> Velocity<N>;
fn inertia(&self) -> Inertia<N>;
fn local_inertia(&self) -> Inertia<N>;
}
impl_downcast!(Body<N> where N: RealField);
impl_downcast!(BodyPart<N> where N: RealField);
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()
}
}