Joint API and joint motors improvements

1 files changed, 118 insertions, 164 deletions

diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs
index 02aa5f9..8b2c258 100644
--- a/examples3d/joints3.rs
+++ b/examples3d/joints3.rs
@@ -11,20 +11,17 @@ fn create_prismatic_joints(
     let rad = 0.4;
     let shift = 2.0;
 
-    let ground = RigidBodyBuilder::new_static()
-        .translation(vector![origin.x, origin.y, origin.z])
-        .build();
+    let ground = RigidBodyBuilder::new_static().translation(vector![origin.x, origin.y, origin.z]);
     let mut curr_parent = bodies.insert(ground);
-    let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+    let collider = ColliderBuilder::cuboid(rad, rad, rad);
     colliders.insert_with_parent(collider, curr_parent, bodies);
 
     for i in 0..num {
         let z = origin.z + (i + 1) as f32 * shift;
-        let rigid_body = RigidBodyBuilder::new_dynamic()
-            .translation(vector![origin.x, origin.y, z])
-            .build();
+        let rigid_body =
+            RigidBodyBuilder::new_dynamic().translation(vector![origin.x, origin.y, z]);
         let curr_child = bodies.insert(rigid_body);
-        let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+        let collider = ColliderBuilder::cuboid(rad, rad, rad);
         colliders.insert_with_parent(collider, curr_child, bodies);
 
         let axis = if i % 2 == 0 {
@@ -33,10 +30,10 @@ fn create_prismatic_joints(
             UnitVector::new_normalize(vector![-1.0f32, 1.0, 0.0])
         };
 
-        let mut prism = JointData::prismatic(axis)
+        let mut prism = GenericJoint::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]);
+            .limits(JointAxis::X, [-2.0, 2.0]);
 
         impulse_joints.insert(curr_parent, curr_child, prism);
 
@@ -54,20 +51,17 @@ fn create_actuated_prismatic_joints(
     let rad = 0.4;
     let shift = 2.0;
 
-    let ground = RigidBodyBuilder::new_static()
-        .translation(vector![origin.x, origin.y, origin.z])
-        .build();
+    let ground = RigidBodyBuilder::new_static().translation(vector![origin.x, origin.y, origin.z]);
     let mut curr_parent = bodies.insert(ground);
-    let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+    let collider = ColliderBuilder::cuboid(rad, rad, rad);
     colliders.insert_with_parent(collider, curr_parent, bodies);
 
     for i in 0..num {
         let z = origin.z + (i + 1) as f32 * shift;
-        let rigid_body = RigidBodyBuilder::new_dynamic()
-            .translation(vector![origin.x, origin.y, z])
-            .build();
+        let rigid_body =
+            RigidBodyBuilder::new_dynamic().translation(vector![origin.x, origin.y, z]);
         let curr_child = bodies.insert(rigid_body);
-        let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+        let collider = ColliderBuilder::cuboid(rad, rad, rad);
         colliders.insert_with_parent(collider, curr_child, bodies);
 
         let axis = if i % 2 == 0 {
@@ -76,13 +70,13 @@ fn create_actuated_prismatic_joints(
             UnitVector::new_normalize(vector![-1.0, 1.0, 0.0])
         };
 
-        let mut prism = JointData::prismatic(axis)
+        let mut prism = GenericJoint::prismatic(axis)
             .local_anchor1(point![0.0, 0.0, 0.0])
             .local_anchor2(point![0.0, 0.0, -shift]);
 
         if i == 1 {
             prism = prism
-                .limit_axis(JointAxis::X, [-Real::MAX, 5.0])
+                .limits(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.
@@ -95,7 +89,7 @@ fn create_actuated_prismatic_joints(
                 // 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]);
+                .limits(JointAxis::X, [-2.0, 5.0]);
         }
 
         impulse_joints.insert(curr_parent, curr_child, prism);
@@ -114,11 +108,9 @@ fn create_revolute_joints(
     let rad = 0.4;
     let shift = 2.0;
 
-    let ground = RigidBodyBuilder::new_static()
-        .translation(vector![origin.x, origin.y, 0.0])
-        .build();
+    let ground = RigidBodyBuilder::new_static().translation(vector![origin.x, origin.y, 0.0]);
     let mut curr_parent = bodies.insert(ground);
-    let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+    let collider = ColliderBuilder::cuboid(rad, rad, rad);
     colliders.insert_with_parent(collider, curr_parent, bodies);
 
     for i in 0..num {
@@ -133,11 +125,9 @@ fn create_revolute_joints(
 
         let mut handles = [curr_parent; 4];
         for k in 0..4 {
-            let rigid_body = RigidBodyBuilder::new_dynamic()
-                .position(positions[k])
-                .build();
+            let rigid_body = RigidBodyBuilder::new_dynamic().position(positions[k]);
             handles[k] = bodies.insert(rigid_body);
-            let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+            let collider = ColliderBuilder::cuboid(rad, rad, rad);
             colliders.insert_with_parent(collider, handles[k], bodies);
         }
 
@@ -166,63 +156,41 @@ fn create_revolute_joints_with_limits(
     impulse_joints: &mut ImpulseJointSet,
     origin: Point<f32>,
 ) {
-    let ground = bodies.insert(
-        RigidBodyBuilder::new_static()
-            .translation(origin.coords)
-            .build(),
-    );
+    let ground = bodies.insert(RigidBodyBuilder::new_static().translation(origin.coords));
 
-    let platform1 = bodies.insert(
-        RigidBodyBuilder::new_dynamic()
-            .translation(origin.coords)
-            .build(),
-    );
-    colliders.insert_with_parent(
-        ColliderBuilder::cuboid(4.0, 0.2, 2.0).build(),
-        platform1,
-        bodies,
-    );
+    let platform1 = bodies.insert(RigidBodyBuilder::new_dynamic().translation(origin.coords));
+    colliders.insert_with_parent(ColliderBuilder::cuboid(4.0, 0.2, 2.0), platform1, bodies);
 
     let shift = vector![0.0, 0.0, 6.0];
-    let platform2 = bodies.insert(
-        RigidBodyBuilder::new_dynamic()
-            .translation(origin.coords + shift)
-            .build(),
-    );
-    colliders.insert_with_parent(
-        ColliderBuilder::cuboid(4.0, 0.2, 2.0).build(),
-        platform2,
-        bodies,
-    );
+    let platform2 =
+        bodies.insert(RigidBodyBuilder::new_dynamic().translation(origin.coords + shift));
+    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).limit_axis(JointAxis::X, [-0.2, 0.2]);
+    let mut joint1 = RevoluteJoint::new(z).limits(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]);
+        .limits(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(
-        RigidBodyBuilder::new_dynamic()
-            .translation(origin.coords + vector![-2.0, 4.0, 0.0])
-            .build(),
+        RigidBodyBuilder::new_dynamic().translation(origin.coords + vector![-2.0, 4.0, 0.0]),
     );
     colliders.insert_with_parent(
-        ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0).build(),
+        ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0),
         cuboid_body1,
         bodies,
     );
 
     let cuboid_body2 = bodies.insert(
         RigidBodyBuilder::new_dynamic()
-            .translation(origin.coords + shift + vector![2.0, 16.0, 0.0])
-            .build(),
+            .translation(origin.coords + shift + vector![2.0, 16.0, 0.0]),
     );
     colliders.insert_with_parent(
-        ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0).build(),
+        ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0),
         cuboid_body2,
         bodies,
     );
@@ -254,22 +222,20 @@ fn create_fixed_joints(
                 RigidBodyType::Dynamic
             };
 
-            let rigid_body = RigidBodyBuilder::new(status)
-                .translation(vector![
-                    origin.x + fk * shift,
-                    origin.y,
-                    origin.z + fi * shift
-                ])
-                .build();
+            let rigid_body = RigidBodyBuilder::new(status).translation(vector![
+                origin.x + fk * shift,
+                origin.y,
+                origin.z + fi * shift
+            ]);
             let child_handle = bodies.insert(rigid_body);
-            let collider = ColliderBuilder::ball(rad).build();
+            let collider = ColliderBuilder::ball(rad);
             colliders.insert_with_parent(collider, child_handle, bodies);
 
             // Vertical joint.
             if i > 0 {
                 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]);
+                let joint = GenericJoint::fixed().local_anchor2(point![0.0, 0.0, -shift]);
                 impulse_joints.insert(parent_handle, child_handle, joint);
             }
 
@@ -277,7 +243,7 @@ fn create_fixed_joints(
             if k > 0 {
                 let parent_index = body_handles.len() - 1;
                 let parent_handle = body_handles[parent_index];
-                let joint = JointData::fixed().local_anchor2(point![-shift, 0.0, 0.0]);
+                let joint = GenericJoint::fixed().local_anchor2(point![-shift, 0.0, 0.0]);
                 impulse_joints.insert(parent_handle, child_handle, joint);
             }
 
@@ -308,17 +274,19 @@ fn create_ball_joints(
                 RigidBodyType::Dynamic
             };
 
-            let rigid_body = RigidBodyBuilder::new(status)
-                .translation(vector![fk * shift, 0.0, fi * shift * 2.0])
-                .build();
+            let rigid_body = RigidBodyBuilder::new(status).translation(vector![
+                fk * shift,
+                0.0,
+                fi * shift * 2.0
+            ]);
             let child_handle = bodies.insert(rigid_body);
-            let collider = ColliderBuilder::capsule_z(rad * 1.25, rad).build();
+            let collider = ColliderBuilder::capsule_z(rad * 1.25, rad);
             colliders.insert_with_parent(collider, child_handle, bodies);
 
             // Vertical joint.
             if i > 0 {
                 let parent_handle = *body_handles.last().unwrap();
-                let joint = JointData::ball().local_anchor2(point![0.0, 0.0, -shift * 2.0]);
+                let joint = GenericJoint::ball().local_anchor2(point![0.0, 0.0, -shift * 2.0]);
                 impulse_joints.insert(parent_handle, child_handle, joint);
             }
 
@@ -326,7 +294,7 @@ fn create_ball_joints(
             if k > 0 {
                 let parent_index = body_handles.len() - num;
                 let parent_handle = body_handles[parent_index];
-                let joint = JointData::ball().local_anchor2(point![-shift, 0.0, 0.0]);
+                let joint = GenericJoint::ball().local_anchor2(point![-shift, 0.0, 0.0]);
                 impulse_joints.insert(parent_handle, child_handle, joint);
             }
 
@@ -343,45 +311,29 @@ fn create_ball_joints_with_limits(
 ) {
     let shift = vector![0.0, 0.0, 3.0];
 
-    let ground = bodies.insert(
-        RigidBodyBuilder::new_static()
-            .translation(origin.coords)
-            .build(),
-    );
+    let ground = bodies.insert(RigidBodyBuilder::new_static().translation(origin.coords));
 
     let ball1 = bodies.insert(
         RigidBodyBuilder::new_dynamic()
             .translation(origin.coords + shift)
-            .linvel(vector![20.0, 20.0, 0.0])
-            .build(),
-    );
-    colliders.insert_with_parent(
-        ColliderBuilder::cuboid(1.0, 1.0, 1.0).build(),
-        ball1,
-        bodies,
+            .linvel(vector![20.0, 20.0, 0.0]),
     );
+    colliders.insert_with_parent(ColliderBuilder::cuboid(1.0, 1.0, 1.0), ball1, bodies);
 
-    let ball2 = bodies.insert(
-        RigidBodyBuilder::new_dynamic()
-            .translation(origin.coords + shift * 2.0)
-            .build(),
-    );
-    colliders.insert_with_parent(
-        ColliderBuilder::cuboid(1.0, 1.0, 1.0).build(),
-        ball2,
-        bodies,
-    );
+    let ball2 =
+        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 = JointData::ball()
+    let mut joint1 = GenericJoint::ball()
         .local_anchor2(Point::from(-shift))
-        .limit_axis(JointAxis::X, [-0.2, 0.2])
-        .limit_axis(JointAxis::Y, [-0.2, 0.2]);
+        .limits(JointAxis::X, [-0.2, 0.2])
+        .limits(JointAxis::Y, [-0.2, 0.2]);
     impulse_joints.insert(ground, ball1, joint1);
 
-    let mut joint2 = JointData::ball()
+    let mut joint2 = GenericJoint::ball()
         .local_anchor2(Point::from(-shift))
-        .limit_axis(JointAxis::X, [-0.3, 0.3])
-        .limit_axis(JointAxis::Y, [-0.3, 0.3]);
+        .limits(JointAxis::X, [-0.3, 0.3])
+        .limits(JointAxis::Y, [-0.3, 0.3]);
     impulse_joints.insert(ball1, ball2, joint2);
 }
 
@@ -418,26 +370,28 @@ fn create_actuated_revolute_joints(
         let rigid_body = RigidBodyBuilder::new(status)
             .translation(vector![origin.x, origin.y + shifty, origin.z + fi * shift])
             // .rotation(Vector3::new(0.0, fi * 1.1, 0.0))
-            .build();
+            ;
 
         let child_handle = bodies.insert(rigid_body);
-        let collider = ColliderBuilder::cuboid(rad * 2.0, rad * 6.0 / (fi + 1.0), rad).build();
+        let collider = ColliderBuilder::cuboid(rad * 2.0, rad * 6.0 / (fi + 1.0), rad);
         colliders.insert_with_parent(collider, child_handle, bodies);
 
         if i > 0 {
-            let mut joint = joint_template.clone();
+            let mut joint = joint_template
+                .clone()
+                .motor_model(MotorModel::AccelerationBased);
 
             if i % 3 == 1 {
-                joint = joint.motor_velocity(JointAxis::AngX, -20.0, 0.1);
+                joint.set_motor_velocity(JointAxis::AngX, -20.0, 0.1);
             } else if i == num - 1 {
                 let stiffness = 0.2;
                 let damping = 1.0;
-                joint = joint.motor_position(JointAxis::AngX, 3.14 / 2.0, stiffness, damping);
+                jointset_.motor_position(JointAxis::AngX, 3.14 / 2.0, stiffness, damping);
             }
 
             if i == 1 {
                 joint.local_frame2.translation.vector.y = 2.0;
-                joint = joint.motor_velocity(JointAxis::AngX, -2.0, 0.1);
+                joint.set_motor_velocity(JointAxis::AngX, -2.0, 0.1);
             }
 
             impulse_joints.insert(parent_handle, child_handle, joint);
@@ -458,7 +412,7 @@ fn create_actuated_ball_joints(
     let shift = 2.0;
 
     // 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 joint_template = GenericJoint::ball().local_anchor1(point![0.0, 0.0, shift]);
 
     let mut parent_handle = RigidBodyHandle::invalid();
 
@@ -477,10 +431,10 @@ fn create_actuated_ball_joints(
         let rigid_body = RigidBodyBuilder::new(status)
             .translation(vector![origin.x, origin.y, origin.z + fi * shift])
             // .rotation(Vector3::new(0.0, fi * 1.1, 0.0))
-            .build();
+            ;
 
         let child_handle = bodies.insert(rigid_body);
-        let collider = ColliderBuilder::capsule_y(rad * 2.0 / (fi + 1.0), rad).build();
+        let collider = ColliderBuilder::capsule_y(rad * 2.0 / (fi + 1.0), rad);
         colliders.insert_with_parent(collider, child_handle, bodies);
 
         if i > 0 {
@@ -516,40 +470,40 @@ pub fn init_world(testbed: &mut Testbed) {
     let mut impulse_joints = ImpulseJointSet::new();
     let multibody_joints = MultibodyJointSet::new();
 
-    create_prismatic_joints(
-        &mut bodies,
-        &mut colliders,
-        &mut impulse_joints,
-        point![20.0, 5.0, 0.0],
-        4,
-    );
-    create_actuated_prismatic_joints(
-        &mut bodies,
-        &mut colliders,
-        &mut impulse_joints,
-        point![25.0, 5.0, 0.0],
-        4,
-    );
-    create_revolute_joints(
-        &mut bodies,
-        &mut colliders,
-        &mut impulse_joints,
-        point![20.0, 0.0, 0.0],
-        3,
-    );
-    create_revolute_joints_with_limits(
-        &mut bodies,
-        &mut colliders,
-        &mut impulse_joints,
-        point![34.0, 0.0, 0.0],
-    );
-    create_fixed_joints(
-        &mut bodies,
-        &mut colliders,
-        &mut impulse_joints,
-        point![0.0, 10.0, 0.0],
-        10,
-    );
+    // create_prismatic_joints(
+    //     &mut bodies,
+    //     &mut colliders,
+    //     &mut impulse_joints,
+    //     point![20.0, 5.0, 0.0],
+    //     4,
+    // );
+    // create_actuated_prismatic_joints(
+    //     &mut bodies,
+    //     &mut colliders,
+    //     &mut impulse_joints,
+    //     point![25.0, 5.0, 0.0],
+    //     4,
+    // );
+    // create_revolute_joints(
+    //     &mut bodies,
+    //     &mut colliders,
+    //     &mut impulse_joints,
+    //     point![20.0, 0.0, 0.0],
+    //     3,
+    // );
+    // create_revolute_joints_with_limits(
+    //     &mut bodies,
+    //     &mut colliders,
+    //     &mut impulse_joints,
+    //     point![34.0, 0.0, 0.0],
+    // );
+    // create_fixed_joints(
+    //     &mut bodies,
+    //     &mut colliders,
+    //     &mut impulse_joints,
+    //     point![0.0, 10.0, 0.0],
+    //     10,
+    // );
     create_actuated_revolute_joints(
         &mut bodies,
         &mut colliders,
@@ -557,20 +511,20 @@ pub fn init_world(testbed: &mut Testbed) {
         point![20.0, 10.0, 0.0],
         6,
     );
-    create_actuated_ball_joints(
-        &mut bodies,
-        &mut colliders,
-        &mut impulse_joints,
-        point![13.0, 10.0, 0.0],
-        3,
-    );
-    create_ball_joints(&mut bodies, &mut colliders, &mut impulse_joints, 15);
-    create_ball_joints_with_limits(
-        &mut bodies,
-        &mut colliders,
-        &mut impulse_joints,
-        point![-5.0, 0.0, 0.0],
-    );
+    // create_actuated_ball_joints(
+    //     &mut bodies,
+    //     &mut colliders,
+    //     &mut impulse_joints,
+    //     point![13.0, 10.0, 0.0],
+    //     3,
+    // );
+    // create_ball_joints(&mut bodies, &mut colliders, &mut impulse_joints, 15);
+    // create_ball_joints_with_limits(
+    //     &mut bodies,
+    //     &mut colliders,
+    //     &mut impulse_joints,
+    //     point![-5.0, 0.0, 0.0],
+    // );
 
     /*
      * Set up the testbed.