Add 2-axes coupling for angular joint limits

1 files changed, 232 insertions, 79 deletions

diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs
index 8b2c258..73783d8 100644
--- a/examples3d/joints3.rs
+++ b/examples3d/joints3.rs
@@ -1,12 +1,43 @@
 use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
 
+fn create_coupled_joints(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    impulse_joints: &mut ImpulseJointSet,
+    multibody_joints: &mut MultibodyJointSet,
+    origin: Point<f32>,
+    use_articulations: bool,
+) {
+    let ground = bodies.insert(RigidBodyBuilder::new_static().translation(origin.coords));
+    let body1 = bodies.insert(
+        RigidBodyBuilder::new_dynamic()
+            .translation(origin.coords)
+            .linvel(vector![5.0, 5.0, 5.0]),
+    );
+    colliders.insert_with_parent(ColliderBuilder::cuboid(1.0, 1.0, 1.0), body1, bodies);
+
+    let joint1 = GenericJointBuilder::new(JointAxesMask::empty())
+        .limits(JointAxis::X, [-3.0, 3.0])
+        .limits(JointAxis::Y, [0.0, 3.0])
+        .limits(JointAxis::Z, [0.0, 3.0])
+        .coupled_axes(JointAxesMask::Y | JointAxesMask::Z);
+
+    if use_articulations {
+        multibody_joints.insert(ground, body1, joint1);
+    } else {
+        impulse_joints.insert(ground, body1, joint1);
+    }
+}
+
 fn create_prismatic_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
     impulse_joints: &mut ImpulseJointSet,
+    multibody_joints: &mut MultibodyJointSet,
     origin: Point<f32>,
     num: usize,
+    use_articulations: bool,
 ) {
     let rad = 0.4;
     let shift = 2.0;
@@ -30,13 +61,16 @@ fn create_prismatic_joints(
             UnitVector::new_normalize(vector![-1.0f32, 1.0, 0.0])
         };
 
-        let mut prism = GenericJoint::prismatic(axis)
-            .local_anchor1(point![0.0, 0.0, shift])
-            .local_anchor2(point![0.0, 0.0, 0.0])
-            .limits(JointAxis::X, [-2.0, 2.0]);
-
-        impulse_joints.insert(curr_parent, curr_child, prism);
+        let prism = PrismaticJointBuilder::new(axis)
+            .local_anchor1(point![0.0, 0.0, 0.0])
+            .local_anchor2(point![0.0, 0.0, -shift])
+            .limits([-2.0, 2.0]);
 
+        if use_articulations {
+            multibody_joints.insert(curr_parent, curr_child, prism);
+        } else {
+            impulse_joints.insert(curr_parent, curr_child, prism);
+        }
         curr_parent = curr_child;
     }
 }
@@ -45,8 +79,10 @@ fn create_actuated_prismatic_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
     impulse_joints: &mut ImpulseJointSet,
+    multibody_joints: &mut MultibodyJointSet,
     origin: Point<f32>,
     num: usize,
