Fix some solver issues

- Fix the wrong codepath taken  by the solver for contacts involving a collider without parent.
- Properly adress the non-linear treatment of the friction direction
- Simplify the sleeping strategy
- Add an impulse resolution multiplier

9 files changed, 81 insertions, 82 deletions

diff --git a/src_testbed/camera2d.rs b/src_testbed/camera2d.rs
index 46aacf1..1964385 100644
--- a/src_testbed/camera2d.rs
+++ b/src_testbed/camera2d.rs
@@ -39,10 +39,8 @@ impl OrbitCameraPlugin {
         mut query: Query<(&OrbitCamera, &mut Transform), (Changed<OrbitCamera>, With<Camera>)>,
     ) {
         for (camera, mut transform) in query.iter_mut() {
-            if camera.enabled {
-                transform.translation = camera.center;
-                transform.scale = Vec3::new(1.0 / camera.zoom, 1.0 / camera.zoom, 1.0);
-            }
+            transform.translation = camera.center;
+            transform.scale = Vec3::new(1.0 / camera.zoom, 1.0 / camera.zoom, 1.0);
         }
     }
 
diff --git a/src_testbed/camera3d.rs b/src_testbed/camera3d.rs
index 7df66da..979f8e8 100644
--- a/src_testbed/camera3d.rs
+++ b/src_testbed/camera3d.rs
@@ -50,12 +50,10 @@ impl OrbitCameraPlugin {
         mut query: Query<(&OrbitCamera, &mut Transform), (Changed<OrbitCamera>, With<Camera>)>,
     ) {
         for (camera, mut transform) in query.iter_mut() {
-            if camera.enabled {
-                let rot = Quat::from_axis_angle(Vec3::Y, camera.x)
-                    * Quat::from_axis_angle(-Vec3::X, camera.y);
-                transform.translation = (rot * Vec3::Y) * camera.distance + camera.center;
-                transform.look_at(camera.center, Vec3::Y);
-            }
+            let rot = Quat::from_axis_angle(Vec3::Y, camera.x)
+                * Quat::from_axis_angle(-Vec3::X, camera.y);
+            transform.translation = (rot * Vec3::Y) * camera.distance + camera.center;
+            transform.look_at(camera.center, Vec3::Y);
         }
     }
 
diff --git a/src_testbed/graphics.rs b/src_testbed/graphics.rs
index b623f26..d0dcf2d 100644
--- a/src_testbed/graphics.rs
+++ b/src_testbed/graphics.rs
@@ -2,10 +2,10 @@ use bevy::prelude::*;
 
 use na::{point, Point3};
 
-use crate::math::Isometry;
 use crate::objects::node::EntityWithGraphics;
 use rapier::dynamics::{RigidBodyHandle, RigidBodySet};
 use rapier::geometry::{ColliderHandle, ColliderSet, Shape, ShapeType};
+use rapier::math::{Isometry, Real};
 //use crate::objects::capsule::Capsule;
 //#[cfg(feature = "dim3")]
 //use crate::objects::mesh::Mesh;
