First public release of Rapier.v0.1.0

50 files changed, 12084 insertions, 0 deletions

diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
new file mode 100644
index 0000000..faf22e1
--- /dev/null
+++ b/src/dynamics/integration_parameters.rs
@@ -0,0 +1,207 @@
+/// Parameters for a time-step of the physics engine.
+#[derive(Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct IntegrationParameters {
+    /// The timestep (default: `1.0 / 60.0`)
+    dt: f32,
+    /// The inverse of `dt`.
+    inv_dt: f32,
+    //    /// If `true` and if rapier is compiled with the `parallel` feature, this will enable rayon-based multithreading (default: `true`).
+    //    ///
+    //    /// This parameter is ignored if rapier is not compiled with is `parallel` feature.
+    //    /// Refer to rayon's documentation regarding how to configure the number of threads with either
+    //    /// `rayon::ThreadPoolBuilder::new().num_threads(4).build_global().unwrap()` or `ThreadPool::install`.
+    //    /// Note that using only one thread with `multithreading_enabled` set to `true` will result on a slower
+    //    /// simulation than setting `multithreading_enabled` to `false`.
+    //    pub multithreading_enabled: bool,
+    /// If `true`, the world's `step` method will stop right after resolving exactly one CCD event (default: `false`).
+    /// This allows the user to take action during a timestep, in-between two CCD events.
+    pub return_after_ccd_substep: bool,
+    /// The Error Reduction Parameter in `[0, 1]` is the proportion of
+    /// the positional error to be corrected at each time step (default: `0.2`).
+    pub erp: f32,
+    /// The Error Reduction Parameter for joints in `[0, 1]` is the proportion of
+    /// the positional error to be corrected at each time step (default: `0.2`).
+    pub joint_erp: f32,
+    /// Each cached impulse are multiplied by this coefficient in `[0, 1]`
+    /// when they are re-used to initialize the solver (default `1.0`).
+    pub warmstart_coeff: f32,
+    /// Contacts at points where the involved bodies have a relative
+    /// velocity smaller than this threshold wont be affected by the restitution force (default: `1.0`).
+    pub restitution_velocity_threshold: f32,
+    /// Amount of penetration the engine wont attempt to correct (default: `0.001m`).
+    pub allowed_linear_error: f32,
+    /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`).
+    pub prediction_distance: f32,
+    /// Amount of angular drift of joint limits the engine wont
+    /// attempt to correct (default: `0.001rad`).
+    pub allowed_angular_error: f32,
+    /// Maximum linear correction during one step of the non-linear position solver (default: `0.2`).
+    pub max_linear_correction: f32,
+    /// Maximum angular correction during one step of the non-linear position solver (default: `0.2`).
+    pub max_angular_correction: f32,
+    /// Maximum nonlinear SOR-prox scaling parameter when the constraint
+    /// correction direction is close to the kernel of the involved multibody's
+    /// jacobian (default: `0.2`).
+    pub max_stabilization_multiplier: f32,
+    /// Maximum number of iterations performed by the velocity constraints solver (default: `4`).
+    pub max_velocity_iterations: usize,
+    /// Maximum number of iterations performed by the position-based constraints solver (default: `1`).
+    pub max_position_iterations: usize,
+    /// Minimum number of dynamic bodies in each active island (default: `128`).
+    pub min_island_size: usize,
+    /// Maximum number of iterations performed by the position-based constraints solver for CCD steps (default: `10`).
+    ///
+    /// This should be sufficiently high so all penetration get resolved. For example, if CCD cause your
+    /// objects to stutter, that may be because the number of CCD position iterations is too low, causing
+    /// them to remain stuck in a penetration configuration for a few frames.
+    ///
+    /// The highest this number, the highest its computational cost.
+    pub max_ccd_position_iterations: usize,
+    /// Maximum number of substeps performed by the  solver (default: `1`).
+    pub max_ccd_substeps: usize,
+    /// Controls the number of Proximity::Intersecting events generated by a trigger during CCD resolution (default: `false`).
+    ///
+    /// If false, triggers will only generate one Proximity::Intersecting event during a step, even
+    /// if another colliders repeatedly enters and leaves the triggers during multiple CCD substeps.
+    ///
+    /// If true, triggers will generate as many Proximity::Intersecting and Proximity::Disjoint/Proximity::WithinMargin
+    /// events as the number of times a collider repeatedly enters and leaves the triggers during multiple CCD substeps.
+    /// This is more computationally intensive.
+    pub multiple_ccd_substep_sensor_events_enabled: bool,
+    /// Whether penetration are taken into account in CCD resolution (default: `false`).
+    ///
+    /// If this is set to `false` two penetrating colliders will not be considered to have any time of impact
+    /// while they are penetrating. This may end up allowing some tunelling, but will avoid stuttering effect
+    /// when the constraints solver fails to completely separate two colliders after a CCD contact.
+    ///
+    /// If this is set to `true`, two penetrating colliders will be considered to have a time of impact
+    /// equal to 0 until the constraints solver manages to separate them. This will prevent tunnelling
+    /// almost completely, but may introduce stuttering effects when the constraints solver fails to completely
+    /// separate two colliders after a CCD contact.
+    // FIXME: this is a very binary way of handling penetration.
+    // We should provide a more flexible solution by letting the user choose some
+    // minimal amount of movement applied to an object that get stuck.
+    pub ccd_on_penetration_enabled: bool,
+}
+
+impl IntegrationParameters {
+    /// Creates a set of integration parameters with the given values.
+    pub fn new(
+        dt: f32,
+        //        multithreading_enabled: bool,
+        erp: f32,
+        joint_erp: f32,
+        warmstart_coeff: f32,
+        restitution_velocity_threshold: f32,
+        allowed_linear_error: f32,
+        allowed_angular_error: f32,
+        max_linear_correction: f32,
+        max_angular_correction: f32,
+        prediction_distance: f32,
+        max_stabilization_multiplier: f32,
+        max_velocity_iterations: usize,
+        max_position_iterations: usize,
+        max_ccd_position_iterations: usize,
+        max_ccd_substeps: usize,
+        return_after_ccd_substep: bool,
+        multiple_ccd_substep_sensor_events_enabled: bool,
+        ccd_on_penetration_enabled: bool,
+    ) -> Self {
+        IntegrationParameters {
+            dt,
+            inv_dt: if dt == 0.0 { 0.0 } else { 1.0 / dt },
+            //            multithreading_enabled,
+            erp,
+            joint_erp,
+            warmstart_coeff,
+            restitution_velocity_threshold,
+            allowed_linear_error,
+            allowed_angular_error,
+            max_linear_correction,
+            max_angular_correction,
+            prediction_distance,
+            max_stabilization_multiplier,
+            max_velocity_iterations,
+            max_position_iterations,
+            // FIXME: what is the optimal value for min_island_size?
+            // It should not be too big so that we don't end up with
+            // huge islands that don't fit in cache.
+            // However we don't want it to be too small and end up with
+            // tons of islands, reducing SIMD parallelism opportunities.
+            min_island_size: 128,
+            max_ccd_position_iterations,
+            max_ccd_substeps,
+            return_after_ccd_substep,
+            multiple_ccd_substep_sensor_events_enabled,
+            ccd_on_penetration_enabled,
+        }
+    }
+
+    /// The current time-stepping length.
+    #[inline(always)]
+    pub fn dt(&self) -> f32 {
+        self.dt
+    }
+
+    /// The inverse of the time-stepping length.
+    ///
+    /// This is zero if `self.dt` is zero.
+    #[inline(always)]
+    pub fn inv_dt(&self) -> f32 {
+        self.inv_dt
+    }
+
+    /// Sets the time-stepping length.
+    ///
+    /// This automatically recompute `self.inv_dt`.
+    #[inline]
+    pub fn set_dt(&mut self, dt: f32) {
+        assert!(dt >= 0.0, "The time-stepping length cannot be negative.");
+        self.dt = dt;
+        if dt == 0.0 {
+            self.inv_dt = 0.0
+        } else {
+            self.inv_dt = 1.0 / dt
+        }
+    }
+
+    /// Sets the inverse time-stepping length (i.e. the frequency).
+    ///
+    /// This automatically recompute `self.dt`.
+    #[inline]
+    pub fn set_inv_dt(&mut self, inv_dt: f32) {
+        self.inv_dt = inv_dt;
+        if inv_dt == 0.0 {
+            self.dt = 0.0
+        } else {
+            self.dt = 1.0 / inv_dt
+        }
+    }
+}
+
+impl Default for IntegrationParameters {
+    fn default() -> Self {
+        Self::new(
+            1.0 / 60.0,
+            //            true,
+            0.2,
+            0.2,
+            1.0,
+            1.0,
+            0.005,
+            0.001,
+            0.2,
+            0.2,
+            0.002,
+            0.2,
+            4,
+            1,
+            10,
+            1,
+            false,
+            false,
+            false,
+        )
+    }
+}
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>&