Merge pull request #28 from dimforge/raycast

Add the QueryPipeline for ray-casting and other geometrical queries in the future

5 files changed, 114 insertions, 68 deletions

diff --git a/src_testbed/box2d_backend.rs b/src_testbed/box2d_backend.rs
index 07cef1e..c25ff1f 100644
--- a/src_testbed/box2d_backend.rs
+++ b/src_testbed/box2d_backend.rs
@@ -98,10 +98,10 @@ impl Box2dWorld {
 
     fn insert_joints(&mut self, joints: &JointSet) {
         for joint in joints.iter() {
-            let body_a = self.rapier2box2d[&joint.body1];
-            let body_b = self.rapier2box2d[&joint.body2];
+            let body_a = self.rapier2box2d[&joint.1.body1];
+            let body_b = self.rapier2box2d[&joint.1.body2];
 
-            match &joint.params {
+            match &joint.1.params {
                 JointParams::BallJoint(params) => {
                     let def = RevoluteJointDef {
                         body_a,
@@ -158,7 +158,7 @@ impl Box2dWorld {
     }
 
     fn create_fixture(collider: &Collider, body: &mut b2::MetaBody<NoUserData>) {
-        let center = na_vec_to_b2_vec(collider.delta().translation.vector);
+        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;
@@ -182,7 +182,7 @@ impl Box2dWorld {
                 let points: Vec<_> = poly
                     .vertices()
                     .iter()
-                    .map(|p| collider.delta() * p)
+                    .map(|p| collider.position_wrt_parent() * p)
                     .map(|p| na_vec_to_b2_vec(p.coords))
                     .collect();
                 let b2_shape = b2::PolygonShape::new_with(&points);
@@ -229,7 +229,7 @@ impl Box2dWorld {
 
                 for coll_handle in body.colliders() {
                     let collider = &mut colliders[*coll_handle];
-                    collider.set_position_debug(pos * collider.delta());
+                    collider.set_position_debug(pos * collider.position_wrt_parent());
                 }
             }
         }
diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs
index 62ab37e..217da48 100644
--- a/src_testbed/engine.rs
+++ b/src_testbed/engine.rs
@@ -609,7 +609,7 @@ impl GraphicsManager {
         }
     }
 
-    pub fn draw(&mut self, colliders: &ColliderSet, window: &mut Window) {
+    pub fn draw(&mut self, _bodies: &RigidBodySet, colliders: &ColliderSet, window: &mut Window) {
         // use kiss3d::camera::Camera;
         // println!(
         //     "camera eye {:?}, at: {:?}",
@@ -618,6 +618,20 @@ impl GraphicsManager {
         // );
         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_sleeping() {
+                            n.set_color(Point3::new(1.0, 0.0, 0.0));
+                        } else {
+                            n.set_color(Point3::new(0.0, 1.0, 0.0));
+                        }
+                    }
+                }
+                 */
+
                 n.update(colliders);
                 n.draw(window);
             }
diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs
index c9ff2d1..c2a7fb1 100644
--- a/src_testbed/nphysics_backend.rs
+++ b/src_testbed/nphysics_backend.rs
@@ -66,10 +66,10 @@ impl NPhysicsWorld {
         }
 
         for joint in joints.iter() {
-            let b1 = BodyPartHandle(rapier2nphysics[&joint.body1], 0);
-            let b2 = BodyPartHandle(rapier2nphysics[&joint.body2], 0);
+            let b1 = BodyPartHandle(rapier2nphysics[&joint.1.body1], 0);
+            let b2 = BodyPartHandle(rapier2nphysics[&joint.1.body2], 0);
 
-            match &joint.params {
+            match &joint.1.params {
                 JointParams::FixedJoint(params) => {
                     let c = FixedConstraint::new(
                         b1,
diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs
index b06fd7e..ec2e2bf 100644
--- a/src_testbed/physx_backend.rs
+++ b/src_testbed/physx_backend.rs
@@ -196,7 +196,7 @@ impl PhysxWorld {
                     }
                 } else {
                     physx_sys::PxShapeFlags {
-                        mBits: physx_sys::PxShapeFlag::eSIMULATION_SHAPE as u8,
+                        mBits: physx_sys::PxShapeFlag::eSIMULATION_SHAPE as u8, // | physx_sys::PxShapeFlag::eSCENE_QUERY_SHAPE as u8,
                     }
                 };
 
@@ -261,10 +261,10 @@ impl PhysxWorld {
     fn setup_joints(&mut self, joints: &JointSet) {
         unsafe {
             for joint in joints.iter() {
-                let actor1 = self.rapier2physx[&joint.body1];
-                let actor2 = self.rapier2physx[&joint.body2];
+                let actor1 = self.rapier2physx[&joint.1.body1];
+                let actor2 = self.rapier2physx[&joint.1.body2];
 
-                match &joint.params {
+                match &joint.1.params {
                     JointParams::BallJoint(params) => {
                         let frame1 = physx::transform::gl_to_px_tf(
                             Isometry3::new(params.local_anchor1.coords, na::zero()).into_glam(),
diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs
index 92aea5a..3d7fd7d 100644
--- a/src_testbed/testbed.rs
+++ b/src_testbed/testbed.rs
@@ -21,9 +21,9 @@ use na::{self, Point2, Point3, Vector3};
 use rapier::dynamics::{
     ActivationStatus, IntegrationParameters, JointSet, RigidBodyHandle, RigidBodySet,
 };
-use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, NarrowPhase, ProximityEvent};
+use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, NarrowPhase, ProximityEvent, Ray};
 use rapier::math::Vector;
-use rapier::pipeline::{ChannelEventCollector, PhysicsPipeline};
+use rapier::pipeline::{ChannelEventCollector, PhysicsPipeline, QueryPipeline};
 #[cfg(feature = "fluids")]
 use salva::{coupling::ColliderCouplingSet, object::FluidHandle, LiquidWorld};
 
@@ -179,6 +179,7 @@ pub struct PhysicsState {
     pub colliders: ColliderSet,
     pub joints: JointSet,
     pub pipeline: PhysicsPipeline,
+    pub query_pipeline: QueryPipeline,
 }
 
 impl PhysicsState {
@@ -190,6 +191,7 @@ impl PhysicsState {
             colliders: ColliderSet::new(),
             joints: JointSet::new(),
             pipeline: PhysicsPipeline::new(),
+            query_pipeline: QueryPipeline::new(),
         }
     }
 }
@@ -197,6 +199,7 @@ impl PhysicsState {
 pub struct TestbedState {
     pub running: RunMode,
     pub draw_colls: bool,
+    pub highlighted_body: Option<RigidBodyHandle>,
     //    pub grabbed_object: Option<DefaultBodyPartHandle>,
     //    pub grabbed_object_constraint: Option<DefaultJointConstraintHandle>,
     pub grabbed_object_plane: (Point3<f32>, Vector3<f32>),
@@ -240,7 +243,7 @@ pub struct Testbed {
     hide_counters: bool,
     //    persistant_contacts: HashMap<ContactId, bool>,
     font: Rc<Font>,
-    // cursor_pos: Point2<f32>,
+    cursor_pos: Point2<f32>,
     events: PhysicsEvents,
     event_handler: ChannelEventCollector,
     ui: Option<TestbedUi>,
@@ -301,6 +304,7 @@ impl Testbed {
         let state = TestbedState {
             running: RunMode::Running,
             draw_colls: false,
+            highlighted_body: None,
             //            grabbed_object: None,
             //            grabbed_object_constraint: None,
             grabbed_object_plane: (Point3::origin(), na::zero()),
@@ -349,7 +353,7 @@ impl Testbed {
             hide_counters: true,
             //            persistant_contacts: HashMap::new(),
             font: Font::default(),
-            // cursor_pos: Point2::new(0.0f32, 0.0),
+            cursor_pos: Point2::new(0.0f32, 0.0),
             ui,
             event_handler,
             events,
@@ -408,6 +412,9 @@ impl Testbed {
             .set(TestbedActionFlags::RESET_WORLD_GRAPHICS, true);
         self.time = 0.0;
         self.state.timestep_id = 0;
+        self.state.highlighted_body = None;
+        self.physics.query_pipeline = QueryPipeline::new();
+        self.physics.pipeline = PhysicsPipeline::new();
         self.physics.pipeline.counters.enable();
 
         #[cfg(all(feature = "dim2", feature = "other-backends"))]
@@ -668,6 +675,10 @@ impl Testbed {
                                     &self.event_handler,
                                 );
 
+                                self.physics
+                                    .query_pipeline
+                                    .update(&self.physics.bodies, &self.physics.colliders);
+
                                 #[cfg(feature = "fluids")]
                                 {
                                     fluids_time = instant::now();
@@ -802,13 +813,9 @@ impl Testbed {
 
                 let num_to_delete = (colliders.len() / 10).max(1);
                 for to_delete in &colliders[..num_to_delete] {
-                    self.physics.pipeline.remove_collider(
-                        to_delete[0],
-                        &mut self.physics.broad_phase,
-                        &mut self.physics.narrow_phase,
-                        &mut self.physics.bodies,
-                        &mut self.physics.colliders,
-                    );
+                    self.physics
+                        .colliders
+                        .remove(to_delete[0], &mut self.physics.bodies);
                 }
             }
             WindowEvent::Key(Key::D, Action::Release, _) => {
@@ -817,21 +824,22 @@ impl Testbed {
                     .physics
                     .bodies
                     .iter()
-                    .filter(|e| e.1.is_dynamic())
+                    .filter(|e| !e.1.is_static())
                     .map(|e| e.0)
                     .collect();
                 let num_to_delete = (dynamic_bodies.len() / 10).max(1);
                 for to_delete in &dynamic_bodies[..num_to_delete] {
-                    self.physics.pipeline.remove_rigid_body(
+                    self.physics.bodies.remove(
                         *to_delete,
-                        &mut self.physics.broad_phase,
-                        &mut self.physics.narrow_phase,
-                        &mut self.physics.bodies,
                         &mut self.physics.colliders,
                         &mut self.physics.joints,
                     );
                 }
             }
+            WindowEvent::CursorPos(x, y, _) => {
+                self.cursor_pos.x = x as f32;
+                self.cursor_pos.y = y as f32;
+            }
             _ => {}
         }
 
@@ -1146,36 +1154,45 @@ impl Testbed {
                 self.state.grabbed_object = None;
                 self.state.grabbed_object_constraint = None;
             }
-            WindowEvent::CursorPos(x, y, modifiers) => {
-                self.cursor_pos.x = x as f32;
-                self.cursor_pos.y = y as f32;
+            _ => {}
+        }
+        */
+    }
 
-                // update the joint
-                if let Some(joint) = self.state.grabbed_object_constraint {
-                    let size = window.size();
-                    let (pos, dir) = self
-                        .graphics
-                        .camera()
-                        .unproject(&self.cursor_pos, &na::convert(size));
-                    let (ref ppos, ref pdir) = self.state.grabbed_object_plane;
+    #[cfg(feature = "dim2")]
+    fn highlight_hovered_body(&mut self, _window: &Window) {
+        // Do nothing for now.
+    }
 
-                    if let Some(inter) = query::ray_toi_with_plane(ppos, pdir, &Ray::new(pos, dir))
-                    {
-                        let joint = self
-                            .constraints
-                            .get_mut(joint)
-                            .unwrap()
-                            .downcast_mut::<MouseConstraint<f32, RigidBodyHandle>>()
-                            .unwrap();
-                        joint.set_anchor_1(pos + dir * inter)
-                    }
+    #[cfg(feature = "dim3")]
+    fn highlight_hovered_body(&mut self, window: &Window) {
+        if let Some(highlighted_body) = self.state.highlighted_body {
+            if let Some(nodes) = self.graphics.body_nodes_mut(highlighted_body) {
+                for node in nodes {
+                    node.unselect()
                 }
+            }
+        }
 
-                event.inhibited = modifiers.contains(Modifiers::Shift);
+        let size = window.size();
+        let (pos, dir) = self
+            .graphics
+            .camera()
+            .unproject(&self.cursor_pos, &na::convert(size));
+        let ray = Ray::new(pos, dir);
+        let hit = self
+            .physics
+            .query_pipeline
+            .cast_ray(&self.physics.colliders, &ray, f32::MAX);
+
+        if let Some((_, collider, _)) = hit {
+            if self.physics.bodies[collider.parent()].is_dynamic() {
+                self.state.highlighted_body = Some(collider.parent());
+                for node in self.graphics.body_nodes_mut(collider.parent()).unwrap() {
+                    node.select()
+                }
             }
-            _ => {}
         }
-        */
     }
 }
 
@@ -1287,9 +1304,9 @@ impl State for Testbed {
                     if let Ok(w) = snapshot.restore() {
                         self.clear(window);
                         self.graphics.clear(window);
+                        self.set_world(w.3, w.4, w.5);
                         self.physics.broad_phase = w.1;
                         self.physics.narrow_phase = w.2;
-                        self.set_world(w.3, w.4, w.5);
                         self.state.timestep_id = w.0;
                     }
                 }
@@ -1347,7 +1364,7 @@ impl State for Testbed {
                     }
                 } else {
                     for (_, mut body) in self.physics.bodies.iter_mut() {
-                        body.wake_up();
+                        body.wake_up(true);
                         body.activation.threshold = -1.0;
                     }
                 }
@@ -1446,6 +1463,10 @@ impl State for Testbed {
                         &self.event_handler,
                     );
 
+                    self.physics
+                        .query_pipeline
+                        .update(&self.physics.bodies, &self.physics.colliders);
+
                     #[cfg(feature = "fluids")]
                     {
                         fluids_time = instant::now();
@@ -1550,7 +1571,9 @@ impl State for Testbed {
             }
         }
 
-        self.graphics.draw(&self.physics.colliders, window);
+        self.highlight_hovered_body(window);
+        self.graphics
+            .draw(&self.physics.bodies, &self.physics.colliders, window);
 
         #[cfg(feature = "fluids")]
         {
@@ -1627,26 +1650,35 @@ Fluids: {:.2}ms
         }
 
         if self.state.flags.contains(TestbedStateFlags::DEBUG) {
-            let hash_bf = md5::compute(&bincode::serialize(&self.physics.broad_phase).unwrap());
-            let hash_nf = md5::compute(&bincode::serialize(&self.physics.narrow_phase).unwrap());
-            let hash_bodies = md5::compute(&bincode::serialize(&self.physics.bodies).unwrap());
-            let hash_colliders =
-                md5::compute(&bincode::serialize(&self.physics.colliders).unwrap());
-            let hash_joints = md5::compute(&bincode::serialize(&self.physics.joints).unwrap());
+            let bf = bincode::serialize(&self.physics.broad_phase).unwrap();
+            let nf = bincode::serialize(&self.physics.narrow_phase).unwrap();
+            let bs = bincode::serialize(&self.physics.bodies).unwrap();
+            let cs = bincode::serialize(&self.physics.colliders).unwrap();
+            let js = bincode::serialize(&self.physics.joints).unwrap();
+            let hash_bf = md5::compute(&bf);
+            let hash_nf = md5::compute(&nf);
+            let hash_bodies = md5::compute(&bs);
+            let hash_colliders = md5::compute(&cs);
+            let hash_joints = md5::compute(&js);
             profile = format!(
                 r#"{}
 Hashes at frame: {}
-|_ Broad phase: {:?}
-|_ Narrow phase: {:?}
-|_ Bodies: {:?}
-|_ Colliders: {:?}
-|_ Joints: {:?}"#,
+|_ Broad phase [{:.1}KB]: {:?}
+|_ Narrow phase [{:.1}KB]: {:?}
+|_ Bodies [{:.1}KB]: {:?}
+|_ Colliders [{:.1}KB]: {:?}
+|_ Joints [{:.1}KB]: {:?}"#,
                 profile,
                 self.state.timestep_id,
+                bf.len() as f32 / 1000.0,
                 hash_bf,
+                nf.len() as f32 / 1000.0,
                 hash_nf,
+                bs.len() as f32 / 1000.0,
                 hash_bodies,
+                cs.len() as f32 / 1000.0,
                 hash_colliders,
+                js.len() as f32 / 1000.0,
                 hash_joints
             );
         }