Merge pull request #267 from dimforge/multibody

Implement multibody joints, and new velocity-based constraints solver

17 files changed, 73 insertions, 74 deletions

diff --git a/benchmarks3d/Cargo.toml b/benchmarks3d/Cargo.toml
index 9b76df3..76d1911 100644
--- a/benchmarks3d/Cargo.toml
+++ b/benchmarks3d/Cargo.toml
@@ -2,7 +2,7 @@
 name    = "rapier-benchmarks-3d"
 version = "0.1.0"
 authors = [ "Sébastien Crozet <developer@crozet.re>" ]
-edition = "2018"
+edition = "2021"
 
 [features]
 parallel = [ "rapier3d/parallel", "rapier_testbed3d/parallel" ]
@@ -16,10 +16,10 @@ rand       = "0.8"
 Inflector  = "0.11"
 
 [dependencies.rapier_testbed3d]
-path = "../build/rapier_testbed3d"
+path = "../crates/rapier_testbed3d"
 
 [dependencies.rapier3d]
-path = "../build/rapier3d"
+path = "../crates/rapier3d"
 
 [[bin]]
 name = "all_benchmarks3"
diff --git a/benchmarks3d/all_benchmarks3.rs b/benchmarks3d/all_benchmarks3.rs
index f61b80f..6a3f756 100644
--- a/benchmarks3d/all_benchmarks3.rs
+++ b/benchmarks3d/all_benchmarks3.rs
@@ -58,10 +58,10 @@ pub fn main() {
         ("Stacks", stacks3::init_world),
         ("Pyramid", pyramid3::init_world),
         ("Trimesh", trimesh3::init_world),
-        ("Joint ball", joint_ball3::init_world),
-        ("Joint fixed", joint_fixed3::init_world),
-        ("Joint revolute", joint_revolute3::init_world),
-        ("Joint prismatic", joint_prismatic3::init_world),
+        ("ImpulseJoint ball", joint_ball3::init_world),
+        ("ImpulseJoint fixed", joint_fixed3::init_world),
+        ("ImpulseJoint revolute", joint_revolute3::init_world),
+        ("ImpulseJoint prismatic", joint_prismatic3::init_world),
         ("Keva tower", keva3::init_world),
     ];
 
diff --git a/benchmarks3d/balls3.rs b/benchmarks3d/balls3.rs
index 3f0bf36..b4d5102 100644
--- a/benchmarks3d/balls3.rs
+++ b/benchmarks3d/balls3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Create the balls
@@ -48,6 +49,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/benchmarks3d/boxes3.rs b/benchmarks3d/boxes3.rs
index 9c7ed81..0250619 100644
--- a/benchmarks3d/boxes3.rs
+++ b/benchmarks3d/boxes3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -58,6 +59,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/benchmarks3d/capsules3.rs b/benchmarks3d/capsules3.rs
index 8565503..5b05f23 100644
--- a/benchmarks3d/capsules3.rs
+++ b/benchmarks3d/capsules3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -59,6 +60,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/benchmarks3d/ccd3.rs b/benchmarks3d/ccd3.rs
index 987aba6..d54ebc1 100644
--- a/benchmarks3d/ccd3.rs
+++ b/benchmarks3d/ccd3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -73,6 +74,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/benchmarks3d/compound3.rs b/benchmarks3d/compound3.rs
index a914ce9..872923c 100644
--- a/benchmarks3d/compound3.rs
+++ b/benchmarks3d/compound3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -66,6 +67,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/benchmarks3d/convex_polyhedron3.rs b/benchmarks3d/convex_polyhedron3.rs
index 7065cd5..cb834ea 100644
--- a/benchmarks3d/convex_polyhedron3.rs
+++ b/benchmarks3d/convex_polyhedron3.rs
@@ -9,7 +9,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -73,6 +74,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/benchmarks3d/heightfield3.rs b/benchmarks3d/heightfield3.rs
index 32856fe..b95f1ee 100644
--- a/benchmarks3d/heightfield3.rs
+++ b/benchmarks3d/heightfield3.rs
@@ -8,7 +8,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -73,6 +74,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/benchmarks3d/joint_ball3.rs b/benchmarks3d/joint_ball3.rs
index a1661f8..64128ba 100644
--- a/benchmarks3d/joint_ball3.rs
+++ b/benchmarks3d/joint_ball3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let mut joints = JointSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     let rad = 0.4;
     let num = 100;
