Merge pull request #282 from dimforge/critical-damping

Improve the CFM implementation

3 files changed, 24 insertions, 9 deletions

diff --git a/src_testbed/graphics.rs b/src_testbed/graphics.rs
index d0dcf2d..8bf787d 100644
--- a/src_testbed/graphics.rs
+++ b/src_testbed/graphics.rs
@@ -5,7 +5,7 @@ use na::{point, Point3};
 use crate::objects::node::EntityWithGraphics;
 use rapier::dynamics::{RigidBodyHandle, RigidBodySet};
 use rapier::geometry::{ColliderHandle, ColliderSet, Shape, ShapeType};
-use rapier::math::{Isometry, Real};
+use rapier::math::{Isometry, Real, Vector};
 //use crate::objects::capsule::Capsule;
 //#[cfg(feature = "dim3")]
 //use crate::objects::mesh::Mesh;
@@ -30,6 +30,7 @@ pub struct GraphicsManager {
     b2wireframe: HashMap<RigidBodyHandle, bool>,
     ground_color: Point3<f32>,
     prefab_meshes: HashMap<ShapeType, Handle<Mesh>>,
+    pub gfx_shift: Vector<Real>,
 }
 
 impl GraphicsManager {
@@ -42,6 +43,7 @@ impl GraphicsManager {
             ground_color: point![0.5, 0.5, 0.5],
             b2wireframe: HashMap::new(),
             prefab_meshes: HashMap::new(),
+            gfx_shift: Vector::zeros(),
         }
     }
 
@@ -239,7 +241,7 @@ impl GraphicsManager {
 
         new_nodes
             .iter_mut()
-            .for_each(|n| n.update(colliders, components));
+            .for_each(|n| n.update(colliders, components, &self.gfx_shift));
 
         // for node in new_nodes.iter_mut().filter_map(|n| n.scene_node_mut()) {
         //     if self.b2wireframe.get(&handle).cloned() == Some(true) {
@@ -368,7 +370,7 @@ impl GraphicsManager {
                 //     }
                 // }
 
-                n.update(colliders, components);
+                n.update(colliders, components, &self.gfx_shift);
             }
         }
     }
diff --git a/src_testbed/objects/node.rs b/src_testbed/objects/node.rs
index 45a77db..6a4807f 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, Real};
+use rapier::math::{Isometry, Real, Vector};
 
 use crate::graphics::BevyMaterial;
 #[cfg(feature = "dim2")]
@@ -168,15 +168,20 @@ impl EntityWithGraphics {
         self.base_color = color;
     }
 
-    pub fn update(&mut self, colliders: &ColliderSet, components: &mut Query<(&mut Transform,)>) {
+    pub fn update(
+        &mut self,
+        colliders: &ColliderSet,
+        components: &mut Query<(&mut Transform,)>,
+        gfx_shift: &Vector<Real>,
+    ) {
         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 as f32;
-                pos.translation.y = co_pos.translation.vector.y as f32;
+                pos.translation.x = (co_pos.translation.vector.x + gfx_shift.x) as f32;
+                pos.translation.y = (co_pos.translation.vector.y + gfx_shift.y) as f32;
                 #[cfg(feature = "dim3")]
                 {
-                    pos.translation.z = co_pos.translation.vector.z as f32;
+                    pos.translation.z = (co_pos.translation.vector.z + gfx_shift.z) as f32;
                     pos.rotation = Quat::from_xyzw(
                         co_pos.rotation.i as f32,
                         co_pos.rotation.j as f32,
diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs
index 75eb50b..dba40fd 100644
--- a/src_testbed/testbed.rs
+++ b/src_testbed/testbed.rs
@@ -362,7 +362,7 @@ impl TestbedApp {
                 vsync: true,
                 ..Default::default()
             })
-            .insert_resource(ClearColor(Color::rgb(0.85, 0.85, 0.85)))
+            .insert_resource(ClearColor(Color::rgb(0.15, 0.15, 0.15)))
             .insert_resource(Msaa { samples: 4 })
             .insert_resource(WgpuOptions {
                 // Required for wireframes.
@@ -536,6 +536,14 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
         }
     }
 
+    pub fn set_graphics_shift(&mut self, shift: Vector<Real>) {
+        if !self.state.camera_locked {
+            if let Some(graphics) = &mut self.graphics {
+                graphics.graphics.gfx_shift = shift;
+            }
+        }
+    }
+
     #[cfg(feature = "dim2")]
     pub fn look_at(&mut self, at: Point2<f32>, zoom: f32) {
         if !self.state.camera_locked {