+    use_articulations: bool,
 ) {
     let rad = 0.4;
     let shift = 2.0;
@@ -70,29 +106,36 @@ fn create_actuated_prismatic_joints(
             UnitVector::new_normalize(vector![-1.0, 1.0, 0.0])
         };
 
-        let mut prism = GenericJoint::prismatic(axis)
-            .local_anchor1(point![0.0, 0.0, 0.0])
-            .local_anchor2(point![0.0, 0.0, -shift]);
+        let mut prism = PrismaticJointBuilder::new(axis)
+            .local_anchor1(point![0.0, 0.0, shift])
+            .local_anchor2(point![0.0, 0.0, 0.0])
+            .build();
 
-        if i == 1 {
-            prism = prism
-                .limits(JointAxis::X, [-Real::MAX, 5.0])
-                .motor_velocity(JointAxis::X, 1.0, 1.0)
+        if i == 0 {
+            prism
+                .set_motor_velocity(2.0, 1.0e5)
                 // 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 = prism.motor_position(JointAxis::X, 2.0, 0.01, 1.0);
-        } else {
-            prism = prism
-                .motor_velocity(JointAxis::X, 1.0, 1.0)
+                .set_limits([-2.0, 5.0])
+                .set_motor_max_force(100.0);
+        } else if i == 1 {
+            prism
+                .set_limits([-Real::MAX, 5.0])
+                .set_motor_velocity(6.0, 1.0e3)
                 // We set a max impulse so that the motor doesn't fight
                 // the limits with large forces.
-                .motor_max_impulse(JointAxis::X, 0.7)
-                .limits(JointAxis::X, [-2.0, 5.0]);
+                .set_motor_max_force(100.0);
+        } else if i > 1 {
+            prism
+                .set_motor_position(2.0, 1.0e3, 1.0e2)
+                .set_motor_max_force(60.0);
         }
 
-        impulse_joints.insert(curr_parent, curr_child, prism);
+        if use_articulations {
+            multibody_joints.insert(curr_parent, curr_child, prism);
+        } else {
+            impulse_joints.insert(curr_parent, curr_child, prism);
+        }
 
         curr_parent = curr_child;
     }
@@ -102,8 +145,10 @@ fn create_revolute_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
     impulse_joints: &mut ImpulseJointSet,
+    multibody_joints: &mut MultibodyJointSet,
     origin: Point<f32>,
     num: usize,
+    use_articulations: bool,
 ) {
     let rad = 0.4;
     let shift = 2.0;
@@ -135,16 +180,23 @@ fn create_revolute_joints(
         let x = Vector::x_axis();
         let z = Vector::z_axis();
         let revs = [
-            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]),
+            RevoluteJointBuilder::new(z).local_anchor2(point![0.0, 0.0, -shift]),
+            RevoluteJointBuilder::new(x).local_anchor2(point![-shift, 0.0, 0.0]),
+            RevoluteJointBuilder::new(z).local_anchor2(point![0.0, 0.0, -shift]),
+            RevoluteJointBuilder::new(x).local_anchor2(point![shift, 0.0, 0.0]),
         ];
 
-        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]);
+        if use_articulations {
+            multibody_joints.insert(curr_parent, handles[0], revs[0]);
+            multibody_joints.insert(handles[0], handles[1], revs[1]);
+            multibody_joints.insert(handles[1], handles[2], revs[2]);
+            multibody_joints.insert(handles[2], handles[3], revs[3]);
+        } else {
+            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];
     }