@@ -36,16 +37,16 @@ pub fn init_world(testbed: &mut Testbed) {
             // Vertical joint.
             if i > 0 {
                 let parent_handle = *body_handles.last().unwrap();
-                let joint = BallJoint::new(Point::origin(), point![0.0, 0.0, -shift]);
-                joints.insert(parent_handle, child_handle, joint);
+                let joint = SphericalJoint::new().local_anchor2(point![0.0, 0.0, -shift]);
+                impulse_joints.insert(parent_handle, child_handle, joint);
             }
 
             // Horizontal joint.
             if k > 0 {
                 let parent_index = body_handles.len() - num;
                 let parent_handle = body_handles[parent_index];
-                let joint = BallJoint::new(Point::origin(), point![-shift, 0.0, 0.0]);
-                joints.insert(parent_handle, child_handle, joint);
+                let joint = SphericalJoint::new().local_anchor2(point![-shift, 0.0, 0.0]);
+                impulse_joints.insert(parent_handle, child_handle, joint);
             }
 
             body_handles.push(child_handle);
@@ -55,6 +56,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![-110.0, -46.0, 170.0], point![54.0, -38.0, 29.0]);
 }
diff --git a/benchmarks3d/joint_fixed3.rs b/benchmarks3d/joint_fixed3.rs
index 3d1e317..b3f4039 100644
--- a/benchmarks3d/joint_fixed3.rs
+++ b/benchmarks3d/joint_fixed3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let mut joints = JointSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     let rad = 0.4;
     let num = 5;
@@ -49,22 +50,16 @@ pub fn init_world(testbed: &mut Testbed) {
                         // Vertical joint.
                         if i > 0 {
                             let parent_handle = *body_handles.last().unwrap();
-                            let joint = FixedJoint::new(
-                                Isometry::identity(),
-                                Isometry::translation(0.0, 0.0, -shift),
-                            );
-                            joints.insert(parent_handle, child_handle, joint);
+                            let joint = FixedJoint::new().local_anchor2(point![0.0, 0.0, -shift]);
+                            impulse_joints.insert(parent_handle, child_handle, joint);
                         }
 
                         // Horizontal joint.
                         if k > 0 {
                             let parent_index = body_handles.len() - num;
                             let parent_handle = body_handles[parent_index];
-                            let joint = FixedJoint::new(
-                                Isometry::identity(),
-                                Isometry::translation(-shift, 0.0, 0.0),
-                            );
-                            joints.insert(parent_handle, child_handle, joint);
+                            let joint = FixedJoint::new().local_anchor2(point![-shift, 0.0, 0.0]);
+                            impulse_joints.insert(parent_handle, child_handle, joint);
                         }
 
                         body_handles.push(child_handle);
@@ -77,6 +72,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![-38.0, 14.0, 108.0], point![46.0, 12.0, 23.0]);
 }
diff --git a/benchmarks3d/joint_prismatic3.rs b/benchmarks3d/joint_prismatic3.rs
index b011662..80839d7 100644
--- a/benchmarks3d/joint_prismatic3.rs
+++ b/benchmarks3d/joint_prismatic3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let mut joints = JointSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     let rad = 0.4;
     let num = 5;
@@ -47,19 +48,10 @@ pub fn init_world(testbed: &mut Testbed) {
                         UnitVector::new_normalize(vector![-1.0, 1.0, 0.0])
                     };
 
