Split rigid-bodies and colliders into multiple components

1 files changed, 273 insertions, 455 deletions

diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index ba39873..1bd1ba5 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -1,193 +1,131 @@
-use crate::dynamics::MassProperties;
-use crate::geometry::{
-    Collider, ColliderHandle, ColliderSet, InteractionGraph, RigidBodyGraphIndex,
+use crate::dynamics::{
+    MassProperties, RigidBodyActivation, RigidBodyCcd, RigidBodyChanges, RigidBodyColliders,
+    RigidBodyDamping, RigidBodyDominance, RigidBodyForces, RigidBodyIds, RigidBodyMassProps,
+    RigidBodyMassPropsFlags, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
 };
-use crate::math::{
-    AngVector, AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector,
+use crate::geometry::{
+    Collider, ColliderHandle, ColliderMassProperties, ColliderParent, ColliderPosition,
+    ColliderShape,
 };
-use crate::utils::{self, WAngularInertia, WCross, WDot};
+use crate::math::{AngVector, Isometry, Point, Real, Rotation, Translation, Vector};
+use crate::utils::{self, WAngularInertia, WCross};
 use na::ComplexField;
 use num::Zero;
 
-#[derive(Copy, Clone, Debug, PartialEq, Eq)]
-#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-/// The status of a body, governing the way it is affected by external forces.
-pub enum BodyStatus {
-    /// A `BodyStatus::Dynamic` body can be affected by all external forces.
-    Dynamic,
-    /// A `BodyStatus::Static` body cannot be affected by external forces.
-    Static,
-    /// A `BodyStatus::Kinematic` body cannot be affected by any external forces but can be controlled
-    /// by the user at the position level while keeping realistic one-way interaction with dynamic bodies.
-    ///
-    /// One-way interaction means that a kinematic body can push a dynamic body, but a kinematic body
-    /// cannot be pushed by anything. In other words, the trajectory of a kinematic body can only be
-    /// modified by the user and is independent from any contact or joint it is involved in.
-    Kinematic,
-    // Semikinematic, // A kinematic that performs automatic CCD with the static environment to avoid traversing it?
-    // Disabled,
-}
-
-bitflags::bitflags! {
-    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-    /// Flags affecting the behavior of the constraints solver for a given contact manifold.
-    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;
-        const CCD_ENABLED = 1 << 4;
-        const CCD_ACTIVE = 1 << 5;
-    }
-}
-
-bitflags::bitflags! {
-    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-    /// Flags describing how the rigid-body has been modified by the user.
-    pub(crate) struct RigidBodyChanges: u32 {
-        const MODIFIED    = 1 << 0;
-        const POSITION    = 1 << 1;
-        const SLEEP       = 1 << 2;
-        const COLLIDERS   = 1 << 3;
-        const BODY_STATUS = 1 << 4;
-    }
-}
-
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 /// A rigid body.
 ///
 /// To create a new rigid-body, use the `RigidBodyBuilder` structure.
 #[derive(Debug, Clone)]
 pub struct RigidBody {
-    /// The world-space position of the rigid-body.
-    pub(crate) position: Isometry<Real>,
-    /// The next position of the rigid-body.
-    ///
-    /// At the beginning of the timestep, and when the
-    /// timestep is complete we must have position == next_position
-    /// except for kinematic bodies.
-    ///
-    /// The next_position is updated after the velocity and position
-    /// resolution. Then it is either validated (ie. we set position := set_position)
-    /// or clamped by CCD.
-    pub(crate) next_position: Isometry<Real>,
-    /// The local mass properties of the rigid-body.
-    pub(crate) mass_properties: MassProperties,
-    /// The world-space center of mass of the rigid-body.
-    pub world_com: Point<Real>,
-    /// The inverse mass taking into account translation locking.
-    pub effective_inv_mass: Real,
-    /// The square-root of the world-space inverse angular inertia tensor of the rigid-body,
-    /// taking into account rotation locking.
-    pub effective_world_inv_inertia_sqrt: AngularInertia<Real>,
-    /// The linear velocity of the rigid-body.
-    pub(crate) linvel: Vector<Real>,
-    /// The angular velocity of the rigid-body.
-    pub(crate) angvel: AngVector<Real>,
-    /// Damping factor for gradually slowing down the translational motion of the rigid-body.
-    pub linear_damping: Real,
-    /// Damping factor for gradually slowing down the angular motion of the rigid-body.
-    pub angular_damping: Real,
-    /// Accumulation of external forces (only for dynamic bodies).
-    pub(crate) force: Vector<Real>,
-    /// Accumulation of external torques (only for dynamic bodies).
-    pub(crate) torque: AngVector<Real>,
-    pub(crate) colliders: Vec<ColliderHandle>,
-    pub(crate) gravity_scale: Real,
+    pub rb_pos: RigidBodyPosition, // TODO ECS: public only for initial tests with bevy_rapier.
+    pub rb_mprops: RigidBodyMassProps, // TODO ECS: public only for initial tests with bevy_rapier.
+    pub rb_vels: RigidBodyVelocity, // TODO ECS: public only for initial tests with bevy_rapier.
+    pub rb_damping: RigidBodyDamping, // TODO ECS: public only for initial tests with bevy_rapier.
+    pub rb_forces: RigidBodyForces, // TODO ECS: public only for initial tests with bevy_rapier.
+    pub rb_ccd: RigidBodyCcd,      // TODO ECS: public only for initial tests with bevy_rapier.
+    pub rb_ids: RigidBodyIds,      // TODO ECS: public only for initial tests with bevy_rapier.
+    pub rb_colliders: RigidBodyColliders, // TODO ECS: public only for initial tests with bevy_rapier.
     /// Whether or not this rigid-body is sleeping.
-    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 rb_activation: RigidBodyActivation, // TODO ECS: public only for initial tests with bevy_rapier.
+    pub changes: RigidBodyChanges, // TODO ECS: public only for initial tests with bevy_rapier.
     /// The status of the body, governing how it is affected by external forces.
-    body_status: BodyStatus,
+    pub rb_type: RigidBodyType, // TODO ECS: public only for initial tests with bevy_rapier
     /// The dominance group this rigid-body is part of.
-    dominance_group: i8,
+    pub rb_dominance: RigidBodyDominance,
     /// User-defined data associated to this rigid-body.
     pub user_data: u128,
-    pub(crate) ccd_thickness: Real,
-    pub(crate) ccd_max_dist: Real,
 }
 
 impl RigidBody {
     fn new() -> Self {
         Self {
-            position: Isometry::identity(),
-            next_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(),
-            force: Vector::zeros(),
-            torque: 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(),
+            rb_pos: RigidBodyPosition::default(),
+            rb_mprops: RigidBodyMassProps::default(),
+            rb_vels: RigidBodyVelocity::default(),
+            rb_damping: RigidBodyDamping::default(),
+            rb_forces: RigidBodyForces::default(),
+            rb_ccd: RigidBodyCcd::default(),
+            rb_ids: RigidBodyIds::default(),
+            rb_colliders: RigidBodyColliders::default(),
+            rb_activation: RigidBodyActivation::new_active(),
             changes: RigidBodyChanges::all(),
-            body_status: BodyStatus::Dynamic,
-            dominance_group: 0,
+            rb_type: RigidBodyType::Dynamic,
+            rb_dominance: RigidBodyDominance::default(),
             user_data: 0,
-            ccd_thickness: Real::MAX,
-            ccd_max_dist: 0.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;
+        self.rb_colliders.0 = Vec::new();
+        self.rb_ids = Default::default();
     }
 
-    pub(crate) fn add_gravity(&mut self, gravity: Vector<Real>) {
-        if self.effective_inv_mass != 0.0 {
-            self.force += gravity * self.gravity_scale * self.mass();
-        }
+    pub fn user_data(&self) -> u128 {
+        self.user_data
     }
 
-    #[cfg(not(feature = "parallel"))] // in parallel solver this is not needed
-    pub(crate) fn integrate_accelerations(&mut self, dt: Real) {
-        let linear_acc = self.force * self.effective_inv_mass;
-        let angular_acc = self.effective_world_inv_inertia_sqrt
-            * (self.effective_world_inv_inertia_sqrt * self.torque);
+    pub fn set_user_data(&mut self, data: u128) {
+        self.user_data = data;
+    }
+
+    #[inline]
+    pub fn rb_activation(&self) -> &RigidBodyActivation {
+        &self.rb_activation
+    }
 
-        self.linvel += linear_acc * dt;
-        self.angvel += angular_acc * dt;
+    #[inline]
+    pub fn activation_mut(&mut self) -> &mut RigidBodyActivation {
+        &mut self.rb_activation
+    }
+
+    #[inline]
+    pub(crate) fn changes(&self) -> RigidBodyChanges {
+        self.changes
+    }
+
+    #[inline]
+    pub(crate) fn changes_mut_internal(&mut self) -> &mut RigidBodyChanges {
+        &mut self.changes
+    }
+
+    #[inline]
+    pub fn linear_damping(&self) -> Real {
+        self.rb_damping.linear_damping
+    }
+
+    #[inline]
+    pub fn set_linear_damping(&mut self, damping: Real) {
+        self.rb_damping.linear_damping = damping;
+    }
+
+    #[inline]
+    pub fn angular_damping(&self) -> Real {
+        self.rb_damping.angular_damping
+    }
+
+    #[inline]
+    pub fn set_angular_damping(&mut self, damping: Real) {
+        self.rb_damping.angular_damping = damping
     }
 
     /// The status of this rigid-body.
-    pub fn body_status(&self) -> BodyStatus {
-        self.body_status
+    pub fn rb_type(&self) -> RigidBodyType {
+        self.rb_type
     }
 
     /// Sets the status of this rigid-body.
-    pub fn set_body_status(&mut self, status: BodyStatus) {
-        if status != self.body_status {
-            self.changes.insert(RigidBodyChanges::BODY_STATUS);
-            self.body_status = status;
+    pub fn set_rb_type(&mut self, status: RigidBodyType) {
+        if status != self.rb_type {
+            self.changes.insert(RigidBodyChanges::TYPE);
+            self.rb_type = status;
         }
     }
 
     /// The mass properties of this rigid-body.
     #[inline]
     pub fn mass_properties(&self) -> &MassProperties {
-        &self.mass_properties
+        &self.rb_mprops.mass_properties
     }
 
     /// The dominance group of this rigid-body.
@@ -196,42 +134,48 @@ impl RigidBody {
     /// rigid-bodies.
     #[inline]
     pub fn effective_dominance_group(&self) -> i16 {
-        if self.is_dynamic() {
-            self.dominance_group as i16
-        } else {
-            i8::MAX as i16 + 1
-        }
+        self.rb_dominance.effective_group(&self.rb_type)
     }
 
     /// Are the translations of this rigid-body locked?
     pub fn is_translation_locked(&self) -> bool {
-        self.flags.contains(RigidBodyFlags::TRANSLATION_LOCKED)
+        self.rb_mprops
+            .flags
+            .contains(RigidBodyMassPropsFlags::TRANSLATION_LOCKED)
     }
 
     /// Are the rotations of this rigid-body locked?
     #[cfg(feature = "dim2")]
     pub fn is_rotation_locked(&self) -> bool {
-        self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Z)
+        self.rb_mprops
+            .flags
+            .contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_Z)
     }
 
     /// Returns `true` for each rotational degrees of freedom locked on this rigid-body.
     #[cfg(feature = "dim3")]
     pub fn is_rotation_locked(&self) -> [bool; 3] {
         [
-            self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_X),
-            self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Y),
-            self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Z),
+            self.rb_mprops
+                .flags
+                .contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_X),
+            self.rb_mprops
+                .flags
+                .contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_Y),
+            self.rb_mprops
+                .flags
+                .contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_Z),
         ]
     }
 
     /// Enables of disable CCD (continuous collision-detection) for this rigid-body.
     pub fn enable_ccd(&mut self, enabled: bool) {
-        self.flags.set(RigidBodyFlags::CCD_ENABLED, enabled)
+        self.rb_ccd.ccd_enabled = enabled;
     }
 
     /// Is CCD (continous collision-detection) enabled for this rigid-body?
     pub fn is_ccd_enabled(&self) -> bool {
-        self.flags.contains(RigidBodyFlags::CCD_ENABLED)
+        self.rb_ccd.ccd_enabled
     }
 
     // This is different from `is_ccd_enabled`. This checks that CCD
@@ -246,47 +190,7 @@ impl RigidBody {
     /// checks if CCD is allowed to run for this rigid-body or if
     /// it is completely disabled (independently from its velocity).
     pub fn is_ccd_active(&self) -> bool {
-        self.flags.contains(RigidBodyFlags::CCD_ACTIVE)
-    }
-
-    pub(crate) fn update_ccd_active_flag(&mut self, dt: Real, include_forces: bool) {
-        let ccd_active = self.is_ccd_enabled() && self.is_moving_fast(dt, include_forces);
-        self.flags.set(RigidBodyFlags::CCD_ACTIVE, ccd_active);
-    }
-
-    pub(crate) fn is_moving_fast(&self, dt: Real, include_forces: bool) -> bool {
-        if self.is_dynamic() {
-            // NOTE: for the threshold we don't use the exact CCD thickness. Theoretically, we
-            //       should use `self.ccd_thickness - smallest_contact_dist` where `smallest_contact_dist`
-            //       is the deepest contact (the contact with the largest penetration depth, i.e., the
-            //       negative `dist` with the largest absolute value.
-            //       However, getting this penetration depth assumes querying the contact graph from
-            //       the narrow-phase, which can be pretty expensive. So we use the CCD thickness
-            //       divided by 10 right now. We will see in practice if this value is OK or if we
-            //       should use a smaller (to be less conservative) or larger divisor (to be more conservative).
-            let threshold = self.ccd_thickness / 10.0;
-
-            if include_forces {
-                let linear_part = (self.linvel + self.force * dt).norm();
-                #[cfg(feature = "dim2")]
-                let angular_part = (self.angvel + self.torque * dt).abs() * self.ccd_max_dist;
-                #[cfg(feature = "dim3")]
-                let angular_part = (self.angvel + self.torque * dt).norm() * self.ccd_max_dist;
-                let vel_with_forces = linear_part + angular_part;
-                vel_with_forces > threshold
-            } else {
-                self.max_point_velocity() * dt > threshold
-            }
-        } else {
-            false
-        }
-    }
-
-    pub(crate) fn max_point_velocity(&self) -> Real {
-        #[cfg(feature = "dim2")]
-        return self.linvel.norm() + self.angvel.abs() * self.ccd_max_dist;
-        #[cfg(feature = "dim3")]
-        return self.linvel.norm() + self.angvel.norm() * self.ccd_max_dist;
+        self.rb_ccd.ccd_active
     }
 
     /// Sets the rigid-body's initial mass properties.
@@ -299,41 +203,41 @@ impl RigidBody {
             self.wake_up(true);
         }
 
-        self.mass_properties = props;
+        self.rb_mprops.mass_properties = props;
         self.update_world_mass_properties();
     }
 
     /// The handles of colliders attached to this rigid body.
     pub fn colliders(&self) -> &[ColliderHandle] {
-        &self.colliders[..]
+        &self.rb_colliders.0[..]
     }
 
     /// Is this rigid body dynamic?
     ///
     /// A dynamic body can move freely and is affected by forces.
     pub fn is_dynamic(&self) -> bool {
-        self.body_status == BodyStatus::Dynamic
+        self.rb_type == RigidBodyType::Dynamic
     }
 
     /// Is this rigid body kinematic?
     ///
     /// A kinematic body can move freely but is not affected by forces.
     pub fn is_kinematic(&self) -> bool {
-        self.body_status == BodyStatus::Kinematic
+        self.rb_type == RigidBodyType::Kinematic
     }
 
     /// Is this rigid body static?
     ///
     /// A static body cannot move and is not affected by forces.
     pub fn is_static(&self) -> bool {
-        self.body_status == BodyStatus::Static
+        self.rb_type == RigidBodyType::Static
     }
 
     /// The mass of this rigid body.
     ///
     /// Returns zero if this rigid body has an infinite mass.
     pub fn mass(&self) -> Real {
-        utils::inv(self.mass_properties.inv_mass)
+        utils::inv(self.rb_mprops.mass_properties.inv_mass)
     }
 
     /// The predicted position of this rigid-body.
@@ -342,69 +246,56 @@ impl RigidBody {
     /// method and is used for estimating the kinematic body velocity at the next timestep.
     /// For non-kinematic bodies, this value is currently unspecified.
     pub fn next_position(&self) -> &Isometry<Real> {
-        &self.next_position
+        &self.rb_pos.next_position
     }
 
     /// The scale factor applied to the gravity affecting this rigid-body.
     pub fn gravity_scale(&self) -> Real {
-        self.gravity_scale
+        self.rb_forces.gravity_scale
     }
 
     /// Sets the gravity scale facter for this rigid-body.
     pub fn set_gravity_scale(&mut self, scale: Real, wake_up: bool) {
-        if wake_up && self.activation.sleeping {
+        if wake_up && self.rb_activation.sleeping {
             self.changes.insert(RigidBodyChanges::SLEEP);
-            self.activation.sleeping = false;
+            self.rb_activation.sleeping = false;
         }
 
-        self.gravity_scale = scale;
+        self.rb_forces.gravity_scale = scale;
     }
 
     /// Adds a collider to this rigid-body.
-    pub(crate) fn add_collider(&mut self, handle: ColliderHandle, coll: &Collider) {
-        self.changes.set(
-            RigidBodyChanges::MODIFIED | RigidBodyChanges::COLLIDERS,
-            true,
-        );
-
-        self.ccd_thickness = self.ccd_thickness.min(coll.shape().ccd_thickness());
-
-        let shape_bsphere = coll
-            .shape()
-            .compute_bounding_sphere(coll.position_wrt_parent());
-        self.ccd_max_dist = self
-            .ccd_max_dist
-            .max(shape_bsphere.center.coords.norm() + shape_bsphere.radius);
-
-        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 {
-            // NOTE: we use `get_mut_internal_with_modification_tracking` here because we want to
-            //       benefit from the modification tracking to know the colliders
-            //       we need to update the broad-phase and narrow-phase for.
-            let collider = colliders
-                .get_mut_internal_with_modification_tracking(*handle)
-                .unwrap();
-            collider.set_position(self.position * collider.delta);
-        }
+    // TODO ECS: we keep this public for now just to simply our experiments on bevy_rapier.
+    pub fn add_collider(
+        &mut self,
+        co_handle: ColliderHandle,
+        co_parent: &ColliderParent,
+        co_pos: &mut ColliderPosition,
+        co_shape: &ColliderShape,
+        co_mprops: &ColliderMassProperties,
+    ) {
+        self.rb_colliders.attach_collider(
+            &mut self.changes,
+            &mut self.rb_ccd,
+            &mut self.rb_mprops,
+            &self.rb_pos,
+            co_handle,
+            co_pos,
+            co_parent,
+            co_shape,
+            co_mprops,
+        )
     }
 
     /// Removes a collider from this rigid-body.
     pub(crate) fn remove_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
-        if let Some(i) = self.colliders.iter().position(|e| *e == handle) {
+        if let Some(i) = self.rb_colliders.0.iter().position(|e| *e == handle) {
             self.changes.set(RigidBodyChanges::COLLIDERS, true);
-            self.colliders.swap_remove(i);
+            self.rb_colliders.0.swap_remove(i);
             let mass_properties = coll
                 .mass_properties()
                 .transform_by(coll.position_wrt_parent());
-            self.mass_properties -= mass_properties;
+            self.rb_mprops.mass_properties -= mass_properties;
             self.update_world_mass_properties();
         }
     }
@@ -415,10 +306,8 @@ impl RigidBody {
     /// it is waken up. It can be woken manually with `self.wake_up` or automatically due to
     /// external forces like contacts.
     pub fn sleep(&mut self) {
-        self.activation.energy = 0.0;
-        self.activation.sleeping = true;
-        self.linvel = na::zero();
-        self.angvel = na::zero();
+        self.rb_activation.sleep();
+        self.rb_vels = RigidBodyVelocity::zero();
     }
 
     /// Wakes up this rigid body if it is sleeping.
@@ -426,21 +315,11 @@ impl RigidBody {
     /// If `strong` is `true` then it is assured that the rigid-body will
     /// remain awake during multiple subsequent timesteps.
     pub fn wake_up(&mut self, strong: bool) {
-        if self.activation.sleeping {
+        if self.rb_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);
+        self.rb_activation.wake_up(strong);
     }
 
     /// Is this rigid body sleeping?
@@ -449,60 +328,45 @@ impl RigidBody {
         // - return false for static bodies.
         // - return true for non-sleeping dynamic bodies.
         // - return true only for kinematic bodies with non-zero velocity?
-        self.activation.sleeping
+        self.rb_activation.sleeping
     }
 
     /// Is the velocity of this body not zero?
     pub fn is_moving(&self) -> bool {
-        !self.linvel.is_zero() || !self.angvel.is_zero()
+        !self.rb_vels.linvel.is_zero() || !self.rb_vels.angvel.is_zero()
     }
 
     /// Computes the predict position of this rigid-body after `dt` seconds, taking
     /// into account its velocities and external forces applied to it.
     pub fn predict_position_using_velocity_and_forces(&self, dt: Real) -> Isometry<Real> {
-        let dlinvel = self.force * (self.effective_inv_mass * dt);
+        let dlinvel = self.rb_forces.force * (self.rb_mprops.effective_inv_mass * dt);
         let dangvel = self
+            .rb_mprops
             .effective_world_inv_inertia_sqrt
-            .transform_vector(self.torque * dt);
-        let linvel = self.linvel + dlinvel;
-        let angvel = self.angvel + dangvel;
-
-        let com = self.position * self.mass_properties.local_com;
-        let shift = Translation::from(com.coords);
-        shift * Isometry::new(linvel * dt, angvel * dt) * shift.inverse() * self.position
-    }
+            .transform_vector(self.rb_forces.torque * dt);
+        let linvel = self.rb_vels.linvel + dlinvel;
+        let angvel = self.rb_vels.angvel + dangvel;
 
-    pub(crate) fn integrate_velocity(&self, dt: Real) -> Isometry<Real> {
-        let com = self.position * self.mass_properties.local_com;
+        let com = self.rb_pos.position * self.rb_mprops.mass_properties.local_com;
         let shift = Translation::from(com.coords);
-        shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse()
-    }
-
-    pub(crate) fn apply_damping(&mut self, dt: Real) {
-        self.linvel *= 1.0 / (1.0 + dt * self.linear_damping);
-        self.angvel *= 1.0 / (1.0 + dt * self.angular_damping);
-    }
-
-    pub(crate) fn integrate_next_position(&mut self, dt: Real) {
-        self.next_position = self.integrate_velocity(dt) * self.position;
-        let _ = self.next_position.rotation.renormalize_fast();
+        shift * Isometry::new(linvel * dt, angvel * dt) * shift.inverse() * self.rb_pos.position
     }
 
     /// The linear velocity of this rigid-body.
     pub fn linvel(&self) -> &Vector<Real> {
-        &self.linvel
+        &self.rb_vels.linvel
     }
 
     /// The angular velocity of this rigid-body.
     #[cfg(feature = "dim2")]
     pub fn angvel(&self) -> Real {
-        self.angvel
+        self.rb_vels.angvel
     }
 
     /// The angular velocity of this rigid-body.
     #[cfg(feature = "dim3")]
     pub fn angvel(&self) -> &Vector<Real> {
-        &self.angvel
+        &self.rb_vels.angvel
     }
 
     /// The linear velocity of this rigid-body.
@@ -510,7 +374,7 @@ impl RigidBody {
     /// If `wake_up` is `true` then the rigid-body will be woken up if it was
     /// put to sleep because it did not move for a while.
     pub fn set_linvel(&mut self, linvel: Vector<Real>, wake_up: bool) {
-        self.linvel = linvel;
+        self.rb_vels.linvel = linvel;
 
         if self.is_dynamic() && wake_up {
             self.wake_up(true)
@@ -523,7 +387,7 @@ impl RigidBody {
     /// put to sleep because it did not move for a while.
     #[cfg(feature = "dim2")]
     pub fn set_angvel(&mut self, angvel: Real, wake_up: bool) {
-        self.angvel = angvel;
+        self.rb_vels.angvel = angvel;
 
         if self.is_dynamic() && wake_up {
             self.wake_up(true)
@@ -536,7 +400,7 @@ impl RigidBody {
     /// put to sleep because it did not move for a while.
     #[cfg(feature = "dim3")]
     pub fn set_angvel(&mut self, angvel: Vector<Real>, wake_up: bool) {
-        self.angvel = angvel;
+        self.rb_vels.angvel = angvel;
 
         if self.is_dynamic() && wake_up {
             self.wake_up(true)
@@ -544,8 +408,9 @@ impl RigidBody {
     }
 
     /// The world-space position of this rigid-body.
+    #[inline]
     pub fn position(&self) -> &Isometry<Real> {
-        &self.position
+        &self.rb_pos.position
     }
 
     /// Sets the position and `next_kinematic_position` of this rigid body.
@@ -559,8 +424,8 @@ impl RigidBody {
     /// put to sleep because it did not move for a while.
     pub fn set_position(&mut self, pos: Isometry<Real>, wake_up: bool) {
         self.changes.insert(RigidBodyChanges::POSITION);
-        self.position = pos;
-        self.next_position = pos;
+        self.rb_pos.position = pos;
+        self.rb_pos.next_position = pos;
 
         // TODO: Do we really need to check that the body isn't dynamic?
         if wake_up && self.is_dynamic() {
@@ -568,67 +433,16 @@ impl RigidBody {
         }
     }
 
-    pub(crate) fn set_next_position(&mut self, pos: Isometry<Real>) {
-        self.next_position = pos;
-    }
-
     /// If this rigid body is kinematic, sets its future position after the next timestep integration.
     pub fn set_next_kinematic_position(&mut self, pos: Isometry<Real>) {
         if self.is_kinematic() {
-            self.next_position = pos;
+            self.rb_pos.next_position = pos;
         }
     }
 
-    pub(crate) fn compute_velocity_from_next_position(&mut self, inv_dt: Real) {
-        let dpos = self.next_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_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);
-
-        // Take into account translation/rotation locking.
-        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;
-            }
-        }
+        self.rb_mprops
+            .update_world_mass_properties(&self.rb_pos.position);
     }
 }
 
@@ -638,8 +452,8 @@ impl RigidBody {
     /// The force will be applied in the next simulation step.
     /// This does nothing on non-dynamic bodies.
     pub fn apply_force(&mut self, force: Vector<Real>, wake_up: bool) {
-        if self.body_status == BodyStatus::Dynamic {
-            self.force += force;
+        if self.rb_type == RigidBodyType::Dynamic {
+            self.rb_forces.force += force;
 
             if wake_up {
                 self.wake_up(true);
@@ -652,8 +466,8 @@ impl RigidBody {
     /// This does nothing on non-dynamic bodies.
     #[cfg(feature = "dim2")]
     pub fn apply_torque(&mut self, torque: Real, wake_up: bool) {
-        if self.body_status == BodyStatus::Dynamic {
-            self.torque += torque;
+        if self.rb_type == RigidBodyType::Dynamic {
+            self.rb_forces.torque += torque;
 
             if wake_up {
                 self.wake_up(true);
@@ -666,8 +480,8 @@ impl RigidBody {
     /// This does nothing on non-dynamic bodies.
     #[cfg(feature = "dim3")]
     pub fn apply_torque(&mut self, torque: Vector<Real>, wake_up: bool) {
-        if self.body_status == BodyStatus::Dynamic {
-            self.torque += torque;
+        if self.rb_type == RigidBodyType::Dynamic {
+            self.rb_forces.torque += torque;
 
             if wake_up {
                 self.wake_up(true);
@@ -679,9 +493,9 @@ impl RigidBody {
     /// The force will be applied in the next simulation step.
     /// This does nothing on non-dynamic bodies.
     pub fn apply_force_at_point(&mut self, force: Vector<Real>, point: Point<Real>, wake_up: bool) {
-        if self.body_status == BodyStatus::Dynamic {
-            self.force += force;
-            self.torque += (point - self.world_com).gcross(force);
+        if self.rb_type == RigidBodyType::Dynamic {
+            self.rb_forces.force += force;
+            self.rb_forces.torque += (point - self.rb_mprops.world_com).gcross(force);
 
             if wake_up {
                 self.wake_up(true);
@@ -696,8 +510,8 @@ impl RigidBody {
     /// The impulse is applied right away, changing the linear velocity.
     /// This does nothing on non-dynamic bodies.
     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 self.rb_type == RigidBodyType::Dynamic {
+            self.rb_vels.linvel += impulse * self.rb_mprops.effective_inv_mass;
 
             if wake_up {
                 self.wake_up(true);
@@ -710,9 +524,9 @@ impl RigidBody {
     /// This does nothing on non-dynamic bodies.
     #[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 self.rb_type == RigidBodyType::Dynamic {
+            self.rb_vels.angvel += self.rb_mprops.effective_world_inv_inertia_sqrt
+                * (self.rb_mprops.effective_world_inv_inertia_sqrt * torque_impulse);
 
             if wake_up {
                 self.wake_up(true);
@@ -725,9 +539,9 @@ impl RigidBody {
     /// This does nothing on non-dynamic bodies.
     #[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 self.rb_type == RigidBodyType::Dynamic {
+            self.rb_vels.angvel += self.rb_mprops.effective_world_inv_inertia_sqrt
+                * (self.rb_mprops.effective_world_inv_inertia_sqrt * torque_impulse);
 
             if wake_up {
                 self.wake_up(true);
@@ -744,7 +558,7 @@ impl RigidBody {
         point: Point<Real>,
         wake_up: bool,
     ) {
-        let torque_impulse = (point - self.world_com).gcross(impulse);
+        let torque_impulse = (point - self.rb_mprops.world_com).gcross(impulse);
         self.apply_impulse(impulse, wake_up);
         self.apply_torque_impulse(torque_impulse, wake_up);
     }
@@ -753,24 +567,27 @@ impl RigidBody {
 impl RigidBody {
     /// The velocity of the given world-space point on this rigid-body.
     pub fn velocity_at_point(&self, point: &Point<Real>) -> Vector<Real> {
-        let dpt = point - self.world_com;
-        self.linvel + self.angvel.gcross(dpt)
+        let dpt = point - self.rb_mprops.world_com;
+        self.rb_vels.linvel + self.rb_vels.angvel.gcross(dpt)
     }
 
     /// The kinetic energy of this body.
     pub fn kinetic_energy(&self) -> Real {
-        let mut energy = (self.mass() * self.linvel().norm_squared()) / 2.0;
+        let mut energy = (self.mass() * self.rb_vels.linvel.norm_squared()) / 2.0;
 
         #[cfg(feature = "dim2")]
-        if !self.effective_world_inv_inertia_sqrt.is_zero() {
-            let inertia_sqrt = 1.0 / self.effective_world_inv_inertia_sqrt;
-            energy += (inertia_sqrt * self.angvel).powi(2) / 2.0;
+        if !self.rb_mprops.effective_world_inv_inertia_sqrt.is_zero() {
+            let inertia_sqrt = 1.0 / self.rb_mprops.effective_world_inv_inertia_sqrt;
+            energy += (inertia_sqrt * self.rb_vels.angvel).powi(2) / 2.0;
         }
 
         #[cfg(feature = "dim3")]
-        if !self.effective_world_inv_inertia_sqrt.is_zero() {
-            let inertia_sqrt = self.effective_world_inv_inertia_sqrt.inverse_unchecked();
-            energy += (inertia_sqrt * self.angvel).norm_squared() / 2.0;
+        if !self.rb_mprops.effective_world_inv_inertia_sqrt.is_zero() {
+            let inertia_sqrt = self
+                .rb_mprops
+                .effective_world_inv_inertia_sqrt
+