@@ -154,7 +206,9 @@ fn create_revolute_joints_with_limits(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
     impulse_joints: &mut ImpulseJointSet,
+    multibody_joints: &mut MultibodyJointSet,
     origin: Point<f32>,
+    use_articulations: bool,
 ) {
     let ground = bodies.insert(RigidBodyBuilder::new_static().translation(origin.coords));
 
@@ -167,13 +221,39 @@ fn create_revolute_joints_with_limits(
     colliders.insert_with_parent(ColliderBuilder::cuboid(4.0, 0.2, 2.0), platform2, bodies);
 
     let z = Vector::z_axis();
-    let mut joint1 = RevoluteJoint::new(z).limits(JointAxis::X, [-0.2, 0.2]);
-    impulse_joints.insert(ground, platform1, joint1);
+    let joint1 = RevoluteJointBuilder::new(z).limits([-0.2, 0.2]);
+    // let joint1 = GenericJointBuilder::new(JointAxesMask::X | JointAxesMask::Y | JointAxesMask::Z)
+    //     .local_axis1(z)
+    //     .local_axis2(z)
+    //     .limits(JointAxis::AngX, [-0.2, 0.2])
+    //     .limits(JointAxis::AngY, [0.0, 0.4])
+    //     .limits(JointAxis::AngZ, [0.0, 0.4])
+    //     .coupled_axes(JointAxesMask::ANG_Y | JointAxesMask::ANG_Z);
+
+    if use_articulations {
+        multibody_joints.insert(ground, platform1, joint1);
+    } else {
+        impulse_joints.insert(ground, platform1, joint1);
+    }
 
-    let mut joint2 = RevoluteJoint::new(z)
-        .local_anchor2(shift.into())
-        .limits(JointAxis::Z, [-0.2, 0.2]);
-    impulse_joints.insert(platform1, platform2, joint2);
+    let joint2 = RevoluteJointBuilder::new(z)
+        .local_anchor2(-Point::from(shift))
+        .limits([-0.2, 0.2]);
+
+    // let joint2 = GenericJointBuilder::new(JointAxesMask::X | JointAxesMask::Y | JointAxesMask::Z)
+    //     .local_axis1(z)
+    //     .local_axis2(z)
+    //     .local_anchor2(-Point::from(shift))
+    //     .limits(JointAxis::AngX, [-0.2, 0.2])
+    //     .limits(JointAxis::AngY, [0.0, 0.4])
+    //     .limits(JointAxis::AngZ, [0.0, 0.4])
+    //     .coupled_axes(JointAxesMask::ANG_Y | JointAxesMask::ANG_Z);
+
+    if use_articulations {
+        multibody_joints.insert(platform1, platform2, joint2);
+    } else {
+        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(
@@ -200,8 +280,10 @@ fn create_fixed_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
     impulse_joints: &mut ImpulseJointSet,
+    multibody_joints: &mut MultibodyJointSet,
     origin: Point<f32>,
     num: usize,
+    use_articulations: bool,
 ) {
     let rad = 0.4;
     let shift = 1.0;
@@ -235,15 +317,20 @@ fn create_fixed_joints(
             if i > 0 {
                 let parent_index = body_handles.len() - num;
                 let parent_handle = body_handles[parent_index];
-                let joint = GenericJoint::fixed().local_anchor2(point![0.0, 0.0, -shift]);
-                impulse_joints.insert(parent_handle, child_handle, joint);
+                let joint = FixedJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift]);
+
+                if use_articulations {
+                    multibody_joints.insert(parent_handle, child_handle, joint);
+                } else {
+                    impulse_joints.insert(parent_handle, child_handle, joint);
+                }
             }
 
             // Horizontal joint.
             if k > 0 {
                 let parent_index = body_handles.len() - 1;
                 let parent_handle = body_handles[parent_index];
-                let joint = GenericJoint::fixed().local_anchor2(point![-shift, 0.0, 0.0]);
+                let joint = FixedJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]);
                 impulse_joints.insert(parent_handle, child_handle, joint);
             }
 
@@ -252,11 +339,13 @@ fn create_fixed_joints(
     }
 }
 
-fn create_ball_joints(
+fn create_spherical_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
     impulse_joints: &mut ImpulseJointSet,
+    multibody_joints: &mut MultibodyJointSet,
     num: usize,
+    use_articulations: bool,
 ) {
     let rad = 0.4;
     let shift = 1.0;
@@ -286,15 +375,21 @@ fn create_ball_joints(
             // Vertical joint.
             if i > 0 {
                 let parent_handle = *body_handles.last().unwrap();
-                let joint = GenericJoint::ball().local_anchor2(point![0.0, 0.0, -shift * 2.0]);
-                impulse_joints.insert(parent_handle, child_handle, joint);
+                let joint =
+                    SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift * 2.0]);
+
+                if use_articulations {
+                    multibody_joints.insert(parent_handle, child_handle, joint);
+                } else {
+                    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 = GenericJoint::ball().local_anchor2(point![-shift, 0.0, 0.0]);
+                let joint = SphericalJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]);
                 impulse_joints.insert(parent_handle, child_handle, joint);
             }
 
@@ -303,11 +398,13 @@ fn create_ball_joints(
     }
 }
 
