Implement multibody joints and the new solver

1 files changed, 106 insertions, 142 deletions

diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs
index 11cd533..02aa5f9 100644
--- a/examples3d/joints3.rs
+++ b/examples3d/joints3.rs
@@ -4,7 +4,7 @@ use rapier_testbed3d::Testbed;
 fn create_prismatic_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
-    joints: &mut JointSet,
+    impulse_joints: &mut ImpulseJointSet,
     origin: Point<f32>,
     num: usize,
 ) {
@@ -28,25 +28,17 @@ fn create_prismatic_joints(
         colliders.insert_with_parent(collider, curr_child, bodies);
 
         let axis = if i % 2 == 0 {
-            UnitVector::new_normalize(vector![1.0, 1.0, 0.0])
+            UnitVector::new_normalize(vector![1.0f32, 1.0, 0.0])
         } else {
-            UnitVector::new_normalize(vector![-1.0, 1.0, 0.0])
+            UnitVector::new_normalize(vector![-1.0f32, 1.0, 0.0])
         };
 
-        let z = Vector::z();
-        let mut prism = PrismaticJoint::new(
-            point![0.0, 0.0, 0.0],
-            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 mut prism = JointData::prismatic(axis)
+            .local_anchor1(point![0.0, 0.0, shift])
+            .local_anchor2(point![0.0, 0.0, 0.0])
+            .limit_axis(JointAxis::X, [-2.0, 2.0]);
+
+        impulse_joints.insert(curr_parent, curr_child, prism);
 
         curr_parent = curr_child;
     }
@@ -55,7 +47,7 @@ fn create_prismatic_joints(
 fn create_actuated_prismatic_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
-    joints: &mut JointSet,
+    impulse_joints: &mut ImpulseJointSet,
     origin: Point<f32>,
     num: usize,
 ) {
@@ -84,36 +76,29 @@ fn create_actuated_prismatic_joints(
             UnitVector::new_normalize(vector![-1.0, 1.0, 0.0])
         };
 
-        let z = Vector::z();
-        let mut prism = PrismaticJoint::new(
-            point![0.0, 0.0, 0.0],
-            axis,
-            z,
-            point![0.0, 0.0, -shift],
-            axis,
-            z,
-        );
+        let mut prism = JointData::prismatic(axis)
+            .local_anchor1(point![0.0, 0.0, 0.0])
+            .local_anchor2(point![0.0, 0.0, -shift]);
 
         if i == 1 {
-            prism.configure_motor_velocity(1.0, 1.0);
-            prism.limits_enabled = true;
-            prism.limits[1] = 5.0;
-            // We set a max impulse so that the motor doesn't fight
-            // the limits with large forces.
-            prism.motor_max_impulse = 1.0;
+            prism = prism
+                .limit_axis(JointAxis::X, [-Real::MAX, 5.0])
+                .motor_velocity(JointAxis::X, 1.0, 1.0)
+                // We set a max impulse so that the motor doesn't fight
+                // the limits with large forces.
+                .motor_max_impulse(JointAxis::X, 1.0);
         } else if i > 1 {
-            prism.configure_motor_position(2.0, 0.01, 1.0);
+            prism = prism.motor_position(JointAxis::X, 2.0, 0.01, 1.0);
         } else {
-            prism.configure_motor_velocity(1.0, 1.0);
-            // We set a max impulse so that the motor doesn't fight
-            // the limits with large forces.
-            prism.motor_max_impulse = 0.7;
-            prism.limits_enabled = true;
-            prism.limits[0] = -2.0;
-            prism.limits[1] = 5.0;
+            prism = prism
+                .motor_velocity(JointAxis::X, 1.0, 1.0)
+                // We set a max impulse so that the motor doesn't fight
+                // the limits with large forces.
+                .motor_max_impulse(JointAxis::X, 0.7)
+                .limit_axis(JointAxis::X, [-2.0, 5.0]);
         }
 
-        joints.insert(curr_parent, curr_child, prism);
+        impulse_joints.insert(curr_parent, curr_child, prism);
 
         curr_parent = curr_child;
     }
@@ -122,7 +107,7 @@ fn create_actuated_prismatic_joints(
 fn create_revolute_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
-    joints: &mut JointSet,
+    impulse_joints: &mut ImpulseJointSet,
     origin: Point<f32>,
     num: usize,
 ) {
@@ -156,22 +141,20 @@ fn create_revolute_joints(
             colliders.insert_with_parent(collider, handles[k], 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(x).local_anchor2(point![0.0, 0.0, -shift]),
+            RevoluteJoint::new(z).local_anchor2(point![-shift, 0.0, 0.0]),
+            RevoluteJoint::new(x).local_anchor2(point![0.0, 0.0, -shift]),
+            RevoluteJoint::new(z).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];
     }
@@ -180,7 +163,7 @@ fn create_revolute_joints(
 fn create_revolute_joints_with_limits(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
-    joints: &mut JointSet,
+    impulse_joints: &mut ImpulseJointSet,
     origin: Point<f32>,
 ) {
     let ground = bodies.insert(
@@ -212,25 +195,14 @@ fn create_revolute_joints_with_limits(
         bodies,
     );
 
-    let mut joint1 = RevoluteJoint::new(
-        Point::origin(),
-        Vector::z_axis(),
-        Point::origin(),
-        Vector::z_axis(),
-    );
-    joint1.limits_enabled = true;
-    joint1.limits = [-0.2, 0.2];
-    joints.insert(ground, platform1, joint1);
-
-    let mut joint2 = RevoluteJoint::new(
-        Point::origin(),
-        Vector::z_axis(),
-        Point::from(-shift),
-        Vector::z_axis(),
-    );
-    joint2.limits_enabled = true;
-    joint2.limits = [-0.3, 0.3];
-    joints.insert(platform1, platform2, joint2);
+    let z = Vector::z_axis();
+    let mut joint1 = RevoluteJoint::new(z).limit_axis(JointAxis::X, [-0.2, 0.2]);
+    impulse_joints.insert(ground, platform1, joint1);
+
+    let mut joint2 = RevoluteJoint::new(z)
+        .local_anchor2(shift.into())
+        .limit_axis(JointAxis::Z, [-0.2, 0.2]);
+    impulse_joints.insert(platform1, platform2, joint2);
 
     // Let’s add a couple of cuboids that will fall on the platforms, triggering the joint limits.
     let cuboid_body1 = bodies.insert(
@@ -259,7 +231,7 @@ fn create_revolute_joints_with_limits(
 fn create_fixed_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
-    joints: &mut JointSet,
+    impulse_joints: &mut ImpulseJointSet,
     origin: Point<f32>,
     num: usize,
 ) {
@@ -268,8 +240,8 @@ fn create_fixed_joints(
 
     let mut body_handles = Vec::new();
 
-    for k in 0..num {
-        for i in 0..num {
+    for i in 0..num {
+        for k in 0..num {
             let fk = k as f32;
             let fi = i as f32;
 
@@ -295,23 +267,18 @@ fn create_fixed_joints(
 
             // 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 parent_index = body_handles.len() - num;
+                let parent_handle = body_handles[parent_index];
+                let joint = JointData::fixed().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_index = body_handles.len() - 1;
                 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 = JointData::fixed().local_anchor2(point![-shift, 0.0, 0.0]);
+                impulse_joints.insert(parent_handle, child_handle, joint);
             }
 
             body_handles.push(child_handle);
@@ -322,7 +289,7 @@ fn create_fixed_joints(
 fn create_ball_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
-    joints: &mut JointSet,
+    impulse_joints: &mut ImpulseJointSet,
     num: usize,
 ) {
     let rad = 0.4;
@@ -351,16 +318,16 @@ fn create_ball_joints(
             // Vertical joint.
             if i > 0 {
                 let parent_handle = *body_handles.last().unwrap();
-                let joint = BallJoint::new(Point::origin(), point![0.0, 0.0, -shift * 2.0]);
-                joints.insert(parent_handle, child_handle, joint);
+                let joint = JointData::ball().local_anchor2(point![0.0, 0.0, -shift * 2.0]);
+                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 = JointData::ball().local_anchor2(point![-shift, 0.0, 0.0]);
+                impulse_joints.insert(parent_handle, child_handle, joint);
             }
 
             body_handles.push(child_handle);
@@ -371,7 +338,7 @@ fn create_ball_joints(
 fn create_ball_joints_with_limits(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
-    joints: &mut JointSet,
+    impulse_joints: &mut ImpulseJointSet,
     origin: Point<f32>,
 ) {
     let shift = vector![0.0, 0.0, 3.0];
@@ -405,38 +372,32 @@ fn create_ball_joints_with_limits(
         bodies,
     );
 
-    let mut joint1 = BallJoint::new(Point::origin(), Point::from(-shift));
-    joint1.limits_enabled = true;
-    joint1.limits_local_axis1 = Vector::z_axis();
-    joint1.limits_local_axis2 = Vector::z_axis();
-    joint1.limits_angle = 0.2;
-    joints.insert(ground, ball1, joint1);
-
-    let mut joint2 = BallJoint::new(Point::origin(), Point::from(-shift));
-    joint2.limits_enabled = true;
-    joint2.limits_local_axis1 = Vector::z_axis();
-    joint2.limits_local_axis2 = Vector::z_axis();
-    joint2.limits_angle = 0.3;
-    joints.insert(ball1, ball2, joint2);
+    let mut joint1 = JointData::ball()
+        .local_anchor2(Point::from(-shift))
+        .limit_axis(JointAxis::X, [-0.2, 0.2])
+        .limit_axis(JointAxis::Y, [-0.2, 0.2]);
+    impulse_joints.insert(ground, ball1, joint1);
+
+    let mut joint2 = JointData::ball()
+        .local_anchor2(Point::from(-shift))
+        .limit_axis(JointAxis::X, [-0.3, 0.3])
+        .limit_axis(JointAxis::Y, [-0.3, 0.3]);
+    impulse_joints.insert(ball1, ball2, joint2);
 }
 
 fn create_actuated_revolute_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
-    joints: &mut JointSet,
+    impulse_joints: &mut ImpulseJointSet,
     origin: Point<f32>,
     num: usize,
 ) {
     let rad = 0.4;
     let shift = 2.0;
 
-    // We will reuse this base configuration for all the joints here.
-    let joint_template = RevoluteJoint::new(
-        Point::origin(),
-        Vector::z_axis(),
-        point![0.0, 0.0, -shift],
-        Vector::z_axis(),
-    );
+    // We will reuse this base configuration for all the impulse_joints here.
+    let z = Vector::z_axis();
+    let joint_template = RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]);
 
     let mut parent_handle = RigidBodyHandle::invalid();
 
@@ -467,19 +428,19 @@ fn create_actuated_revolute_joints(
             let mut joint = joint_template.clone();
 
             if i % 3 == 1 {
-                joint.configure_motor_velocity(-20.0, 0.1);
+                joint = joint.motor_velocity(JointAxis::AngX, -20.0, 0.1);
             } else if i == num - 1 {
                 let stiffness = 0.2;
                 let damping = 1.0;
-                joint.configure_motor_position(3.14 / 2.0, stiffness, damping);
+                joint = joint.motor_position(JointAxis::AngX, 3.14 / 2.0, stiffness, damping);
             }
 
             if i == 1 {
-                joint.local_anchor2.y = 2.0;
-                joint.configure_motor_velocity(-2.0, 0.1);
+                joint.local_frame2.translation.vector.y = 2.0;
+                joint = joint.motor_velocity(JointAxis::AngX, -2.0, 0.1);
             }
 
-            joints.insert(parent_handle, child_handle, joint);
+            impulse_joints.insert(parent_handle, child_handle, joint);
         }
 
         parent_handle = child_handle;
@@ -489,15 +450,15 @@ fn create_actuated_revolute_joints(
 fn create_actuated_ball_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
-    joints: &mut JointSet,
+    impulse_joints: &mut ImpulseJointSet,
     origin: Point<f32>,
     num: usize,
 ) {
     let rad = 0.4;
     let shift = 2.0;
 
-    // We will reuse this base configuration for all the joints here.
-    let joint_template = BallJoint::new(point![0.0, 0.0, shift], Point::origin());
+    // We will reuse this base configuration for all the impulse_joints here.
+    let joint_template = JointData::ball().local_anchor1(point![0.0, 0.0, shift]);
 
     let mut parent_handle = RigidBodyHandle::invalid();
 
@@ -526,18 +487,20 @@ fn create_actuated_ball_joints(
             let mut joint = joint_template.clone();
 
             if i == 1 {
-                joint.configure_motor_velocity(vector![0.0, 0.5, -2.0], 0.1);
+                joint = joint
+                    .motor_velocity(JointAxis::AngX, 0.0, 0.1)
+                    .motor_velocity(JointAxis::AngY, 0.5, 0.1)
+                    .motor_velocity(JointAxis::AngZ, -2.0, 0.1);
             } else if i == num - 1 {
                 let stiffness = 0.2;
                 let damping = 1.0;
-                joint.configure_motor_position(
-                    Rotation::new(vector![0.0, 1.0, 3.14 / 2.0]),
-                    stiffness,
-                    damping,
-                );
+                joint = joint
+                    .motor_position(JointAxis::AngX, 0.0, stiffness, damping)
+                    .motor_position(JointAxis::AngY, 1.0, stiffness, damping)
+                    .motor_position(JointAxis::AngZ, 3.14 / 2.0, stiffness, damping);
             }
 
-            joints.insert(parent_handle, child_handle, joint);
+            impulse_joints.insert(parent_handle, child_handle, joint);
         }
 
         parent_handle = child_handle;
@@ -550,67 +513,68 @@ 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();
 
     create_prismatic_joints(
         &mut bodies,
         &mut colliders,
-        &mut joints,
+        &mut impulse_joints,
         point![20.0, 5.0, 0.0],
         4,
     );
     create_actuated_prismatic_joints(
         &mut bodies,
         &mut colliders,
-        &mut joints,
+        &mut impulse_joints,
         point![25.0, 5.0, 0.0],
         4,
     );
     create_revolute_joints(
         &mut bodies,
         &mut colliders,
-        &mut joints,
+        &mut impulse_joints,
         point![20.0, 0.0, 0.0],
         3,
     );
     create_revolute_joints_with_limits(
         &mut bodies,
         &mut colliders,
-        &mut joints,
+        &mut impulse_joints,
         point![34.0, 0.0, 0.0],
     );
     create_fixed_joints(
         &mut bodies,
         &mut colliders,
-        &mut joints,
+        &mut impulse_joints,
         point![0.0, 10.0, 0.0],
         10,
     );
     create_actuated_revolute_joints(
         &mut bodies,
         &mut colliders,
-        &mut joints,
+        &mut impulse_joints,
         point![20.0, 10.0, 0.0],
         6,
     );
     create_actuated_ball_joints(
         &mut bodies,
         &mut colliders,
-        &mut joints,
+        &mut impulse_joints,
         point![13.0, 10.0, 0.0],
         3,
     );
-    create_ball_joints(&mut bodies, &mut colliders, &mut joints, 15);
+    create_ball_joints(&mut bodies, &mut colliders, &mut impulse_joints, 15);
     create_ball_joints_with_limits(
         &mut bodies,
         &mut colliders,
-        &mut joints,
+        &mut impulse_joints,
         point![-5.0, 0.0, 0.0],
     );
 
     /*
      * Set up the testbed.
      */
-    testbed.set_world(bodies, colliders, joints);
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![15.0, 5.0, 42.0], point![13.0, 1.0, 1.0]);
 }