7 files changed, 652 insertions, 0 deletions
diff --git a/src/dynamics/joint/ball_joint.rs b/src/dynamics/joint/ball_joint.rs
new file mode 100644
index 0000000..ec255d4
--- /dev/null
+++ b/src/dynamics/joint/ball_joint.rs
@@ -0,0 +1,34 @@
+use crate::math::{Point, Vector};
+
+#[derive(Copy, Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A joint that removes all relative linear motion between a pair of points on two bodies.
+pub struct BallJoint {
+    /// Where the ball joint is attached on the first body, expressed in the first body local frame.
+    pub local_anchor1: Point<f32>,
+    /// Where the ball joint is attached on the first body, expressed in the first body local frame.
+    pub local_anchor2: Point<f32>,
+    /// The impulse applied by this joint on the first body.
+    ///
+    /// The impulse applied to the second body is given by `-impulse`.
+    pub impulse: Vector<f32>,
+}
+
+impl BallJoint {
+    /// Creates a new Ball joint from two anchors given on the local spaces of the respective bodies.
+    pub fn new(local_anchor1: Point<f32>, local_anchor2: Point<f32>) -> Self {
+        Self::with_impulse(local_anchor1, local_anchor2, Vector::zeros())
+    }
+
+    pub(crate) fn with_impulse(
+        local_anchor1: Point<f32>,
+        local_anchor2: Point<f32>,
+        impulse: Vector<f32>,
+    ) -> Self {
+        Self {
+            local_anchor1,
+            local_anchor2,
+            impulse,
+        }
+    }
+}
diff --git a/src/dynamics/joint/fixed_joint.rs b/src/dynamics/joint/fixed_joint.rs
new file mode 100644
index 0000000..0731cfb
--- /dev/null
+++ b/src/dynamics/joint/fixed_joint.rs
@@ -0,0 +1,33 @@
+use crate::math::{Isometry, SpacialVector};
+
+#[derive(Copy, Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A joint that prevents all relative movement between two bodies.
+///
+/// Given two frames of references, this joint aims to ensure these frame always coincide in world-space.
+pub struct FixedJoint {
+    /// The frame of reference for the first body affected by this joint, expressed in the local frame
+    /// of the first body.
+    pub local_anchor1: Isometry<f32>,
+    /// The frame of reference for the second body affected by this joint, expressed in the local frame
+    /// of the first body.
+    pub local_anchor2: Isometry<f32>,
+    /// The impulse applied to the first body affected by this joint.
+    ///
+    /// The impulse applied to the second body affected by this joint is given by `-impulse`.
+    /// This combines both linear and angular impulses:
+    /// - In 2D, `impulse.xy()` gives the linear impulse, and `impulse.z` the angular impulse.
+    /// - In 3D, `impulse.xyz()` gives the linear impulse, and `(impulse[3], impulse[4], impulse[5])` the angular impulse.
+    pub impulse: SpacialVector<f32>,
+}
+
+impl FixedJoint {
+    /// Creates a new fixed joint from the frames of reference of both bodies.
+    pub fn new(local_anchor1: Isometry<f32>, local_anchor2: Isometry<f32>) -> Self {
+        Self {
+            local_anchor1,
+            local_anchor2,
+            impulse: SpacialVector::zeros(),
+        }
+    }
+}
diff --git a/src/dynamics/joint/joint.rs b/src/dynamics/joint/joint.rs
new file mode 100644
index 0000000..074f802
--- /dev/null
+++ b/src/dynamics/joint/joint.rs
@@ -0,0 +1,112 @@
+#[cfg(feature = "dim3")]
+use crate::dynamics::RevoluteJoint;
+use crate::dynamics::{BallJoint, FixedJoint, JointHandle, PrismaticJoint, RigidBodyHandle};
+
+#[derive(Copy, Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// An enum grouping all possible types of joints.
+pub enum JointParams {
+    /// A Ball joint that removes all relative linear degrees of freedom between the affected bodies.
+    BallJoint(BallJoint),
+    /// A fixed joint that removes all relative degrees of freedom between the affected bodies.
+    FixedJoint(FixedJoint),
+    /// A prismatic joint that removes all degrees of degrees of freedom between the affected
+    /// bodies except for the translation along one axis.
+    PrismaticJoint(PrismaticJoint),
+    #[cfg(feature = "dim3")]
+    /// A revolute joint that removes all degrees of degrees of freedom between the affected
+    /// bodies except for the translation along one axis.
+    RevoluteJoint(RevoluteJoint),
+}
+
+impl JointParams {
+    /// An integer identifier for each type of joint.
+    pub fn type_id(&self) -> usize {
+        match self {
+            JointParams::BallJoint(_) => 0,
+            JointParams::FixedJoint(_) => 1,
+            JointParams::PrismaticJoint(_) => 2,
+            #[cfg(feature = "dim3")]
+            JointParams::RevoluteJoint(_) => 3,
+        }
+    }
+
+    /// Gets a reference to the underlying ball joint, if `self` is one.
+    pub fn as_ball_joint(&self) -> Option<&BallJoint> {
+        if let JointParams::BallJoint(j) = self {
+            Some(j)
+        } else {
+            None
+        }
+    }
+
+    /// Gets a reference to the underlying fixed joint, if `self` is one.
+    pub fn as_fixed_joint(&self) -> Option<&FixedJoint> {
+        if let JointParams::FixedJoint(j) = self {
+            Some(j)
+        } else {
+            None
+        }
+    }
+
+    /// Gets a reference to the underlying prismatic joint, if `self` is one.
+    pub fn as_prismatic_joint(&self) -> Option<&PrismaticJoint> {
+        if let JointParams::PrismaticJoint(j) = self {
+            Some(j)
+        } else {
+            None
+        }
+    }
+
+    /// Gets a reference to the underlying revolute joint, if `self` is one.
+    #[cfg(feature = "dim3")]
+    pub fn as_revolute_joint(&self) -> Option<&RevoluteJoint> {
+        if let JointParams::RevoluteJoint(j) = self {
+            Some(j)
+        } else {
+            None
+        }
+    }
+}
+
+impl From<BallJoint> for JointParams {
+    fn from(j: BallJoint) -> Self {
+        JointParams::BallJoint(j)
+    }
+}
+
+impl From<FixedJoint> for JointParams {
+    fn from(j: FixedJoint) -> Self {
+        JointParams::FixedJoint(j)
+    }
+}
+
+#[cfg(feature = "dim3")]
+impl From<RevoluteJoint> for JointParams {
+    fn from(j: RevoluteJoint) -> Self {
+        JointParams::RevoluteJoint(j)
+    }
+}
+
+impl From<PrismaticJoint> for JointParams {
+    fn from(j: PrismaticJoint) -> Self {
+        JointParams::PrismaticJoint(j)
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A joint attached to two bodies.
+pub struct Joint {
+    /// Handle to the first body attached to this joint.
+    pub body1: RigidBodyHandle,
+    /// Handle to the second body attached to this joint.
+    pub body2: RigidBodyHandle,
+    // A joint needs to know its handle to simplify its removal.
+    pub(crate) handle: JointHandle,
+    #[cfg(feature = "parallel")]
+    pub(crate) constraint_index: usize,
+    #[cfg(feature = "parallel")]
+    pub(crate) position_constraint_index: usize,
+    /// The joint geometric parameters and impulse.
+    pub params: JointParams,
+}
diff --git a/src/dynamics/joint/joint_set.rs b/src/dynamics/joint/joint_set.rs
new file mode 100644
index 0000000..51d7432
--- /dev/null
+++ b/src/dynamics/joint/joint_set.rs
@@ -0,0 +1,218 @@
+use super::Joint;
+use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex};
+
+use crate::data::arena::{Arena, Index};
+use crate::dynamics::{JointParams, RigidBodyHandle, RigidBodySet};
+
+/// The unique identifier of a joint added to the joint set.
+pub type JointHandle = Index;
+pub(crate) type JointIndex = usize;
+pub(crate) type JointGraphEdge = crate::data::graph::Edge<Joint>;
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A set of joints that can be handled by a physics `World`.
+pub struct JointSet {
+    joint_ids: Arena<TemporaryInteractionIndex>, // Map joint handles to edge ids on the graph.
+    joint_graph: InteractionGraph<Joint>,
+}
+
+impl JointSet {
+    /// Creates a new empty set of joints.
+    pub fn new() -> Self {
+        Self {
+            joint_ids: Arena::new(),
+            joint_graph: InteractionGraph::new(),
+        }
+    }
+
+    /// An always-invalid joint handle.
+    pub fn invalid_handle() -> JointHandle {
+        JointHandle::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64)
+    }
+
+    /// The number of joints on this set.
+    pub fn len(&self) -> usize {
+        self.joint_graph.graph.edges.len()
+    }
+
+    /// Retrieve the joint graph where edges are joints and nodes are rigid body handles.
+    pub fn joint_graph(&self) -> &InteractionGraph<Joint> {
+        &self.joint_graph
+    }
+
+    /// Is the given joint handle valid?
+    pub fn contains(&self, handle: JointHandle) -> bool {
+        self.joint_ids.contains(handle)
+    }
+
+    /// Gets the joint with the given handle.
+    pub fn get(&self, handle: JointHandle) -> Option<&Joint> {
+        let id = self.joint_ids.get(handle)?;
+        self.joint_graph.graph.edge_weight(*id)
+    }
+
+    /// Gets the joint with the given handle without a known generation.
+    ///
+    /// This is useful when you know you want the joint at position `i` but
+    /// don't know what is its current generation number. Generation numbers are
+    /// used to protect from the ABA problem because the joint position `i`
+    /// are recycled between two insertion and a removal.
+    ///
+    /// Using this is discouraged in favor of `self.get(handle)` which does not
+    /// suffer form the ABA problem.
+    pub fn get_unknown_gen(&self, i: usize) -> Option<(&Joint, JointHandle)> {
+        let (id, handle) = self.joint_ids.get_unknown_gen(i)?;
+        Some((self.joint_graph.graph.edge_weight(*id)?, handle))
+    }
+
+    /// Iterates through all the joint on this set.
+    pub fn iter(&self) -> impl Iterator<Item = &Joint> {
+        self.joint_graph.graph.edges.iter().map(|e| &e.weight)
+    }
+
+    /// Iterates mutably through all the joint on this set.
+    pub fn iter_mut(&mut self) -> impl Iterator<Item = &mut Joint> {
+        self.joint_graph
+            .graph
+            .edges
+            .iter_mut()
+            .map(|e| &mut e.weight)
+    }
+
+    // /// The set of joints as an array.
+    // pub(crate) fn joints(&self) -> &[JointGraphEdge] {
+    //     // self.joint_graph
+    //     //     .graph
+    //     //     .edges
+    //     //     .iter_mut()
+    //     //     .map(|e| &mut e.weight)
+    // }
+
+    #[cfg(not(feature = "parallel"))]
+    pub(crate) fn joints_mut(&mut self) -> &mut [JointGraphEdge] {
+        &mut self.joint_graph.graph.edges[..]
+    }
+
+    #[cfg(feature = "parallel")]
+    pub(crate) fn joints_vec_mut(&mut self) -> &mut Vec<JointGraphEdge> {
+        &mut self.joint_graph.graph.edges
+    }
+
+    /// Inserts a new joint into this set and retrieve its handle.
+    pub fn insert<J>(
+        &mut self,
+        bodies: &mut RigidBodySet,
+        body1: RigidBodyHandle,
+        body2: RigidBodyHandle,
+        joint_params: J,
+    ) -> JointHandle
+    where
+        J: Into<JointParams>,
+    {
+        let handle = self.joint_ids.insert(0.into());
+        let joint = Joint {
+            body1,
+            body2,
+            handle,
+            #[cfg(feature = "parallel")]
+            constraint_index: 0,
+            #[cfg(feature = "parallel")]
+            position_constraint_index: 0,
+            params: joint_params.into(),
+        };
+
+        let (rb1, rb2) = bodies.get2_mut_internal(joint.body1, joint.body2);
+        let (rb1, rb2) = (
+            rb1.expect("Attempt to attach a joint to a non-existing body."),
+            rb2.expect("Attempt to attach a joint to a non-existing body."),
+        );
+
+        // NOTE: the body won't have a graph index if it does not
+        // have any joint attached.
+        if !InteractionGraph::<Joint>::is_graph_index_valid(rb1.joint_graph_index) {
+            rb1.joint_graph_index = self.joint_graph.graph.add_node(joint.body1);
+        }
+
+        if !InteractionGraph::<Joint>::is_graph_index_valid(rb2.joint_graph_index) {
+            rb2.joint_graph_index = self.joint_graph.graph.add_node(joint.body2);
+        }
+
+        let id = self
+            .joint_graph
+            .add_edge(rb1.joint_graph_index, rb2.joint_graph_index, joint);
+
+        self.joint_ids[handle] = id;
+        handle
+    }
+
+    /// Retrieve all the joints happening between two active bodies.
+    // NOTE: this is very similar to the code from NarrowPhase::select_active_interactions.
+    pub(crate) fn select_active_interactions(
+        &self,
+        bodies: &RigidBodySet,
+        out: &mut Vec<Vec<JointIndex>>,
+    ) {
+        for out_island in &mut out[..bodies.num_islands()] {
+            out_island.clear();
+        }
+
+        // FIXME: don't iterate through all the interactions.
+        for (i, edge) in self.joint_graph.graph.edges.iter().enumerate() {
+            let joint = &edge.weight;
+            let rb1 = &bodies[joint.body1];
+            let rb2 = &bodies[joint.body2];
+
+            if (rb1.is_dynamic() || rb2.is_dynamic())
+                && (!rb1.is_dynamic() || !rb1.is_sleeping())
+                && (!rb2.is_dynamic() || !rb2.is_sleeping())
+            {
+                let island_index = if !rb1.is_dynamic() {
+                    rb2.active_island_id
+                } else {
+                    rb1.active_island_id
+                };
+
+                out[island_index].push(i);
+            }
+        }
+    }
+
+    pub(crate) fn remove_rigid_body(
+        &mut self,
+        deleted_id: RigidBodyGraphIndex,
+        bodies: &mut RigidBodySet,
+    ) {
+        if InteractionGraph::<()>::is_graph_index_valid(deleted_id) {
+            // We have to delete each joint one by one in order to:
+            // - Wake-up the attached bodies.
+            // - Update our Handle -> graph edge mapping.
+            // Delete the node.
+            let to_delete: Vec<_> = self
+                .joint_graph
+                .interactions_with(deleted_id)
+                .map(|e| (e.0, e.1, e.2.handle))
+                .collect();
+            for (h1, h2, to_delete_handle) in to_delete {
+                let to_delete_edge_id = self.joint_ids.remove(to_delete_handle).unwrap();
+                self.joint_graph.graph.remove_edge(to_delete_edge_id);
+
+                // Update the id of the edge which took the place of the deleted one.
+                if let Some(j) = self.joint_graph.graph.edge_weight_mut(to_delete_edge_id) {
+                    self.joint_ids[j.handle] = to_delete_edge_id;
+                }
+
+                // Wake up the attached bodies.
+                bodies.wake_up(h1);
+                bodies.wake_up(h2);
+            }
+
+            if let Some(other) = self.joint_graph.remove_node(deleted_id) {
+                // One rigid-body joint graph index may have been invalidated
+                // so we need to update it.
+                if let Some(replacement) = bodies.get_mut_internal(other) {
+                    replacement.joint_graph_index = deleted_id;
+                }
+            }
+        }
+    }
+}
diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs
new file mode 100644
index 0000000..b4dd60e
--- /dev/null
+++ b/src/dynamics/joint/mod.rs
@@ -0,0 +1,16 @@
+pub use self::ball_joint::BallJoint;
+pub use self::fixed_joint::FixedJoint;
+pub use self::joint::{Joint, JointParams};
+pub(crate) use self::joint_set::{JointGraphEdge, JointIndex};
+pub use self::joint_set::{JointHandle, JointSet};
+pub use self::prismatic_joint::PrismaticJoint;
+#[cfg(feature = "dim3")]
+pub use self::revolute_joint::RevoluteJoint;
+
+mod ball_joint;
+mod fixed_joint;
+mod joint;
+mod joint_set;
+mod prismatic_joint;
+#[cfg(feature = "dim3")]
+mod revolute_joint;
diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs
new file mode 100644
index 0000000..a6fd558
--- /dev/null
+++ b/src/dynamics/joint/prismatic_joint.rs
@@ -0,0 +1,193 @@
+use crate::math::{Isometry, Point, Vector, DIM};
+use crate::utils::WBasis;
+use na::Unit;
+#[cfg(feature = "dim2")]
+use na::Vector2;
+#[cfg(feature = "dim3")]
+use na::Vector5;
+
+#[derive(Copy, Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A joint that removes all relative motion between two bodies, except for the translations along one axis.
+pub struct PrismaticJoint {
+    /// Where the prismatic joint is attached on the first body, expressed in the local space of the first attached body.
+    pub local_anchor1: Point<f32>,
+    /// Where the prismatic joint is attached on the second body, expressed in the local space of the second attached body.
+    pub local_anchor2: Point<f32>,
+    pub(crate) local_axis1: Unit<Vector<f32>>,
+    pub(crate) local_axis2: Unit<Vector<f32>>,
+    pub(crate) basis1: [Vector<f32>; DIM - 1],
+    pub(crate) basis2: [Vector<f32>; DIM - 1],
+    /// The impulse applied by this joint on the first body.
+    ///
+    /// The impulse applied to the second body is given by `-impulse`.
+    #[cfg(feature = "dim3")]
+    pub impulse: Vector5<f32>,
+    /// The impulse applied by this joint on the first body.
+    ///
+    /// The impulse applied to the second body is given by `-impulse`.
+    #[cfg(feature = "dim2")]
+    pub impulse: Vector2<f32>,
+    /// Whether or not this joint should enforce translational limits along its axis.
+    pub limits_enabled: bool,
+    /// The min an max relative position of the attached bodies along this joint's axis.
+    pub limits: [f32; 2],
+    /// The impulse applied by this joint on the first body to enforce the position limit along this joint's axis.
+    ///
+    /// The impulse applied to the second body is given by `-impulse`.
+    pub limits_impulse: f32,
+    // pub motor_enabled: bool,
+    // pub target_motor_vel: f32,
+    // pub max_motor_impulse: f32,
+    // pub motor_impulse: f32,
+}
+
+impl PrismaticJoint {
+    /// Creates a new prismatic joint with the given point of applications and axis, all expressed
+    /// in the local-space of the affected bodies.
+    #[cfg(feature = "dim2")]
+    pub fn new(
+        local_anchor1: Point<f32>,
+        local_axis1: Unit<Vector<f32>>,
+        local_anchor2: Point<f32>,
+        local_axis2: Unit<Vector<f32>>,
+    ) -> Self {
+        Self {
+            local_anchor1,
+            local_anchor2,
+            local_axis1,
+            local_axis2,
+            basis1: local_axis1.orthonormal_basis(),
+            basis2: local_axis2.orthonormal_basis(),
+            impulse: na::zero(),
+            limits_enabled: false,
+            limits: [-f32::MAX, f32::MAX],
+            limits_impulse: 0.0,
+            // motor_enabled: false,
+            // target_motor_vel: 0.0,
+            // max_motor_impulse: f32::MAX,
+            // motor_impulse: 0.0,
+        }
+    }
+
+    /// Creates a new prismatic joint with the given point of applications and axis, all expressed
+    /// in the local-space of the affected bodies.
+    ///
+    /// The local tangent are vector orthogonal to the local axis. It is used to compute a basis orthonormal
+    /// to the joint's axis. If this tangent is set to zero, te orthonormal basis will be automatically
+    /// computed arbitrarily.
+    #[cfg(feature = "dim3")]
+    pub fn new(
+        local_anchor1: Point<f32>,
+        local_axis1: Unit<Vector<f32>>,
+        local_tangent1: Vector<f32>,
+        local_anchor2: Point<f32>,
+        local_axis2: Unit<Vector<f32>>,
+        local_tangent2: Vector<f32>,
+    ) -> Self {
+        let basis1 = if let Some(local_bitangent1) =
+            Unit::try_new(local_axis1.cross(&local_tangent1), 1.0e-3)
+        {
+            [
+                local_bitangent1.into_inner(),
+                local_bitangent1.cross(&local_axis1),
+            ]
+        } else {
+            local_axis1.orthonormal_basis()
+        };
+
+        let basis2 = if let Some(local_bitangent2) =
+            Unit::try_new(local_axis2.cross(&local_tangent2), 2.0e-3)
+        {
+            [
+                local_bitangent2.into_inner(),
+                local_bitangent2.cross(&local_axis2),
+            ]
+        } else {
+            local_axis2.orthonormal_basis()
+        };
+
+        Self {
+            local_anchor1,
+            local_anchor2,
+            local_axis1,
+            local_axis2,
+            basis1,
+            basis2,
+            impulse: na::zero(),
+            limits_enabled: false,
+            limits: [-f32::MAX, f32::MAX],
+            limits_impulse: 0.0,
+            // motor_enabled: false,
+            // target_motor_vel: 0.0,
+            // max_motor_impulse: f32::MAX,
+            // motor_impulse: 0.0,
+        }
+    }
+
+    /// The local axis of this joint, expressed in the local-space of the first attached body.
+    pub fn local_axis1(&self) -> Unit<Vector<f32>> {
+        self.local_axis1
+    }
+
+    /// The local axis of this joint, expressed in the local-space of the second attached body.
+    pub fn local_axis2(&self) -> Unit<Vector<f32>> {
+        self.local_axis2
+    }
+
+    // FIXME: precompute this?
+    #[cfg(feature = "dim2")]
+    pub(crate) fn local_frame1(&self) -> Isometry<f32> {
+        use na::{Matrix2, Rotation2, UnitComplex};
+
+        let mat = Matrix2::from_columns(&[self.local_axis1.into_inner(), self.basis1[0]]);
+        let rotmat = Rotation2::from_matrix_unchecked(mat);
+        let rotation = UnitComplex::from_rotation_matrix(&rotmat);
+        let translation = self.local_anchor1.coords.into();
+        Isometry::from_parts(translation, rotation)
+    }
+
+    // FIXME: precompute this?
+    #[cfg(feature = "dim2")]
+    pub(crate) fn local_frame2(&self) -> Isometry<f32> {
+        use na::{Matrix2, Rotation2, UnitComplex};
+
+        let mat = Matrix2::from_columns(&[self.local_axis2.into_inner(), self.basis2[0]]);
+        let rotmat = Rotation2::from_matrix_unchecked(mat);
+        let rotation = UnitComplex::from_rotation_matrix(&rotmat);
+        let translation = self.local_anchor2.coords.into();
+        Isometry::from_parts(translation, rotation)
+    }
+
+    // FIXME: precompute this?
+    #[cfg(feature = "dim3")]
+    pub(crate) fn local_frame1(&self) -> Isometry<f32> {
+        use na::{Matrix3, Rotation3, UnitQuaternion};
+
+        let mat = Matrix3::from_columns(&[
+            self.local_axis1.into_inner(),
+            self.basis1[0],
+            self.basis1[1],
+        ]);
+        let rotmat = Rotation3::from_matrix_unchecked(mat);
+        let rotation = UnitQuaternion::from_rotation_matrix(&rotmat);
+        let translation = self.local_anchor1.coords.into();
+        Isometry::from_parts(translation, rotation)
+    }
+
+    // FIXME: precompute this?
+    #[cfg(feature = "dim3")]
+    pub(crate) fn local_frame2(&self) -> Isometry<f32> {
+        use na::{Matrix3, Rotation3, UnitQuaternion};
+
+        let mat = Matrix3::from_columns(&[
+            self.local_axis2.into_inner(),
+            self.basis2[0],
+            self.basis2[1],
+        ]);
+        let rotmat = Rotation3::from_matrix_unchecked(mat);
+        let rotation = UnitQuaternion::from_rotation_matrix(&rotmat);
+        let translation = self.local_anchor2.coords.into();
+        Isometry::from_parts(translation, rotation)
+    }
+}
diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs
new file mode 100644
index 0000000..cdb424b
--- /dev/null
+++ b/src/dynamics/joint/revolute_joint.rs
@@ -0,0 +1,46 @@
+use crate::math::{Point, Vector};
+use crate::utils::WBasis;
+use na::{Unit, Vector5};
+
+#[derive(Copy, Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A joint that removes all relative motion between two bodies, except for the rotations along one axis.
+pub struct RevoluteJoint {
+    /// Where the revolute joint is attached on the first body, expressed in the local space of the first attached body.
+    pub local_anchor1: Point<f32>,
+    /// Where the revolute joint is attached on the second body, expressed in the local space of the second attached body.
+    pub local_anchor2: Point<f32>,
+    /// The rotation axis of this revolute joint expressed in the local space of the first attached body.
+    pub local_axis1: Unit<Vector<f32>>,
+    /// The rotation axis of this revolute joint expressed in the local space of the second attached body.
+    pub local_axis2: Unit<Vector<f32>>,
+    /// The basis orthonormal to `local_axis1`, expressed in the local space of the first attached body.
+    pub basis1: [Vector<f32>; 2],
+    /// The basis orthonormal to `local_axis2`, expressed in the local space of the second attached body.
+    pub basis2: [Vector<f32>; 2],
+    /// The impulse applied by this joint on the first body.
+    ///
+    /// The impulse applied to the second body is given by `-impulse`.
+    pub impulse: Vector5<f32>,
+}
+
+impl RevoluteJoint {
+    /// Creates a new revolute joint with the given point of applications and axis, all expressed
+    /// in the local-space of the affected bodies.
+    pub fn new(
+        local_anchor1: Point<f32>,
+        local_axis1: Unit<Vector<f32>>,
+        local_anchor2: Point<f32>,
+        local_axis2: Unit<Vector<f32>>,
+    ) -> Self {
+        Self {
+            local_anchor1,
+            local_anchor2,
+            local_axis1,
+            local_axis2,
+            basis1: local_axis1.orthonormal_basis(),
+            basis2: local_axis2.orthonormal_basis(),
+            impulse: na::zero(),
+        }
+    }
+}