@@ -301,8 +301,8 @@ impl GraphicsManager {
         handle: Option<ColliderHandle>,
         shape: &dyn Shape,
         sensor: bool,
-        pos: &Isometry<f32>,
-        delta: &Isometry<f32>,
+        pos: &Isometry<Real>,
+        delta: &Isometry<Real>,
         color: Point3<f32>,
         out: &mut Vec<EntityWithGraphics>,
     ) {
@@ -347,18 +347,24 @@ impl GraphicsManager {
         _bodies: &RigidBodySet,
         colliders: &ColliderSet,
         components: &mut Query<(&mut Transform,)>,
+        _materials: &mut Assets<BevyMaterial>,
     ) {
         for (_, ns) in self.b2sn.iter_mut() {
             for n in ns.iter_mut() {
-                // if let Some(co) = colliders.get(n.collider()) {
-                //     let bo = &_bodies[co.parent()];
-                //
-                //     if bo.is_dynamic() {
-                //         if bo.is_ccd_active() {
-                //             n.set_color(point![1.0, 0.0, 0.0]);
-                //         } else {
-                //             n.set_color(point![0.0, 1.0, 0.0]);
-                //         }
+                // if let Some(bo) = n
+                //     .collider
+                //     .and_then(|h| bodies.get(colliders.get(h)?.parent()?))
+                // {
+                //     if bo.activation().time_since_can_sleep
+                //         >= RigidBodyActivation::default_time_until_sleep()
+                //     {
+                //         n.set_color(materials, point![1.0, 0.0, 0.0]);
+                //     }
+                //     /* else if bo.activation().energy < bo.activation().threshold {
+                //         n.set_color(materials, point![0.0, 0.0, 1.0]);
+                //     } */
+                //     else {
+                //         n.set_color(materials, point![0.0, 1.0, 0.0]);
                 //     }
                 // }
 
diff --git a/src_testbed/harness/mod.rs b/src_testbed/harness/mod.rs
index 3358a70..beebe80 100644
--- a/src_testbed/harness/mod.rs
+++ b/src_testbed/harness/mod.rs
@@ -8,7 +8,7 @@ use rapier::dynamics::{
     RigidBodySet,
 };
 use rapier::geometry::{BroadPhase, ColliderSet, NarrowPhase};
-use rapier::math::Vector;
+use rapier::math::{Real, Vector};
 use rapier::pipeline::{ChannelEventCollector, PhysicsHooks, PhysicsPipeline, QueryPipeline};
 
 pub mod plugin;
@@ -131,7 +131,7 @@ impl Harness {
         colliders: ColliderSet,
         impulse_joints: ImpulseJointSet,
         multibody_joints: MultibodyJointSet,
-        gravity: Vector<f32>,
+        gravity: Vector<Real>,
         hooks: impl PhysicsHooks<RigidBodySet, ColliderSet> + 'static,
     ) {
         // println!("Num bodies: {}", bodies.len());
@@ -235,7 +235,7 @@ impl Harness {
 
         self.events.poll_all();
 
-        self.state.time += self.physics.integration_parameters.dt;
+        self.state.time += self.physics.integration_parameters.dt as f32;
         self.state.timestep_id += 1;
     }
 
diff --git a/src_testbed/lib.rs b/src_testbed/lib.rs
index b9a8822..bbf2d2f 100644
--- a/src_testbed/lib.rs
+++ b/src_testbed/lib.rs
@@ -1,12 +1,4 @@
 extern crate nalgebra as na;
-#[cfg(feature = "dim2")]
-extern crate parry2d as parry;
-#[cfg(feature = "dim3")]
-extern crate parry3d as parry;
-#[cfg(feature = "dim2")]
-extern crate rapier2d as rapier;
-#[cfg(feature = "dim3")]
-extern crate rapier3d as rapier;
 
 #[macro_use]
 extern crate bitflags;
diff --git a/src_testbed/objects/node.rs b/src_testbed/objects/node.rs
index 1310415..45a77db 100644
--- a/src_testbed/objects/node.rs
+++ b/src_testbed/objects/node.rs
@@ -10,7 +10,7 @@ use bevy::render::render_resource::PrimitiveTopology;
 use rapier::geometry::{ColliderHandle, ColliderSet, Shape, ShapeType};
 #[cfg(feature = "dim3")]
 use rapier::geometry::{Cone, Cylinder};
-use rapier::math::Isometry;
+use rapier::math::{Isometry, Real};
 
 use crate::graphics::BevyMaterial;
 #[cfg(feature = "dim2")]
@@ -26,7 +26,7 @@ pub struct EntityWithGraphics {
     pub color: Point3<f32>,
     pub base_color: Point3<f32>,
     pub collider: Option<ColliderHandle>,
-    pub delta: Isometry<f32>,
+    pub delta: Isometry<Real>,
     pub opacity: f32,
     material: Handle<BevyMaterial>,
 }
@@ -39,8 +39,8 @@ impl EntityWithGraphics {
         prefab_meshs: &HashMap<ShapeType, Handle<Mesh>>,
         shape: &dyn Shape,
         collider: Option<ColliderHandle>,
-        collider_pos: Isometry<f32>,
-        delta: Isometry<f32>,
+        collider_pos: Isometry<Real>,
+        delta: Isometry<Real>,
         color: Point3<f32>,
         sensor: bool,
     ) -> Self {
@@ -56,16 +56,16 @@ impl EntityWithGraphics {
         let bevy_color = Color::rgba(color.x, color.y, color.z, opacity);
         let shape_pos = collider_pos * delta;
         let mut transform = Transform::from_scale(scale);
-        transform.translation.x = shape_pos.translation.vector.x;
-        transform.translation.y = shape_pos.translation.vector.y;
+        transform.translation.x = shape_pos.translation.vector.x as f32;
+        transform.translation.y = shape_pos.translation.vector.y as f32;
         #[cfg(feature = "dim3")]
         {
-            transform.translation.z = shape_pos.translation.vector.z;
+            transform.translation.z = shape_pos.translation.vector.z as f32;
             transform.rotation = Quat::from_xyzw(
-                shape_pos.rotation.i,
-                shape_pos.rotation.j,
-                shape_pos.rotation.k,
-                shape_pos.rotation.w,
+                shape_pos.rotation.i as f32,
+                shape_pos.rotation.j as f32,
+                shape_pos.rotation.k as f32,
+                shape_pos.rotation.w as f32,
             );
         }
         #[cfg(feature = "dim2")]
@@ -73,7 +73,7 @@ impl EntityWithGraphics {
             if sensor {
                 transform.translation.z = -10.0;
             }
-            transform.rotation = Quat::from_rotation_z(shape_pos.rotation.angle());
+            transform.rotation = Quat::from_rotation_z(shape_pos.rotation.angle() as f32);
         }
 
         #[cfg(feature = "dim2")]
@@ -172,21 +172,21 @@ impl EntityWithGraphics {
         if let Some(Some(co)) = self.collider.map(|c| colliders.get(c)) {
             if let Ok(mut pos) = components.get_component_mut::<Transform>(self.entity) {
                 let co_pos = co.position() * self.delta;
-                pos.translation.x = co_pos.translation.vector.x;
-                pos.translation.y = co_pos.translation.vector.y;
+                pos.translation.x = co_pos.translation.vector.x as f32;
+                pos.translation.y = co_pos.translation.vector.y as f32;
                 #[cfg(feature = "dim3")]
                 {
-                    pos.translation.z = co_pos.translation.vector.z;
+                    pos.translation.z = co_pos.translation.vector.z as f32;
                     pos.rotation = Quat::from_xyzw(
-                        co_pos.rotation.i,
-                        co_pos.rotation.j,
-                        co_pos.rotation.k,
-                        co_pos.rotation.w,
+                        co_pos.rotation.i as f32,
+                        co_pos.rotation.j as f32,
+                        co_pos.rotation.k as f32,
+                        co_pos.rotation.w as f32,
                     );
                 }
                 #[cfg(feature = "dim2")]
                 {
-                    pos.rotation = Quat::from_rotation_z(co_pos.rotation.angle());
+                    pos.rotation = Quat::from_rotation_z(co_pos.rotation.angle() as f32);
                 }
             }
         }
@@ -266,7 +266,7 @@ impl EntityWithGraphics {
 }
 
 #[cfg(feature = "dim2")]
-fn bevy_mesh_from_polyline(vertices: Vec<Point2<f32>>) -> Mesh {
+fn bevy_mesh_from_polyline(vertices: Vec<Point2<Real>>) -> Mesh {
     let n = vertices.len();
     let idx = (1..n as u32 - 1).map(|i| [0, i, i + 1]).collect();
     let vtx = vertices
@@ -277,7 +277,7 @@ fn bevy_mesh_from_polyline(vertices: Vec<Point2<f32>>) -> Mesh {
 }
 
 #[cfg(feature = "dim2")]
-fn bevy_polyline(buffers: (Vec<Point2<f32>>, Option<Vec<[u32; 2]>>)) -> Mesh {
+fn bevy_polyline(buffers: (Vec<Point2<Real>>, Option<Vec<[u32; 2]>>)) -> Mesh {
     let (vtx, idx) = buffers;
     // let mut normals: Vec<[f32; 3]> = vec![];
     let mut vertices: Vec<[f32; 3]> = vec![];
@@ -287,11 +287,11 @@ fn bevy_polyline(buffers: (Vec<Point2<f32>>, Option<Vec<[u32; 2]>>)) -> Mesh {
             let a = vtx[idx[0] as usize];
             let b = vtx[idx[1] as usize];
 
-            vertices.push([a.x, a.y, 0.0]);
-            vertices.push([b.x, b.y, 0.0]);
+            vertices.push([a.x as f32, a.y as f32, 0.0]);
+            vertices.push([b.x as f32, b.y as f32, 0.0]);
         }
     } else {
-        vertices = vtx.iter().map(|v| [v.x, v.y, 0.0]).collect();
+        vertices = vtx.iter().map(|v| [v.x as f32, v.y as f32, 0.0]).collect();
     }
 
     let indices: Vec<_> = (0..vertices.len() as u32).collect();
@@ -310,7 +310,7 @@ fn bevy_polyline(buffers: (Vec<Point2<f32>>, Option<Vec<[u32; 2]>>)) -> Mesh {
     mesh
 }
 
-fn bevy_mesh(buffers: (Vec<Point3<f32>>, Vec<[u32; 3]>)) -> Mesh {
+fn bevy_mesh(buffers: (Vec<Point3<Real>>, Vec<[u32; 3]>)) -> Mesh {
     let (vtx, idx) = buffers;
     let mut normals: Vec<[f32; 3]> = vec![];
     let mut vertices: Vec<[f32; 3]> = vec![];
@@ -320,9 +320,9 @@ fn bevy_mesh(buffers: (Vec<Point3<f32>>, Vec<[u32; 3]>)) -> Mesh {
         let b = vtx[idx[1] as usize];
         let c = vtx[idx[2] as usize];
 
-        vertices.push(a.into());
-        vertices.push(b.into());
-        vertices.push(c.into());
+        vertices.push(a.cast::<f32>().into());
+        vertices.push(b.cast::<f32>().into());
+        vertices.push(c.cast::<f32>().into());
     }
 
     for vtx in vertices.chunks(3) {
@@ -330,9 +330,9 @@ fn bevy_mesh(buffers: (Vec<Point3<f32>>, Vec<[u32; 3]>)) -> Mesh {
         let b = Point3::from(vtx[1]);
         let c = Point3::from(vtx[2]);
         let n = (b - a).cross(&(c - a)).normalize();
-        normals.push(n.into());
-        normals.push(n.into());
-        normals.push(n.into());
+        normals.push(n.cast::<f32>().into());
+        normals.push(n.cast::<f32>().into());
+        normals.push(n.cast::<f32>().into());
     }
 
     normals
@@ -358,36 +358,36 @@ fn collider_mesh_scale(co_shape: &dyn Shape) -> Vec3 {
         #[cfg(feature = "dim2")]
         ShapeType::Cuboid => {
             let c = co_shape.as_cuboid().unwrap();
-            Vec3::new(c.half_extents.x, c.half_extents.y, 1.0)
+            Vec3::new(c.half_extents.x as f32, c.half_extents.y as f32, 1.0)
         }
         ShapeType::Ball => {
             let b = co_shape.as_ball().unwrap();
-            Vec3::new(b.radius, b.radius, b.radius)
+            Vec3::new(b.radius as f32, b.radius as f32, b.radius as f32)
         }
         #[cfg(feature = "dim3")]
         ShapeType::Cuboid => {
             let c = co_shape.as_cuboid().unwrap();
-            Vec3::from_slice(c.half_extents.as_slice())
+            Vec3::from_slice(c.half_extents.cast::<f32>().as_slice())
         }
         #[cfg(feature = "dim3")]
         ShapeType::Cylinder => {
             let c = co_shape.as_cylinder().unwrap();
-            Vec3::new(c.radius, c.half_height, c.radius)
+            Vec3::new(c.radius as f32, c.half_height as f32, c.radius as f32)
         }
         #[cfg(feature = "dim3")]
         ShapeType::RoundCylinder => {
             let c = &co_shape.as_round_cylinder().unwrap().base_shape;
-            Vec3::new(c.radius, c.half_height, c.radius)
+            Vec3::new(c.radius as f32, c.half_height as f32, c.radius as f32)
         }
         #[cfg(feature = "dim3")]
         ShapeType::Cone => {
             let c = co_shape.as_cone().unwrap();
-            Vec3::new(c.radius, c.half_height, c.radius)
+            Vec3::new(c.radius as f32, c.half_height as f32, c.radius as f32)
         }
         #[cfg(feature = "dim3")]
         ShapeType::RoundCone => {
             let c = &co_shape.as_round_cone().unwrap().base_shape;
-            Vec3::new(c.radius, c.half_height, c.radius)
+            Vec3::new(c.radius as f32, c.half_height as f32, c.radius as f32)
         }
         _ => Vec3::ONE,
     }
diff --git a/src_testbed/physics/mod.rs b/src_testbed/physics/mod.rs
index 13bf915..c3f21ef 100644
--- a/src_testbed/physics/mod.rs
+++ b/src_testbed/physics/mod.rs
@@ -4,7 +4,7 @@ use rapier::dynamics::{
     RigidBodySet,
 };
 use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, IntersectionEvent, NarrowPhase};
-use rapier::math::Vector;
+use rapier::math::{Real, Vector};
 use rapier::pipeline::{PhysicsHooks, PhysicsPipeline, QueryPipeline};
 
 pub struct PhysicsSnapshot {
@@ -82,7 +82,7 @@ pub struct PhysicsState {
     pub pipeline: PhysicsPipeline,
     pub query_pipeline: QueryPipeline,
     pub integration_parameters: IntegrationParameters,
-    pub gravity: Vector<f32>,
+    pub gravity: Vector<Real>,
     pub hooks: Box<dyn PhysicsHooks<RigidBodySet, ColliderSet>>,
 }
 
diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs
index ce6544e..75eb50b 100644
--- a/src_testbed/testbed.rs
+++ b/src_testbed/testbed.rs
@@ -16,7 +16,7 @@ use rapier::dynamics::{
 use rapier::geometry::{ColliderHandle, ColliderSet, NarrowPhase};
 #[cfg(feature = "dim3")]
 use rapier::geometry::{InteractionGroups, Ray};
-use rapier::math::Vector;
+use rapier::math::{Real, Vector};
 use rapier::pipeline::PhysicsHooks;
 
 #[cfg(all(feature = "dim2", feature = "other-backends"))]
@@ -487,7 +487,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
         colliders: ColliderSet,
         impulse_joints: ImpulseJointSet,
         multibody_joints: MultibodyJointSet,
-        gravity: Vector<f32>,
+        gravity: Vector<Real>,
         hooks: impl PhysicsHooks<RigidBodySet, ColliderSet> + 'static,
     ) {
         self.harness.set_world_with_params(
@@ -1129,12 +1129,15 @@ fn update_testbed(
         {
             if state.flags.contains(TestbedStateFlags::SLEEP) {
                 for (_, body) in harness.physics.bodies.iter_mut() {
-                    body.activation_mut().threshold = RigidBodyActivation::default_threshold();
+                    body.activation_mut().linear_threshold =
+                        RigidBodyActivation::default_linear_threshold();
+                    body.activation_mut().angular_threshold =
+                        RigidBodyActivation::default_angular_threshold();
                 }
             } else {
                 for (_, body) in harness.physics.bodies.iter_mut() {
                     body.wake_up(true);
-                    body.activation_mut().threshold = -1.0;
+                    body.activation_mut().linear_threshold = -1.0;
                 }
             }
         }
@@ -1226,6 +1229,7 @@ fn update_testbed(
         &harness.physics.bodies,
         &harness.physics.colliders,
         &mut gfx_components,
+        &mut *materials,
     );
 
     for plugin in &mut plugins.0 {
@@ -1299,14 +1303,14 @@ fn highlight_hovered_body(
         let ray_pt1 = ndc_to_world.project_point3(Vec3::new(ndc_cursor.x, ndc_cursor.y, -1.0));
         let ray_pt2 = ndc_to_world.project_point3(Vec3::new(ndc_cursor.x, ndc_cursor.y, 1.0));
         let ray_dir = ray_pt2 - ray_pt1;
-        let ray_origin = Point3::new(ray_pt1.x, ray_pt1.y, ray_pt1.z);
-        let ray_dir = Vector3::new(ray_dir.x, ray_dir.y, ray_dir.z);
+        let ray_origin = Point3::new(ray_pt1.x as Real, ray_pt1.y as Real, ray_pt1.z as Real);
+        let ray_dir = Vector3::new(ray_dir.x as Real, ray_dir.y as Real, ray_dir.z as Real);
 
         let ray = Ray::new(ray_origin, ray_dir);
         let hit = physics.query_pipeline.cast_ray(
             &physics.colliders,
             &ray,
-            f32::MAX,
+            Real::MAX,
             true,
             InteractionGroups::all(),
             None,
diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs
index 894b6a4..788cc8f 100644
--- a/src_testbed/ui.rs
+++ b/src_testbed/ui.rs
@@ -1,4 +1,5 @@
 use rapier::counters::Counters;
+use rapier::math::Real;
 
 use crate::harness::Harness;
 use crate::testbed::{
@@ -147,7 +148,7 @@ pub fn update_ui(ui_context: &EguiContext, state: &mut TestbedState, harness: &m
         );
         let mut frequency = integration_parameters.inv_dt().round() as u32;
         ui.add(Slider::new(&mut frequency, 0..=240).text("frequency (Hz)"));
-        integration_parameters.set_inv_dt(frequency as f32);
+        integration_parameters.set_inv_dt(frequency as Real);
 
         let mut sleep = state.flags.contains(TestbedStateFlags::SLEEP);
         // let mut contact_points = state.flags.contains(TestbedStateFlags::CONTACT_POINTS);