-fn create_ball_joints_with_limits(
+fn create_spherical_joints_with_limits(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
     impulse_joints: &mut ImpulseJointSet,
+    multibody_joints: &mut MultibodyJointSet,
     origin: Point<f32>,
+    use_articulations: bool,
 ) {
     let shift = vector![0.0, 0.0, 3.0];
 
@@ -324,32 +421,40 @@ fn create_ball_joints_with_limits(
         bodies.insert(RigidBodyBuilder::new_dynamic().translation(origin.coords + shift * 2.0));
     colliders.insert_with_parent(ColliderBuilder::cuboid(1.0, 1.0, 1.0), ball2, bodies);
 
-    let mut joint1 = GenericJoint::ball()
+    let joint1 = SphericalJointBuilder::new()
         .local_anchor2(Point::from(-shift))
         .limits(JointAxis::X, [-0.2, 0.2])
         .limits(JointAxis::Y, [-0.2, 0.2]);
-    impulse_joints.insert(ground, ball1, joint1);
 
-    let mut joint2 = GenericJoint::ball()
+    let joint2 = SphericalJointBuilder::new()
         .local_anchor2(Point::from(-shift))
         .limits(JointAxis::X, [-0.3, 0.3])
         .limits(JointAxis::Y, [-0.3, 0.3]);
-    impulse_joints.insert(ball1, ball2, joint2);
+
+    if use_articulations {
+        multibody_joints.insert(ground, ball1, joint1);
+        multibody_joints.insert(ball1, ball2, joint2);
+    } else {
+        impulse_joints.insert(ground, ball1, joint1);
+        impulse_joints.insert(ball1, ball2, joint2);
+    }
 }
 
 fn create_actuated_revolute_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
     impulse_joints: &mut ImpulseJointSet,
+    multibody_joints: &mut MultibodyJointSet,
     origin: Point<f32>,
     num: usize,
+    use_articulations: bool,
 ) {
     let rad = 0.4;
     let shift = 2.0;
 
     // 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 joint_template = RevoluteJointBuilder::new(z).local_anchor2(point![0.0, 0.0, -shift]);
 
     let mut parent_handle = RigidBodyHandle::invalid();
 
@@ -377,42 +482,47 @@ fn create_actuated_revolute_joints(
         colliders.insert_with_parent(collider, child_handle, bodies);
 
         if i > 0 {
-            let mut joint = joint_template
-                .clone()
-                .motor_model(MotorModel::AccelerationBased);
+            let mut joint = joint_template.motor_model(MotorModel::AccelerationBased);
 
             if i % 3 == 1 {
-                joint.set_motor_velocity(JointAxis::AngX, -20.0, 0.1);
+                joint = joint.motor_velocity(-20.0, 100.0);
             } else if i == num - 1 {
-                let stiffness = 0.2;
-                let damping = 1.0;
-                jointset_.motor_position(JointAxis::AngX, 3.14 / 2.0, stiffness, damping);
+                let stiffness = 200.0;
+                let damping = 100.0;
+                joint = joint.motor_position(3.14 / 2.0, stiffness, damping);
             }
 
             if i == 1 {
-                joint.local_frame2.translation.vector.y = 2.0;
-                joint.set_motor_velocity(JointAxis::AngX, -2.0, 0.1);
+                joint = joint
+                    .local_anchor2(point![0.0, 2.0, -shift])
+                    .motor_velocity(-2.0, 1000.0);
             }
 
-            impulse_joints.insert(parent_handle, child_handle, joint);
+            if use_articulations {
+                multibody_joints.insert(parent_handle, child_handle, joint);
+            } else {
+                impulse_joints.insert(parent_handle, child_handle, joint);
+            }
         }
 
         parent_handle = child_handle;
     }
 }
 
-fn create_actuated_ball_joints(
+fn create_actuated_spherical_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
     impulse_joints: &mut ImpulseJointSet,
+    multibody_joints: &mut MultibodyJointSet,
     origin: Point<f32>,
     num: usize,
+    use_articulations: bool,
 ) {
     let rad = 0.4;
     let shift = 2.0;
 
     // We will reuse this base configuration for all the impulse_joints here.
-    let joint_template = GenericJoint::ball().local_anchor1(point![0.0, 0.0, shift]);
+    let joint_template = SphericalJointBuilder::new().local_anchor1(point![0.0, 0.0, shift]);
 
     let mut parent_handle = RigidBodyHandle::invalid();
 
@@ -454,76 +564,111 @@ fn create_actuated_ball_joints(
                     .motor_position(JointAxis::AngZ, 3.14 / 2.0, stiffness, damping);
             }
 
-            impulse_joints.insert(parent_handle, child_handle, joint);
+            if use_articulations {
+                multibody_joints.insert(parent_handle, child_handle, joint);
+            } else {
+                impulse_joints.insert(parent_handle, child_handle, joint);
+            }
         }
 
         parent_handle = child_handle;
     }
 }
 
