Outsource the contact manifold, SAT, and some shapes.

62 files changed, 552 insertions, 2904 deletions

diff --git a/Cargo.toml b/Cargo.toml
index cfb166a..55e9b43 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -10,6 +10,8 @@ members = [ "build/rapier2d", "build/rapier_testbed2d", "examples2d", "benchmark
 #nphysics2d = { path = "../nphysics/build/nphysics2d" }
 #nphysics3d = { path = "../nphysics/build/nphysics3d" }
 #kiss3d = { path = "../kiss3d" }
+buckler2d = { path = "../buckler/build/buckler2d" }
+buckler3d = { path = "../buckler/build/buckler3d" }
 
 [profile.release]
 #debug = true
diff --git a/benchmarks3d/heightfield3.rs b/benchmarks3d/heightfield3.rs
index 4f8211c..fc64856 100644
--- a/benchmarks3d/heightfield3.rs
+++ b/benchmarks3d/heightfield3.rs
@@ -1,4 +1,4 @@
-use na::{DMatrix, Point3, Vector3};
+use na::{ComplexField, DMatrix, Point3, Vector3};
 use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
 use rapier3d::geometry::{ColliderBuilder, ColliderSet};
 use rapier_testbed3d::Testbed;
@@ -23,7 +23,11 @@ pub fn init_world(testbed: &mut Testbed) {
         } else {
             let x = i as f32 * ground_size.x / (nsubdivs as f32);
             let z = j as f32 * ground_size.z / (nsubdivs as f32);
-            x.sin() + z.cos()
+
+            // NOTE: make sure we use the sin/cos from simba to ensure
+            // cross-platform determinism of the example when the
+            // enhanced_determinism feature is enabled.
+            <f32 as ComplexField>::sin(x) + <f32 as ComplexField>::cos(z)
         }
     });
 
diff --git a/benchmarks3d/trimesh3.rs b/benchmarks3d/trimesh3.rs
index ce1d1c5..e0701f1 100644
--- a/benchmarks3d/trimesh3.rs
+++ b/benchmarks3d/trimesh3.rs
@@ -1,6 +1,6 @@
-use na::Point3;
+use na::{ComplexField, DMatrix, Point3, Vector3};
 use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
-use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet, HeightField};
 use rapier_testbed3d::Testbed;
 
 pub fn init_world(testbed: &mut Testbed) {
@@ -14,28 +14,27 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Ground
      */
-    let ground_size = 200.0f32;
-    let ground_height = 1.0;
+    let ground_size = Vector3::new(200.0, 1.0, 200.0);
     let nsubdivs = 20;
 
-    let quad = rapier3d::ncollide::procedural::quad(ground_size, ground_size, nsubdivs, nsubdivs);
-    let indices = quad
-        .flat_indices()
-        .chunks(3)
-        .map(|is| Point3::new(is[0], is[2], is[1]))
-        .collect();
-    let mut vertices = quad.coords;
+    let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| {
+        if i == 0 || i == nsubdivs || j == 0 || j == nsubdivs {
+            10.0
+        } else {
+            let x = i as f32 * ground_size.x / (nsubdivs as f32);
+            let z = j as f32 * ground_size.z / (nsubdivs as f32);
 
-    // ncollide generates a quad with `z` as the normal.
-    // so we switch z and y here and set a random altitude at each point.
-    for p in vertices.iter_mut() {
-        p.z = p.y;
-        p.y = (p.x.sin() + p.z.cos()) * ground_height;
-
-        if p.x.abs() == ground_size / 2.0 || p.z.abs() == ground_size / 2.0 {
-            p.y = 10.0;
+            // NOTE: make sure we use the sin/cos from simba to ensure
+            // cross-platform determinism of the example when the
+            // enhanced_determinism feature is enabled.
+            <f32 as ComplexField>::sin(x) + <f32 as ComplexField>::cos(z)
         }
-    }
+    });
+
+    // Here we will build our trimesh from the mesh representation of an
+    // heightfield.
+    let heightfield = HeightField::new(heights, ground_size);
+    let (vertices, indices) = heightfield.to_trimesh();
 
     let rigid_body = RigidBodyBuilder::new_static().build();
     let handle = bodies.insert(rigid_body);
diff --git a/build/rapier2d/Cargo.toml b/build/rapier2d/Cargo.toml
index 40e2db4..987fa2f 100644
--- a/build/rapier2d/Cargo.toml
+++ b/build/rapier2d/Cargo.toml
@@ -21,7 +21,7 @@ simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ]
 # enabled with the "simd-stable" or "simd-nightly" feature.
 simd-is-enabled = [ ]
 wasm-bindgen = [ "instant/wasm-bindgen" ]
-serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "ncollide2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ]
+serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "buckler2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ]
 enhanced-determinism = [ "simba/libm_force", "indexmap" ]
 
 [lib]
@@ -35,7 +35,7 @@ vec_map = "0.8"
 instant = { version = "0.1", features = [ "now" ]}
 num-traits = "0.2"
 nalgebra = "0.23"
-ncollide2d = "0.26"
+buckler2d = "0.1"
 simba = "0.3"
 approx = "0.4"
 rayon = { version = "1", optional = true }
diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml
index b1a1026..d409035 100644
--- a/build/rapier3d/Cargo.toml
+++ b/build/rapier3d/Cargo.toml
@@ -21,7 +21,7 @@ simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ]
 # enabled with the "simd-stable" or "simd-nightly" feature.
 simd-is-enabled = [ ]
 wasm-bindgen = [ "instant/wasm-bindgen" ]
-serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "ncollide3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ]
+serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "buckler3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ]
 enhanced-determinism = [ "simba/libm_force", "indexmap" ]
 
 [lib]
@@ -35,7 +35,7 @@ vec_map = "0.8"
 instant = { version = "0.1", features = [ "now" ]}
 num-traits = "0.2"
 nalgebra = "0.23"
-ncollide3d = "0.26"
+buckler3d = "0.1"
 simba = "0.3"
 approx = "0.4"
 rayon = { version = "1", optional = true }
diff --git a/build/rapier_testbed2d/Cargo.toml b/build/rapier_testbed2d/Cargo.toml
index 0b981e3..72ead41 100644
--- a/build/rapier_testbed2d/Cargo.toml
+++ b/build/rapier_testbed2d/Cargo.toml
@@ -31,6 +31,7 @@ instant    = { version = "0.1", features = [ "web-sys", "now" ]}
 bitflags   = "1"
 num_cpus   = { version = "1", optional = true }
 wrapped2d  = { version = "0.4", optional = true }
+buckler2d = "0.1"
 ncollide2d = "0.26"
 nphysics2d = { version = "0.18", optional = true }
 crossbeam = "0.8"
diff --git a/build/rapier_testbed3d/Cargo.toml b/build/rapier_testbed3d/Cargo.toml
index 3019021..d4ad764 100644
--- a/build/rapier_testbed3d/Cargo.toml
+++ b/build/rapier_testbed3d/Cargo.toml
@@ -30,6 +30,7 @@ instant    = { version = "0.1", features = [ "web-sys", "now" ]}
 bitflags   = "1"
 glam       = { version = "0.10", optional = true }
 num_cpus   = { version = "1", optional = true }
+buckler3d = "0.1"
 ncollide3d = "0.26"
 nphysics3d = { version = "0.18", optional = true }
 physx      = { version = "0.8", optional = true }
diff --git a/examples3d/trimesh3.rs b/examples3d/trimesh3.rs
index 0cee465..d753988 100644
--- a/examples3d/trimesh3.rs
+++ b/examples3d/trimesh3.rs
@@ -1,6 +1,6 @@
-use na::{ComplexField, Point3};
+use na::{ComplexField, DMatrix, Point3, Vector3};
 use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
