feat: add PD and PID controller implementations (#804)

* feat: add a PID controller implementation

* feat: add small rigid-body utilities + test interpolation test

* fix: make scrolling weaker on macos

* feat: add the option to use the PID controller in the character controller demo.

* feat: add a stateless PD controller

* feat(rapier_testbed): cleanup & support PidController in 2D too

* chore: add comments for the PD and PID controllers

* chore: update changelog

* feat: rename PidErrors to PdErrors which is more accurate

* fix cargo doc

* chore: remove dead code

* chore: make test module non-pub

src/control/mod.rs

@@ -4,11 +4,13 @@ pub use self::character_controller::{
     CharacterAutostep, CharacterCollision, CharacterLength, EffectiveCharacterMovement,
     KinematicCharacterController,
 };
+pub use self::pid_controller::{PdController, PdErrors, PidController};
 
 #[cfg(feature = "dim3")]
 pub use self::ray_cast_vehicle_controller::{DynamicRayCastVehicleController, Wheel, WheelTuning};
 
 mod character_controller;
 
+mod pid_controller;
 #[cfg(feature = "dim3")]
 mod ray_cast_vehicle_controller;

src/control/pid_controller.rs

@@ -0,0 +1,411 @@
+use crate::dynamics::{AxisMask, RigidBody, RigidBodyPosition, RigidBodyVelocity};
+use crate::math::{Isometry, Point, Real, Rotation, Vector};
+use parry::math::AngVector;
+
+/// A Proportional-Derivative (PD) controller.
+///
+/// This is useful for controlling a rigid-body at the velocity level so it matches a target
+/// pose.
+///
+/// This is a [PID controller](https://en.wikipedia.org/wiki/Proportional%E2%80%93integral%E2%80%93derivative_controller)
+/// without the Integral part to keep the API immutable, while having a behaviour generally
+/// sufficient for games.
+#[derive(Debug, Copy, Clone, PartialEq)]
+pub struct PdController {
+    /// The Proportional gain applied to the instantaneous linear position errors.
+    ///
+    /// This is usually set to a multiple of the inverse of simulation step time
+    /// (e.g. `60` if the delta-time is `1.0 / 60.0`).
+    pub lin_kp: Vector<Real>,
+    /// The Derivative gain applied to the instantaneous linear velocity errors.
+    ///
+    /// This is usually set to a value in `[0.0, 1.0]` where `0.0` implies no damping
+    /// (no correction of velocity errors) and `1.0` implies complete damping (velocity errors
+    /// are corrected in a single simulation step).
+    pub lin_kd: Vector<Real>,
+    /// The Proportional gain applied to the instantaneous angular position errors.
+    ///
+    /// This is usually set to a multiple of the inverse of simulation step time
+    /// (e.g. `60` if the delta-time is `1.0 / 60.0`).
+    pub ang_kp: AngVector<Real>,
+    /// The Derivative gain applied to the instantaneous angular velocity errors.
+    ///
+    /// This is usually set to a value in `[0.0, 1.0]` where `0.0` implies no damping
+    /// (no correction of velocity errors) and `1.0` implies complete damping (velocity errors
+    /// are corrected in a single simulation step).
+    pub ang_kd: AngVector<Real>,
+    /// The axes affected by this controller.
+    ///
+    /// Only coordinate axes with a bit flags set to `true` will be taken into
+    /// account when calculating the errors and corrections.
+    pub axes: AxisMask,
+}
+
+impl Default for PdController {
+    fn default() -> Self {
+        Self::new(60.0, 0.8, AxisMask::all())
+    }
+}
+
+/// A Proportional-Integral-Derivative (PID) controller.
+///
+/// For video games, the Proportional-Derivative [`PdController`] is generally sufficient and
+/// offers an immutable API.
+#[derive(Debug, Copy, Clone, PartialEq)]
+pub struct PidController {
+    /// The Proportional-Derivative (PD) part of this PID controller.
+    pub pd: PdController,
+    /// The translational error accumulated through time for the Integral part of the PID controller.
+    pub lin_integral: Vector<Real>,
+    /// The angular error accumulated through time for the Integral part of the PID controller.
+    pub ang_integral: AngVector<Real>,
+    /// The linear gain applied to the Integral part of the PID controller.
+    pub lin_ki: Vector<Real>,
+    /// The angular gain applied to the Integral part of the PID controller.
+    pub ang_ki: AngVector<Real>,
+}
+
+impl Default for PidController {
+    fn default() -> Self {
+        Self::new(60.0, 1.0, 0.8, AxisMask::all())
+    }
+}
+
+/// Position or velocity errors measured for PID control.
+pub struct PdErrors {
+    /// The linear (translational) part of the error.
+    pub linear: Vector<Real>,
+    /// The angular (rotational) part of the error.
+    pub angular: AngVector<Real>,
+}
+
+impl From<RigidBodyVelocity> for PdErrors {
+    fn from(vels: RigidBodyVelocity) -> Self {
+        Self {
+            linear: vels.linvel,
+            angular: vels.angvel,
+        }
+    }
+}
+
+impl PdController {
+    /// Initialized the PD controller with uniform gain.
+    ///
+    /// The same gain are applied on all axes. To configure per-axes gains, construct
+    /// the [`PdController`] by setting its fields explicitly instead.
+    ///
+    /// Only the axes specified in `axes` will be enabled (but the gain values are set
+    /// on all axes regardless).
+    pub fn new(kp: Real, kd: Real, axes: AxisMask) -> PdController {
+        #[cfg(feature = "dim2")]
+        return Self {
+            lin_kp: Vector::repeat(kp),
+            lin_kd: Vector::repeat(kd),
+            ang_kp: kp,
+            ang_kd: kd,
+            axes,
+        };
+
+        #[cfg(feature = "dim3")]
+        return Self {
+            lin_kp: Vector::repeat(kp),
+            lin_kd: Vector::repeat(kd),
+            ang_kp: AngVector::repeat(kp),
+            ang_kd: AngVector::repeat(kd),
+            axes,
+        };
+    }
+
+    /// Calculates the linear correction from positional and velocity errors calculated automatically
+    /// from a rigid-body and the desired positions/velocities.
+    ///
+    /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to
+    /// the inverse of the simulation step so the returned value is a linear rigid-body velocity
+    /// change.
+    pub fn linear_rigid_body_correction(
+        &self,
+        rb: &RigidBody,
+        target_pos: Point<Real>,
+        target_linvel: Vector<Real>,
+    ) -> Vector<Real> {
+        self.rigid_body_correction(
+            rb,
+            Isometry::from(target_pos),
+            RigidBodyVelocity {
+                linvel: target_linvel,
+                #[allow(clippy::clone_on_copy)]
+                angvel: rb.angvel().clone(),
+            },
+        )
+        .linvel
+    }
+
+    /// Calculates the angular correction from positional and velocity errors calculated automatically
+    /// from a rigid-body and the desired positions/velocities.
+    ///
+    /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to
+    /// the inverse of the simulation step so the returned value is an angular rigid-body velocity
+    /// change.
+    pub fn angular_rigid_body_correction(
+        &self,
+        rb: &RigidBody,
+        target_rot: Rotation<Real>,
+        target_angvel: AngVector<Real>,
+    ) -> AngVector<Real> {
+        self.rigid_body_correction(
+            rb,
+            Isometry::from_parts(na::one(), target_rot),
+            RigidBodyVelocity {
+                linvel: *rb.linvel(),
+                angvel: target_angvel,
+            },
+        )
+        .angvel
+    }
+
+    /// Calculates the linear and angular  correction from positional and velocity errors calculated
+    /// automatically from a rigid-body and the desired poses/velocities.
+    ///
+    /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to
+    /// the inverse of the simulation step so the returned value is a rigid-body velocity
+    /// change.
+    pub fn rigid_body_correction(
+        &self,
+        rb: &RigidBody,
+        target_pose: Isometry<Real>,
+        target_vels: RigidBodyVelocity,
+    ) -> RigidBodyVelocity {
+        let pose_errors = RigidBodyPosition {
+            position: rb.pos.position,
+            next_position: target_pose,
+        }
+        .pose_errors(rb.local_center_of_mass());
+        let vels_errors = target_vels - rb.vels;
+        self.correction(&pose_errors, &vels_errors.into())
+    }
+
+    /// Mask where each component is 1.0 or 0.0 depending on whether
+    /// the corresponding linear axis is enabled.
+    fn lin_mask(&self) -> Vector<Real> {
+        #[cfg(feature = "dim2")]
+        return Vector::new(
+            self.axes.contains(AxisMask::LIN_X) as u32 as Real,
+            self.axes.contains(AxisMask::LIN_Y) as u32 as Real,
+        );
+        #[cfg(feature = "dim3")]
+        return Vector::new(
+            self.axes.contains(AxisMask::LIN_X) as u32 as Real,
+            self.axes.contains(AxisMask::LIN_Y) as u32 as Real,
+            self.axes.contains(AxisMask::LIN_Z) as u32 as Real,
+        );
+    }
+
+    /// Mask where each component is 1.0 or 0.0 depending on whether
+    /// the corresponding angular axis is enabled.
+    fn ang_mask(&self) -> AngVector<Real> {
+        #[cfg(feature = "dim2")]
+        return self.axes.contains(AxisMask::ANG_Z) as u32 as Real;
+        #[cfg(feature = "dim3")]
+        return Vector::new(
+            self.axes.contains(AxisMask::ANG_X) as u32 as Real,
+            self.axes.contains(AxisMask::ANG_Y) as u32 as Real,
+            self.axes.contains(AxisMask::ANG_Z) as u32 as Real,
+        );
+    }
+
+    /// Calculates the linear and angular correction from the given positional and velocity errors.
+    ///
+    /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to
+    /// the inverse of the simulation step so the returned value is a rigid-body velocity
+    /// change.
+    pub fn correction(&self, pose_errors: &PdErrors, vel_errors: &PdErrors) -> RigidBodyVelocity {
+        let lin_mask = self.lin_mask();
+        let ang_mask = self.ang_mask();
+
+        RigidBodyVelocity {
+            linvel: (pose_errors.linear.component_mul(&self.lin_kp)
+                + vel_errors.linear.component_mul(&self.lin_kd))
+            .component_mul(&lin_mask),
+            #[cfg(feature = "dim2")]
+            angvel: (pose_errors.angular * self.ang_kp + vel_errors.angular * self.ang_kd)
+                * ang_mask,
+            #[cfg(feature = "dim3")]
+            angvel: (pose_errors.angular.component_mul(&self.ang_kp)
+                + vel_errors.angular.component_mul(&self.ang_kd))
+            .component_mul(&ang_mask),
+        }
+    }
+}
+
+impl PidController {
+    /// Initialized the PDI controller with uniform gain.
+    ///
+    /// The same gain are applied on all axes. To configure per-axes gains, construct
+    /// the [`PidController`] by setting its fields explicitly instead.
+    ///
+    /// Only the axes specified in `axes` will be enabled (but the gain values are set
+    /// on all axes regardless).
+    pub fn new(kp: Real, ki: Real, kd: Real, axes: AxisMask) -> PidController {
+        #[cfg(feature = "dim2")]
+        return Self {
+            pd: PdController::new(kp, kd, axes),
+            lin_integral: na::zero(),
+            ang_integral: na::zero(),
+            lin_ki: Vector::repeat(ki),
+            ang_ki: ki,
+        };
+
+        #[cfg(feature = "dim3")]
+        return Self {
+            pd: PdController::new(kp, kd, axes),
+            lin_integral: na::zero(),
+            ang_integral: na::zero(),
+            lin_ki: Vector::repeat(ki),
+            ang_ki: AngVector::repeat(ki),
+        };
+    }
+
+    /// Set the axes errors and corrections are computed for.
+    ///
+    /// This doesn’t modify any of the gains.
+    pub fn set_axes(&mut self, axes: AxisMask) {
+        self.pd.axes = axes;
+    }
+
+    /// Get the axes errors and corrections are computed for.
+    pub fn axes(&self) -> AxisMask {
+        self.pd.axes
+    }
+
+    /// Resets to zero the accumulated linear and angular errors used by
+    /// the Integral part of the controller.
+    pub fn reset_integrals(&mut self) {
+        self.lin_integral = na::zero();
+        self.ang_integral = na::zero();
+    }
+
+    /// Calculates the linear correction from positional and velocity errors calculated automatically
+    /// from a rigid-body and the desired positions/velocities.
+    ///
+    /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to
+    /// the inverse of the simulation step so the returned value is a linear rigid-body velocity
+    /// change.
+    ///
+    /// This method is mutable because of the need to update the accumulated positional
+    /// errors for the Integral part of this controller. Prefer the [`PdController`] instead if
+    /// an immutable API is needed.
+    pub fn linear_rigid_body_correction(
+        &mut self,
+        dt: Real,
+        rb: &RigidBody,
+        target_pos: Point<Real>,
+        target_linvel: Vector<Real>,
+    ) -> Vector<Real> {
+        self.rigid_body_correction(
+            dt,
+            rb,
+            Isometry::from(target_pos),
+            RigidBodyVelocity {
+                linvel: target_linvel,
+                #[allow(clippy::clone_on_copy)]
+                angvel: rb.angvel().clone(),
+            },
+        )
+        .linvel
+    }
+
+    /// Calculates the angular correction from positional and velocity errors calculated automatically
+    /// from a rigid-body and the desired positions/velocities.
+    ///
+    /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to
+    /// the inverse of the simulation step so the returned value is an angular rigid-body velocity
+    /// change.
+    ///
+    /// This method is mutable because of the need to update the accumulated positional
+    /// errors for the Integral part of this controller. Prefer the [`PdController`] instead if
+    /// an immutable API is needed.
+    pub fn angular_rigid_body_correction(
+        &mut self,
+        dt: Real,
+        rb: &RigidBody,
+        target_rot: Rotation<Real>,
+        target_angvel: AngVector<Real>,
+    ) -> AngVector<Real> {
+        self.rigid_body_correction(
+            dt,
+            rb,
+            Isometry::from_parts(na::one(), target_rot),
+            RigidBodyVelocity {
+                linvel: *rb.linvel(),
+                #[allow(clippy::clone_on_copy)]
+                angvel: target_angvel.clone(),
+            },
+        )
+        .angvel
+    }
+
+    /// Calculates the linear and angular  correction from positional and velocity errors calculated
+    /// automatically from a rigid-body and the desired poses/velocities.
+    ///
+    /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to
+    /// the inverse of the simulation step so the returned value is a rigid-body velocity
+    /// change.
+    ///
+    /// This method is mutable because of the need to update the accumulated positional
+    /// errors for the Integral part of this controller. Prefer the [`PdController`] instead if
+    /// an immutable API is needed.
+    pub fn rigid_body_correction(
+        &mut self,
+        dt: Real,
+        rb: &RigidBody,
+        target_pose: Isometry<Real>,
+        target_vels: RigidBodyVelocity,
+    ) -> RigidBodyVelocity {
+        let pose_errors = RigidBodyPosition {
+            position: rb.pos.position,
+            next_position: target_pose,
+        }
+        .pose_errors(rb.local_center_of_mass());
+        let vels_errors = target_vels - rb.vels;
+        self.correction(dt, &pose_errors, &vels_errors.into())
+    }
+
+    /// Calculates the linear and angular correction from the given positional and velocity errors.
+    ///
+    /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to
+    /// the inverse of the simulation step so the returned value is a rigid-body velocity
+    /// change.
+    ///
+    /// This method is mutable because of the need to update the accumulated positional
+    /// errors for the Integral part of this controller. Prefer the [`PdController`] instead if
+    /// an immutable API is needed.
+    pub fn correction(
+        &mut self,
+        dt: Real,
+        pose_errors: &PdErrors,
+        vel_errors: &PdErrors,
+    ) -> RigidBodyVelocity {
+        self.lin_integral += pose_errors.linear * dt;
+        self.ang_integral += pose_errors.angular * dt;
+
+        let lin_mask = self.pd.lin_mask();
+        let ang_mask = self.pd.ang_mask();
+
+        RigidBodyVelocity {
+            linvel: (pose_errors.linear.component_mul(&self.pd.lin_kp)
+                + vel_errors.linear.component_mul(&self.pd.lin_kd)
+                + self.lin_integral.component_mul(&self.lin_ki))
+            .component_mul(&lin_mask),
+            #[cfg(feature = "dim2")]
+            angvel: (pose_errors.angular * self.pd.ang_kp
+                + vel_errors.angular * self.pd.ang_kd
+                + self.ang_integral * self.ang_ki)
+                * ang_mask,
+            #[cfg(feature = "dim3")]
+            angvel: (pose_errors.angular.component_mul(&self.pd.ang_kp)
+                + vel_errors.angular.component_mul(&self.pd.ang_kd)
+                + self.ang_integral.component_mul(&self.ang_ki))
+            .component_mul(&ang_mask),
+        }
+    }
+}

src/dynamics/rigid_body.rs

@@ -1,3 +1,5 @@
+#[cfg(doc)]
+use super::IntegrationParameters;
 use crate::dynamics::{
     LockedAxes, MassProperties, RigidBodyActivation, RigidBodyAdditionalMassProps, RigidBodyCcd,
     RigidBodyChanges, RigidBodyColliders, RigidBodyDamping, RigidBodyDominance, RigidBodyForces,
@@ -10,9 +12,6 @@ use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector};
 use crate::utils::SimdCross;
 use num::Zero;
 
-#[cfg(doc)]
-use super::IntegrationParameters;
-
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 /// A rigid body.
 ///
@@ -237,6 +236,12 @@ impl RigidBody {
         &self.mprops.world_com
     }
 
+    /// The local-space center-of-mass of this rigid-body.
+    #[inline]
+    pub fn local_center_of_mass(&self) -> &Point<Real> {
+        &self.mprops.local_mprops.local_com
+    }
+
     /// The mass-properties of this rigid-body.
     #[inline]
     pub fn mass_properties(&self) -> &RigidBodyMassProps {
@@ -704,6 +709,11 @@ impl RigidBody {
         !self.vels.linvel.is_zero() || !self.vels.angvel.is_zero()
     }
 
+    /// The linear and angular velocity of this rigid-body.
+    pub fn vels(&self) -> &RigidBodyVelocity {
+        &self.vels
+    }
+
     /// The linear velocity of this rigid-body.
     pub fn linvel(&self) -> &Vector<Real> {
         &self.vels.linvel
@@ -721,6 +731,15 @@ impl RigidBody {
         &self.vels.angvel
     }
 
+    /// Set both the angular and linear velocity of this rigid-body.
+    ///
+    /// 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_vels(&mut self, vels: RigidBodyVelocity, wake_up: bool) {
+        self.set_linvel(vels.linvel, wake_up);
+        self.set_angvel(vels.angvel, wake_up);
+    }
+
     /// The linear velocity of this rigid-body.
     ///
     /// If `wake_up` is `true` then the rigid-body will be woken up if it was
@@ -1481,7 +1500,7 @@ impl RigidBodyBuilder {
     /// Build a new rigid-body with the parameters configured with this builder.
     pub fn build(&self) -> RigidBody {
         let mut rb = RigidBody::new();
-        rb.pos.next_position = self.position; // FIXME: compute the correct value?
+        rb.pos.next_position = self.position;
         rb.pos.position = self.position;
         rb.vels.linvel = self.linvel;
         rb.vels.angvel = self.angvel;

src/dynamics/rigid_body_components.rs

@@ -1,3 +1,6 @@
+#[cfg(doc)]
+use super::IntegrationParameters;
+use crate::control::PdErrors;
 use crate::dynamics::MassProperties;
 use crate::geometry::{
     ColliderChanges, ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition,
@@ -11,7 +14,7 @@ use crate::utils::{SimdAngularInertia, SimdCross, SimdDot};
 use num::Zero;
 
 #[cfg(doc)]
-use super::IntegrationParameters;
+use crate::control::PidController;
 
 /// The unique handle of a rigid body added to a `RigidBodySet`.
 #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash, Default)]
@@ -159,22 +162,11 @@ impl RigidBodyPosition {
     /// a time equal to `1.0 / inv_dt`.
     #[must_use]
     pub fn interpolate_velocity(&self, inv_dt: Real, local_com: &Point<Real>) -> RigidBodyVelocity {
-        let com = self.position * local_com;
-        let shift = Translation::from(com.coords);
-        let dpos = shift.inverse() * self.next_position * self.position.inverse() * shift;
-
-        let angvel;
-        #[cfg(feature = "dim2")]
-        {
-            angvel = dpos.rotation.angle() * inv_dt;
+        let pose_err = self.pose_errors(local_com);
+        RigidBodyVelocity {
+            linvel: pose_err.linear * inv_dt,
+            angvel: pose_err.angular * inv_dt,
         }
-        #[cfg(feature = "dim3")]
-        {
-            angvel = dpos.rotation.scaled_axis() * inv_dt;
-        }
-        let linvel = dpos.translation.vector * inv_dt;
-
-        RigidBodyVelocity { linvel, angvel }
     }
 
     /// Compute new positions after integrating the given forces and velocities.
@@ -191,6 +183,32 @@ impl RigidBodyPosition {
         let new_vels = forces.integrate(dt, vels, mprops);
         new_vels.integrate(dt, &self.position, &mprops.local_mprops.local_com)
     }
+
+    /// Computes the difference between [`Self::next_position`] and [`Self::position`].
+    ///
+    /// This error measure can for example be used for interpolating the velocity between two poses,
+    /// or be given to the [`PidController`].
+    ///
+    /// Note that interpolating the velocity can be done more conveniently with
+    /// [`Self::interpolate_velocity`].
+    pub fn pose_errors(&self, local_com: &Point<Real>) -> PdErrors {
+        let com = self.position * local_com;
+        let shift = Translation::from(com.coords);
+        let dpos = shift.inverse() * self.next_position * self.position.inverse() * shift;
+
+        let angular;
+        #[cfg(feature = "dim2")]
+        {
+            angular = dpos.rotation.angle();
+        }
+        #[cfg(feature = "dim3")]
+        {
+            angular = dpos.rotation.scaled_axis();
+        }
+        let linear = dpos.translation.vector;
+
+        PdErrors { linear, angular }
+    }
 }
 
 impl<T> From<T> for RigidBodyPosition
@@ -210,7 +228,34 @@ bitflags::bitflags! {
     #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
     #[derive(Copy, Clone, PartialEq, Eq, Debug)]
     /// Flags affecting the behavior of the constraints solver for a given contact manifold.
-    // FIXME: rename this to LockedAxes
+    pub struct AxisMask: u8 {
+        /// The translational X axis.
+        const LIN_X = 1 << 0;
+        /// The translational Y axis.
+        const LIN_Y = 1 << 1;
+        /// The translational Z axis.
+        const LIN_Z = 1 << 2;
+        /// The rotational X axis.
+        #[cfg(feature = "dim3")]
+        const ANG_X = 1 << 3;
+        /// The rotational Y axis.
+        #[cfg(feature = "dim3")]
+        const ANG_Y = 1 << 4;
+        /// The rotational Z axis.
+        const ANG_Z = 1 << 5;
+    }
+}
+
+impl Default for AxisMask {
+    fn default() -> Self {
+        AxisMask::empty()
+    }
+}
+
+bitflags::bitflags! {
+    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+    #[derive(Copy, Clone, PartialEq, Eq, Debug)]
+    /// Flags affecting the behavior of the constraints solver for a given contact manifold.
     pub struct LockedAxes: u8 {
         /// Flag indicating that the rigid-body cannot translate along the `X` axis.
         const TRANSLATION_LOCKED_X = 1 << 0;
@@ -720,6 +765,25 @@ impl std::ops::AddAssign<RigidBodyVelocity> for RigidBodyVelocity {
     }
 }
 
+impl std::ops::Sub<RigidBodyVelocity> for RigidBodyVelocity {
+    type Output = Self;
+
+    #[must_use]
+    fn sub(self, rhs: Self) -> Self {
+        RigidBodyVelocity {
+            linvel: self.linvel - rhs.linvel,
+            angvel: self.angvel - rhs.angvel,
+        }
+    }
+}
+
+impl std::ops::SubAssign<RigidBodyVelocity> for RigidBodyVelocity {
+    fn sub_assign(&mut self, rhs: Self) {
+        self.linvel -= rhs.linvel;
+        self.angvel -= rhs.angvel;
+    }
+}
+
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone, Debug, Copy, PartialEq)]
 /// Damping factors to progressively slow down a rigid-body.
@@ -1092,3 +1156,57 @@ impl RigidBodyActivation {
         self.time_since_can_sleep = self.time_until_sleep;
     }
 }
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::math::Real;
+
+    #[test]
+    fn test_interpolate_velocity() {
+        // Interpolate and then integrate the velocity to see if
+        // the end positions match.
+        #[cfg(feature = "f32")]
+        let mut rng = oorandom::Rand32::new(0);
+        #[cfg(feature = "f64")]
+        let mut rng = oorandom::Rand64::new(0);
+
+        for i in -10..=10 {
+            let mult = i as Real;
+            let (local_com, curr_pos, next_pos);
+            #[cfg(feature = "dim2")]
+            {
+                local_com = Point::new(rng.rand_float(), rng.rand_float());
+                curr_pos = Isometry::new(
+                    Vector::new(rng.rand_float(), rng.rand_float()) * mult,
+                    rng.rand_float(),
+                );
+                next_pos = Isometry::new(
+                    Vector::new(rng.rand_float(), rng.rand_float()) * mult,
+                    rng.rand_float(),
+                );
+            }
+            #[cfg(feature = "dim3")]
+            {
+                local_com = Point::new(rng.rand_float(), rng.rand_float(), rng.rand_float());
+                curr_pos = Isometry::new(
+                    Vector::new(rng.rand_float(), rng.rand_float(), rng.rand_float()) * mult,
+                    Vector::new(rng.rand_float(), rng.rand_float(), rng.rand_float()),
+                );
+                next_pos = Isometry::new(
+                    Vector::new(rng.rand_float(), rng.rand_float(), rng.rand_float()) * mult,
+                    Vector::new(rng.rand_float(), rng.rand_float(), rng.rand_float()),
+                );
+            }
+
+            let dt = 0.016;
+            let rb_pos = RigidBodyPosition {
+                position: curr_pos,
+                next_position: next_pos,
+            };
+            let vel = rb_pos.interpolate_velocity(1.0 / dt, &local_com);
+            let interp_pos = vel.integrate(dt, &curr_pos, &local_com);
+            approx::assert_relative_eq!(interp_pos, next_pos, epsilon = 1.0e-5);
+        }
+    }
+}