-pub fn init_world(testbed: &mut Testbed) {
+fn do_init_world(testbed: &mut Testbed, use_articulations: bool) {
     /*
      * World
      */
     let mut bodies = RigidBodySet::new();
     let mut colliders = ColliderSet::new();
     let mut impulse_joints = ImpulseJointSet::new();
-    let multibody_joints = MultibodyJointSet::new();
+    let mut multibody_joints = MultibodyJointSet::new();
 
     // create_prismatic_joints(
     //     &mut bodies,
     //     &mut colliders,
     //     &mut impulse_joints,
+    //     &mut multibody_joints,
     //     point![20.0, 5.0, 0.0],
     //     4,
+    //     use_articulations,
     // );
     // create_actuated_prismatic_joints(
     //     &mut bodies,
     //     &mut colliders,
     //     &mut impulse_joints,
+    //     &mut multibody_joints,
     //     point![25.0, 5.0, 0.0],
     //     4,
+    //     use_articulations,
     // );
     // create_revolute_joints(
     //     &mut bodies,
     //     &mut colliders,
     //     &mut impulse_joints,
+    //     &mut multibody_joints,
     //     point![20.0, 0.0, 0.0],
     //     3,
+    //     use_articulations,
     // );
-    // create_revolute_joints_with_limits(
+    create_revolute_joints_with_limits(
+        &mut bodies,
+        &mut colliders,
+        &mut impulse_joints,
+        &mut multibody_joints,
+        point![34.0, 0.0, 0.0],
+        use_articulations,
+    );
+    // create_fixed_joints(
     //     &mut bodies,
     //     &mut colliders,
     //     &mut impulse_joints,
-    //     point![34.0, 0.0, 0.0],
+    //     &mut multibody_joints,
+    //     point![0.0, 10.0, 0.0],
+    //     10,
+    //     use_articulations,
     // );
-    // create_fixed_joints(
+    // create_actuated_revolute_joints(
     //     &mut bodies,
     //     &mut colliders,
     //     &mut impulse_joints,
-    //     point![0.0, 10.0, 0.0],
-    //     10,
+    //     &mut multibody_joints,
+    //     point![20.0, 10.0, 0.0],
+    //     6,
+    //     use_articulations,
     // );
-    create_actuated_revolute_joints(
-        &mut bodies,
-        &mut colliders,
-        &mut impulse_joints,
-        point![20.0, 10.0, 0.0],
-        6,
-    );
-    // create_actuated_ball_joints(
+    // create_actuated_spherical_joints(
     //     &mut bodies,
     //     &mut colliders,
     //     &mut impulse_joints,
+    //     &mut multibody_joints,
     //     point![13.0, 10.0, 0.0],
     //     3,
+    //     use_articulations,
+    // );
+    // create_spherical_joints(
+    //     &mut bodies,
+    //     &mut colliders,
+    //     &mut impulse_joints,
+    //     &mut multibody_joints,
+    //     15,
+    //     use_articulations,
     // );
-    // create_ball_joints(&mut bodies, &mut colliders, &mut impulse_joints, 15);
-    // create_ball_joints_with_limits(
+    // create_spherical_joints_with_limits(
     //     &mut bodies,
     //     &mut colliders,
     //     &mut impulse_joints,
+    //     &mut multibody_joints,
     //     point![-5.0, 0.0, 0.0],
+    //     use_articulations,
+    // );
+    // create_coupled_joints(
+    //     &mut bodies,
+    //     &mut colliders,
+    //     &mut impulse_joints,
+    //     &mut multibody_joints,
+    //     point![0.0, 20.0, 0.0],
+    //     use_articulations,
     // );
 
     /*
@@ -532,3 +677,11 @@ pub fn init_world(testbed: &mut Testbed) {
     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]);
 }
+
+pub fn init_world_with_joints(testbed: &mut Testbed) {
+    do_init_world(testbed, false)
+}
+
+pub fn init_world_with_articulations(testbed: &mut Testbed) {
+    do_init_world(testbed, true)
+}