Merge pull request #51 from dimforge/restitution

Add restitution.

examples2d/all_examples2.rs

@@ -17,6 +17,7 @@ mod heightfield2;
 mod joints2;
 mod platform2;
 mod pyramid2;
+mod restitution2;
 mod sensor2;
 
 fn demo_name_from_command_line() -> Option<String> {
@@ -58,6 +59,7 @@ pub fn main() {
         ("Joints", joints2::init_world),
         ("Platform", platform2::init_world),
         ("Pyramid", pyramid2::init_world),
+        ("Restitution", restitution2::init_world),
         ("Sensor", sensor2::init_world),
         ("(Debug) box ball", debug_box_ball2::init_world),
     ];

examples2d/restitution2.rs

@@ -0,0 +1,56 @@
+use na::Point2;
+use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 20.;
+    let ground_height = 1.0;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height)
+        .restitution(1.0)
+        .build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    let num = 10;
+    let rad = 0.5;
+
+    for j in 0..2 {
+        for i in 0..=num {
+            let x = (i as f32) - num as f32 / 2.0;
+            let rigid_body = RigidBodyBuilder::new_dynamic()
+                .translation(x * 2.0, 10.0 * (j as f32 + 1.0))
+                .build();
+            let handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::ball(rad)
+                .restitution((i as f32) / (num as f32))
+                .build();
+            colliders.insert(collider, handle, &mut bodies);
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point2::new(0.0, 1.0), 25.0);
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}

examples3d/all_examples3.rs

@@ -23,6 +23,7 @@ mod joints3;
 mod keva3;
 mod platform3;
 mod primitives3;
+mod restitution3;
 mod sensor3;
 mod stacks3;
 mod trimesh3;
@@ -72,6 +73,7 @@ pub fn main() {
         ("Heightfield", heightfield3::init_world),
         ("Joints", joints3::init_world),
         ("Platform", platform3::init_world),
+        ("Restitution", restitution3::init_world),
         ("Stacks", stacks3::init_world),
         ("Sensor", sensor3::init_world),
         ("Trimesh", trimesh3::init_world),

examples3d/restitution3.rs

@@ -0,0 +1,56 @@
+use na::Point3;
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 20.;
+    let ground_height = 1.0;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, 2.0)
+        .restitution(1.0)
+        .build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    let num = 10;
+    let rad = 0.5;
+
+    for j in 0..2 {
+        for i in 0..=num {
+            let x = (i as f32) - num as f32 / 2.0;
+            let rigid_body = RigidBodyBuilder::new_dynamic()
+                .translation(x * 2.0, 10.0 * (j as f32 + 1.0), 0.0)
+                .build();
+            let handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::ball(rad)
+                .restitution((i as f32) / (num as f32))
+                .build();
+            colliders.insert(collider, handle, &mut bodies);
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(0.0, 3.0, 30.0), Point3::new(0.0, 3.0, 0.0));
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}

src/dynamics/solver/velocity_constraint.rs

@@ -238,8 +238,13 @@ impl VelocityConstraint {
                             + gcross1.gdot(gcross1)
                             + gcross2.gdot(gcross2));
 
-                    let rhs = (vel1 - vel2).dot(&force_dir1)
-                        + manifold_point.dist.max(0.0) * params.inv_dt();
+                    let mut rhs = (vel1 - vel2).dot(&force_dir1);
+
+                    if rhs <= -params.restitution_velocity_threshold {
+                        rhs += manifold.restitution * rhs
+                    }
+
+                    rhs += manifold_point.dist.max(0.0) * params.inv_dt();
 
                     let impulse = manifold_points[k].impulse * warmstart_coeff;
 

src/dynamics/solver/velocity_constraint_wide.rs

@@ -104,6 +104,10 @@ impl WVelocityConstraint {
         let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
 
         let friction = SimdFloat::from(array![|ii| manifolds[ii].friction; SIMD_WIDTH]);
+        let restitution = SimdFloat::from(array![|ii| manifolds[ii].restitution; SIMD_WIDTH]);
+        let restitution_velocity_threshold =
+            SimdFloat::splat(params.restitution_velocity_threshold);
+
         let warmstart_multiplier =
             SimdFloat::from(array![|ii| manifolds[ii].warmstart_multiplier; SIMD_WIDTH]);
         let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff);