-                    let z = Vector::z();
-                    let mut prism = PrismaticJoint::new(
-                        Point::origin(),
-                        axis,
-                        z,
-                        point![0.0, 0.0, -shift],
-                        axis,
-                        z,
-                    );
-                    prism.limits_enabled = true;
-                    prism.limits[0] = -2.0;
-                    prism.limits[1] = 2.0;
-                    joints.insert(curr_parent, curr_child, prism);
+                    let prism = PrismaticJoint::new(axis)
+                        .local_anchor2(point![0.0, 0.0, -shift])
+                        .limit_axis([-2.0, 0.0]);
+                    impulse_joints.insert(curr_parent, curr_child, prism);
 
                     curr_parent = curr_child;
                 }
@@ -70,6 +62,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![262.0, 63.0, 124.0], point![101.0, 4.0, -3.0]);
 }
diff --git a/benchmarks3d/joint_revolute3.rs b/benchmarks3d/joint_revolute3.rs
index d6dc06c..8bdf0e9 100644
--- a/benchmarks3d/joint_revolute3.rs
+++ b/benchmarks3d/joint_revolute3.rs
@@ -7,7 +7,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let mut joints = JointSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     let rad = 0.4;
     let num = 10;
@@ -49,22 +50,21 @@ pub fn init_world(testbed: &mut Testbed) {
                     colliders.insert_with_parent(collider, handles[k], &mut bodies);
                 }
 
-                // Setup four joints.
-                let o = Point::origin();
+                // Setup four impulse_joints.
                 let x = Vector::x_axis();
                 let z = Vector::z_axis();
 
                 let revs = [
-                    RevoluteJoint::new(o, z, point![0.0, 0.0, -shift], z),
-                    RevoluteJoint::new(o, x, point![-shift, 0.0, 0.0], x),
-                    RevoluteJoint::new(o, z, point![0.0, 0.0, -shift], z),
-                    RevoluteJoint::new(o, x, point![shift, 0.0, 0.0], x),
+                    RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]),
+                    RevoluteJoint::new(x).local_anchor2(point![-shift, 0.0, 0.0]),
+                    RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]),
+                    RevoluteJoint::new(x).local_anchor2(point![shift, 0.0, 0.0]),
                 ];
 
-                joints.insert(curr_parent, handles[0], revs[0]);
-                joints.insert(handles[0], handles[1], revs[1]);
-                joints.insert(handles[1], handles[2], revs[2]);
-                joints.insert(handles[2], handles[3], revs[3]);
+                impulse_joints.insert(curr_parent, handles[0], revs[0]);
+                impulse_joints.insert(handles[0], handles[1], revs[1]);
+                impulse_joints.insert(handles[1], handles[2], revs[2]);
+                impulse_joints.insert(handles[2], handles[3], revs[3]);
 
                 curr_parent = handles[3];
             }
@@ -74,6 +74,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![478.0, 83.0, 228.0], point![134.0, 83.0, -116.0]);
 }
diff --git a/benchmarks3d/keva3.rs b/benchmarks3d/keva3.rs
index 38a0432..ad9e1ae 100644
--- a/benchmarks3d/keva3.rs
+++ b/benchmarks3d/keva3.rs
@@ -83,7 +83,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -132,6 +133,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/benchmarks3d/pyramid3.rs b/benchmarks3d/pyramid3.rs
index 655ecb7..b378da4 100644
--- a/benchmarks3d/pyramid3.rs
+++ b/benchmarks3d/pyramid3.rs
@@ -41,7 +41,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -73,6 +74,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/benchmarks3d/stacks3.rs b/benchmarks3d/stacks3.rs
index 39386ea..7fc9097 100644
--- a/benchmarks3d/stacks3.rs
+++ b/benchmarks3d/stacks3.rs
@@ -101,7 +101,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -183,6 +184,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }
diff --git a/benchmarks3d/trimesh3.rs b/benchmarks3d/trimesh3.rs
index 8621f19..1039a09 100644
--- a/benchmarks3d/trimesh3.rs
+++ b/benchmarks3d/trimesh3.rs
@@ -8,7 +8,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
 
     /*
      * Ground
@@ -78,6 +79,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
 }