-use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet, HeightField};
 use rapier_testbed3d::Testbed;
 
 pub fn init_world(testbed: &mut Testbed) {
@@ -14,31 +14,27 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Ground
      */
-    let ground_size = 100.0f32;
-    let ground_height = 1.0;
+    let ground_size = Vector3::new(100.0, 1.0, 100.0);
     let nsubdivs = 20;
 
-    let quad = rapier3d::ncollide::procedural::quad(ground_size, ground_size, nsubdivs, nsubdivs);
-    let indices = quad
-        .flat_indices()
-        .chunks(3)
-        .map(|is| Point3::new(is[0], is[2], is[1]))
-        .collect();
-    let mut vertices = quad.coords;
+    let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| {
+        if i == 0 || i == nsubdivs || j == 0 || j == nsubdivs {
+            10.0
+        } else {
+            let x = i as f32 * ground_size.x / (nsubdivs as f32);
+            let z = j as f32 * ground_size.z / (nsubdivs as f32);
 
-    // ncollide generates a quad with `z` as the normal.
-    // so we switch z and y here and set a random altitude at each point.
-    for p in vertices.iter_mut() {
-        p.z = p.y;
-        // NOTE: make sure we use the sin/cos from simba to ensure
-        // cross-platform determinism of the example when the
-        // enhanced_determinism feature is enabled.
-        p.y = (<f32 as ComplexField>::sin(p.x) + <f32 as ComplexField>::cos(p.z)) * ground_height;
-
-        if p.x.abs() == ground_size / 2.0 || p.z.abs() == ground_size / 2.0 {
-            p.y = 10.0;
+            // NOTE: make sure we use the sin/cos from simba to ensure
+            // cross-platform determinism of the example when the
+            // enhanced_determinism feature is enabled.
+            <f32 as ComplexField>::sin(x) + <f32 as ComplexField>::cos(z)
         }
-    }
+    });
+
+    // Here we will build our trimesh from the mesh representation of an
+    // heightfield.
+    let heightfield = HeightField::new(heights, ground_size);
+    let (vertices, indices) = heightfield.to_trimesh();
 
     let rigid_body = RigidBodyBuilder::new_static().build();
     let handle = bodies.insert(rigid_body);
diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs
index b8667bc..c32c402 100644
--- a/src/dynamics/rigid_body_set.rs
+++ b/src/dynamics/rigid_body_set.rs
@@ -479,7 +479,7 @@ impl RigidBodySet {
                 if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) {
                     for inter in contacts {
                         for manifold in &inter.2.manifolds {
-                            if manifold.num_active_contacts() > 0 {
+                            if manifold.num_active_contacts > 0 {
                                 let other = crate::utils::other_handle(
                                     (inter.0, inter.1),
                                     *collider_handle,
diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs
index 9ddf791..d0a2d0f 100644
--- a/src/dynamics/solver/categorization.rs
+++ b/src/dynamics/solver/categorization.rs
@@ -12,8 +12,8 @@ pub(crate) fn categorize_position_contacts(
 ) {
     for manifold_i in manifold_indices {
         let manifold = &manifolds[*manifold_i];
-        let rb1 = &bodies[manifold.body_pair.body1];
-        let rb2 = &bodies[manifold.body_pair.body2];
+        let rb1 = &bodies[manifold.data.body_pair.body1];
+        let rb2 = &bodies[manifold.data.body_pair.body2];
 
         if !rb1.is_dynamic() || !rb2.is_dynamic() {
             match manifold.kinematics.category {
@@ -38,8 +38,8 @@ pub(crate) fn categorize_velocity_contacts(
 ) {
     for manifold_i in manifold_indices {
         let manifold = &manifolds[*manifold_i];
-        let rb1 = &bodies[manifold.body_pair.body1];
-        let rb2 = &bodies[manifold.body_pair.body2];
+        let rb1 = &bodies[manifold.data.body_pair.body1];
+        let rb2 = &bodies[manifold.data.body_pair.body2];
 
         if !rb1.is_dynamic() || !rb2.is_dynamic() {
             out_ground.push(*manifold_i)
diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs
index 04acaaf..18e846b 100644
--- a/src/dynamics/solver/interaction_groups.rs
+++ b/src/dynamics/solver/interaction_groups.rs
@@ -12,7 +12,7 @@ pub(crate) trait PairInteraction {
 
 impl<'a> PairInteraction for &'a mut ContactManifold {
     fn body_pair(&self) -> BodyPair {
-        self.body_pair
+        self.data.body_pair
     }
 }
 
@@ -77,7 +77,7 @@ impl ParallelInteractionGroups {
             .iter()
             .zip(self.interaction_colors.iter_mut())
         {
-            let body_pair = interactions[*interaction_id].body_pair();
+            let body_pair = interactions[*interaction_id].data.body_pair();
             let rb1 = &bodies[body_pair.body1];
             let rb2 = &bodies[body_pair.body2];
 
@@ -338,7 +338,7 @@ impl InteractionGroups {
         let mut occupied_mask = 0u128;
         let max_interaction_points = interaction_indices
             .iter()
-            .map(|i| interactions[*i].num_active_contacts())
+            .map(|i| interactions[*i].num_active_contacts)
             .max()
             .unwrap_or(1);
 
@@ -351,12 +351,12 @@ impl InteractionGroups {
 
                 // FIXME: how could we avoid iterating
                 // on each interaction at every iteration on k?
-                if interaction.num_active_contacts() != k {
+                if interaction.num_active_contacts != k {
                     continue;
                 }
 
-                let body1 = &bodies[interaction.body_pair.body1];
-                let body2 = &bodies[interaction.body_pair.body2];
+                let body1 = &bodies[interaction.data.body_pair.body1];
+                let body2 = &bodies[interaction.data.body_pair.body2];
                 let is_static1 = !body1.is_dynamic();
                 let is_static2 = !body2.is_dynamic();
 
diff --git a/src/dynamics/solver/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs
index 303a18f..451dd32 100644
--- a/src/dynamics/solver/parallel_velocity_solver.rs
+++ b/src/dynamics/solver/parallel_velocity_solver.rs
@@ -247,13 +247,13 @@ impl ParallelVelocitySolverPart<AnyJointVelocityConstraint> {
                     VelocityConstraintDesc::NongroundGrouped(joint_id) => {
                         let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
                         let constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, joints, bodies);
-                        self.constraints[joints[0].constraint_index] = constraint;
+                        self.constraints[joints[0].data.constraint_index] = constraint;
                     }
                     #[cfg(feature = "simd-is-enabled")]
                     VelocityConstraintDesc::GroundGrouped(joint_id) => {
                         let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
                         let constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, joints, bodies);
-                        self.constraints[joints[0].constraint_index] = constraint;
+                        self.constraints[joints[0].data.constraint_index] = constraint;
                     }
                 }
             }
diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs
index 69fcf57..b329e39 100644
--- a/src/dynamics/solver/position_constraint.rs
+++ b/src/dynamics/solver/position_constraint.rs
@@ -88,8 +88,8 @@ impl PositionConstraint {
         out_constraints: &mut Vec<AnyPositionConstraint>,
         push: bool,
     ) {
-        let rb1 = &bodies[manifold.body_pair.body1];
-        let rb2 = &bodies[manifold.body_pair.body2];
+        let rb1 = &bodies[manifold.data.body_pair.body1];
+        let rb2 = &bodies[manifold.data.body_pair.body2];
         let shift1 = manifold.local_n1 * -manifold.kinematics.radius1;
         let shift2 = manifold.local_n2 * -manifold.kinematics.radius2;
         let radius =
@@ -104,8 +104,8 @@ impl PositionConstraint {
             let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS];
 
             for l in 0..manifold_points.len() {
-                local_p1[l] = manifold.delta1 * (manifold_points[l].local_p1 + shift1);
-                local_p2[l] = manifold.delta2 * (manifold_points[l].local_p2 + shift2);
+                local_p1[l] = manifold.data.delta1 * (manifold_points[l].local_p1 + shift1);
+                local_p2[l] = manifold.data.delta2 * (manifold_points[l].local_p2 + shift2);
             }
 
             let constraint = PositionConstraint {
@@ -132,10 +132,10 @@ impl PositionConstraint {
                 }
             } else {
                 if manifold.kinematics.category == KinematicsCategory::PointPoint {
-                    out_constraints[manifold.constraint_index + l] =
+                    out_constraints[manifold.data.constraint_index + l] =
                         AnyPositionConstraint::NongroupedPointPoint(constraint);
                 } else {
-                    out_constraints[manifold.constraint_index + l] =
+                    out_constraints[manifold.data.constraint_index + l] =
                         AnyPositionConstraint::NongroupedPlanePoint(constraint);
                 }
             }
diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs
index 0633926..ecc0fc3 100644
--- a/src/dynamics/solver/position_constraint_wide.rs
+++ b/src/dynamics/solver/position_constraint_wide.rs
@@ -35,8 +35,8 @@ impl WPositionConstraint {
         out_constraints: &mut Vec<AnyPositionConstraint>,
         push: bool,
     ) {
-        let rbs1 = array![|ii| bodies.get(manifolds[ii].body_pair.body1).unwrap(); SIMD_WIDTH];
-        let rbs2 = array![|ii| bodies.get(manifolds[ii].body_pair.body2).unwrap(); SIMD_WIDTH];
+        let rbs1 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH];
+        let rbs2 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH];
 
         let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
         let sqrt_ii1: AngularInertia<SimdFloat> =
@@ -51,8 +51,8 @@ impl WPositionConstraint {
         let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
         let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]);
 
-        let delta1 = Isometry::from(array![|ii| manifolds[ii].delta1; SIMD_WIDTH]);
-        let delta2 = Isometry::from(array![|ii| manifolds[ii].delta2; SIMD_WIDTH]);
+        let delta1 = Isometry::from(array![|ii| manifolds[ii].data.delta1; SIMD_WIDTH]);
+        let delta2 = Isometry::from(array![|ii| manifolds[ii].data.delta2; SIMD_WIDTH]);
 
         let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
         let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -100,10 +100,10 @@ impl WPositionConstraint {
                 }
             } else {
                 if manifolds[0].kinematics.category == KinematicsCategory::PointPoint {
-                    out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
+                    out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
                         AnyPositionConstraint::GroupedPointPoint(constraint);
                 } else {
-                    out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
+                    out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
                         AnyPositionConstraint::GroupedPlanePoint(constraint);
                 }
             }
diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs
index dcd2d64..7a15f3e 100644
--- a/src/dynamics/solver/position_ground_constraint.rs
+++ b/src/dynamics/solver/position_ground_constraint.rs