@@ -151,8 +155,11 @@ impl WVelocityConstraint {
 
                     let r = SimdFloat::splat(1.0)
                         / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
-                    let rhs =
-                        (vel1 - vel2).dot(&force_dir1) + dist.simd_max(SimdFloat::zero()) * inv_dt;
+                    let mut rhs = (vel1 - vel2).dot(&force_dir1);
+                    let use_restitution = rhs.simd_le(-restitution_velocity_threshold);
+                    let rhs_with_restitution = rhs + rhs * restitution;
+                    rhs = rhs_with_restitution.select(use_restitution, rhs);
+                    rhs += dist.simd_max(SimdFloat::zero()) * inv_dt;
 
                     constraint.elements[k].normal_part = WVelocityConstraintElementPart {
                         gcross1,

src/dynamics/solver/velocity_ground_constraint.rs

@@ -169,9 +169,15 @@ impl VelocityGroundConstraint {
                         .transform_vector(dp2.gcross(-force_dir1));
 
                     let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2));
-                    let rhs = -vel2.dot(&force_dir1)
-                        + vel1.dot(&force_dir1)
-                        + manifold_point.dist.max(0.0) * params.inv_dt();
+
+                    let mut rhs = (vel1 - vel2).dot(&force_dir1);
+
+                    if rhs <= -params.restitution_velocity_threshold {
+                        rhs += manifold.restitution * rhs
+                    }
+
+                    rhs += manifold_point.dist.max(0.0) * params.inv_dt();
+
                     let impulse = manifold_points[k].impulse * warmstart_coeff;
 
                     constraint.elements[k].normal_part = VelocityGroundConstraintElementPart {

src/dynamics/solver/velocity_ground_constraint_wide.rs

@@ -110,6 +110,10 @@ impl WVelocityGroundConstraint {
         let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
 
         let friction = SimdFloat::from(array![|ii| manifolds[ii].friction; SIMD_WIDTH]);
+        let restitution = SimdFloat::from(array![|ii| manifolds[ii].restitution; SIMD_WIDTH]);
+        let restitution_velocity_threshold =
+            SimdFloat::splat(params.restitution_velocity_threshold);
+
         let warmstart_multiplier =
             SimdFloat::from(array![|ii| manifolds[ii].warmstart_multiplier; SIMD_WIDTH]);
         let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff);
@@ -154,9 +158,11 @@ impl WVelocityGroundConstraint {
                     let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
 
                     let r = SimdFloat::splat(1.0) / (im2 + gcross2.gdot(gcross2));
-                    let rhs = -vel2.dot(&force_dir1)
-                        + vel1.dot(&force_dir1)
-                        + dist.simd_max(SimdFloat::zero()) * inv_dt;
+                    let mut rhs = (vel1 - vel2).dot(&force_dir1);
+                    let use_restitution = rhs.simd_le(-restitution_velocity_threshold);
+                    let rhs_with_restitution = rhs + rhs * restitution;
+                    rhs = rhs_with_restitution.select(use_restitution, rhs);
+                    rhs += dist.simd_max(SimdFloat::zero()) * inv_dt;
 
                     constraint.elements[k].normal_part = WVelocityGroundConstraintElementPart {
                         gcross2,

src_testbed/box2d_backend.rs

@@ -161,7 +161,7 @@ impl Box2dWorld {
         let center = na_vec_to_b2_vec(collider.position_wrt_parent().translation.vector);
         let mut fixture_def = b2::FixtureDef::new();
 
-        fixture_def.restitution = 0.0;
+        fixture_def.restitution = collider.restitution;
         fixture_def.friction = collider.friction;
         fixture_def.density = collider.density();
         fixture_def.is_sensor = collider.is_sensor();

src_testbed/nphysics_backend.rs

@@ -45,7 +45,7 @@ impl NPhysicsWorld {
         let force_generators = DefaultForceGeneratorSet::new();
 
         for (rapier_handle, rb) in bodies.iter() {
-            //            let material = physics.create_material(rb.collider.friction, rb.collider.friction, 0.0);
+            // let material = physics.create_material(rb.collider.friction, rb.collider.friction, 0.0);
             let nphysics_rb = RigidBodyDesc::new().position(rb.position).build();
             let nphysics_rb_handle = nphysics_bodies.insert(nphysics_rb);