food collection

simulator/src/main.rs

@@ -62,7 +62,7 @@ impl Robot {
     pub fn policy(&mut self, sensor: &Sensor){
         match sensor {
             Sensor::Food => self.move_step(1.0, 0.0),
-            Sensor::Kin => self.move_step(-0.3, -0.02),
+            Sensor::Kin => self.move_step(-0.3, 0.02),
             Sensor::None => self.move_step(0.3, 0.01),
         }
     }
@@ -95,6 +95,12 @@ impl Food {
     pub fn collider(&self) -> (&Pose, &dyn Shape) {
         (&self.pose, &self.collider)
     }
+
+    fn detect<'a>(&self, things: impl Iterator<Item = (&'a Pose, &'a dyn Shape)>) -> Option<f32> {
+        things
+            .map(|thing| contact(&self.pose, &self.collider, thing.0, thing.1, 0.0).unwrap()).flatten()
+            .map(|contact| contact.point1.distance(self.pose.translation)).reduce(|x: f32,y: f32| x.min(y))
+    }
 }
 
 
@@ -115,10 +121,17 @@ async fn main() {
     loop {
         clear_background(LIGHTGRAY);
 
-        for food in &food{
+        for food in &mut food{
+            if let Some(detect_robot) = food.detect(robots.iter().map(Robot::collider)) {
+                println!("robot detected in contact");
+                food.alive = false;
+            }
             food.draw();
+            // TODO: die upon collision with robot
         }
 
+        food = food.into_iter().filter(|f| f.alive).collect(); // remove dead food
+
         let mut sensors = Vec::new();
         for robot in &robots {
             println!("loop");