diff --git a/simulator/src/main.rs b/simulator/src/main.rs index 0691deb..49603c8 100644 --- a/simulator/src/main.rs +++ b/simulator/src/main.rs @@ -1,7 +1,12 @@ +use std::cmp::min; + use macroquad::prelude::*; use parry2d::math::Pose; use parry2d::math::Rot2; use parry2d::math::Vec2; +use parry2d::query::contact; +use parry2d::shape::Ball; +use parry2d::shape::Shape; const VIEW_DIST: f32 = 80.0; const BOT_RADIUS: f32 = 16.0; @@ -28,11 +33,22 @@ impl Robot { pub fn draw(&self) { let tl = self.pose.translation; - let tl2 = self.pose.translation + self.ahead() * VIEW_DIST; + let tl2 = self.view_end(); draw_circle(tl.x, tl.y, BOT_RADIUS, BLACK); draw_line(tl.x, tl.y, tl2.x, tl2.y, 2.0, BLUE); draw_circle(tl.x, tl.y, BOT_RADIUS-1.0, YELLOW); } + + fn view_end(&self) -> Vec2 { + self.pose.translation + self.ahead() * VIEW_DIST + } + + fn detect(&self, things: &[(&Pose, &dyn Shape)]) -> Option { + let collider = Ball::new(BOT_RADIUS); + things.iter() + .map(|thing| contact(&self.pose, &collider, thing.0, thing.1, 0.0).unwrap()).flatten() + .map(|contactl| contactl.point1.distance(self.pose.translation)).reduce(|x,y| x.min(y)) + } } #[macroquad::main("Adversarial Foraging Simulator")]