Added Vector2 for direction, steering behavior gut draft not working

3 files changed, 144 insertions, 27 deletions

diff --git a/src/boid.rs b/src/boid.rs
index dfc0755..5c66272 100644
--- a/src/boid.rs
+++ b/src/boid.rs
@@ -2,26 +2,71 @@ use std::f32::consts::{FRAC_PI_2};
 
 use sdl2::rect::Point;
 use sdl2::render::Canvas;
+use sdl2::video::Window;
+
+use crate::vector2::Vector2;
+
+const NEIGHBOURS_RADIUS: i32 = 50;
 
+#[derive(PartialEq, Clone)]
 pub struct Boid {
-    dir_x: f32,
-    dir_y: f32,
-    position: Point,
+    dir: Vector2,
+    pos: Point,
 }
 
 const TRIANGLE_SIZE: i32 = 20;
-const SPEED: f32 = 10.0;
+const SPEED: f32 = 15.0;
 
-use sdl2::video::Window;
 impl Boid {
-    pub fn new(position: Point) -> Boid {
-        Boid { dir_x: 0.1, dir_y: 0.2, position }
+    pub fn new(x: i32, y: i32, dir_x: f32, dir_y: f32) -> Boid {
+        let d = Vector2::new(dir_x, dir_y);
+        Boid { dir: d / d.norm(), pos: Point::new(x, y) }
+    }
+
+    pub fn step(&mut self, boids: &Vec<Boid>, width: i32, height: i32) {
+        let ns = self.neighbours(boids);
+
+        let ns_len = if ns.len() != 0 { ns.len() } else { 1 };
+
+        let center = ns.iter()
+            .fold(Point::new(0, 0), |acc, x| acc + x.pos) / ns_len as i32;
+        let mut center_dir = Vector2::from_point(center - self.pos);
+        center_dir.normalize();
+
+        let mut align_dir = ns.iter()
+            .fold(Vector2::new(0.0, 0.0), |acc, x| acc + x.dir) / ns_len as f32;
+        align_dir.normalize();
+
+        let sep_dir = Vector2::from_point(ns.iter()
+            .fold(Point::new(0, 0), |acc, x| acc + (x.pos - self.pos)) / ns_len as i32) * -1.0;
+
+
+        if ns.len() != 0 {
+            self.dir = (self.dir + align_dir + center_dir + sep_dir) / 4.0;
+        }
+        self.dir.normalize();
+
+        self.pos = self.pos.offset(
+            (self.dir.x * SPEED as f32) as i32,
+            (self.dir.y * SPEED as f32) as i32
+        );
+        self.pos.x %= width;
+        self.pos.y %= height;
+    }
+
+    fn neighbours<'a>(&self, boids: &'a Vec<Boid>) -> Vec<&'a Boid> {
+        boids.iter().filter(|n| self.dist(n) <= NEIGHBOURS_RADIUS && *n != self).collect()
+    }
+
+    fn dist(&self, other: &Boid) -> i32 {
+        let p = self.pos - other.pos;
+        ((p.x * p.x + p.y * p.y) as f32).sqrt() as i32
     }
 
     pub fn draw(&self, canvas: &mut Canvas<Window>) {
-        let top       = self.position.offset(0, -TRIANGLE_SIZE);
-        let bot_left  = self.position.offset(-TRIANGLE_SIZE / 3, TRIANGLE_SIZE / 2);
-        let bot_right = self.position.offset(TRIANGLE_SIZE / 3, TRIANGLE_SIZE / 2);
+        let top       = self.pos.offset(0, -TRIANGLE_SIZE);
+        let bot_left  = self.pos.offset(-TRIANGLE_SIZE / 3, TRIANGLE_SIZE / 2);
+        let bot_right = self.pos.offset(TRIANGLE_SIZE / 3, TRIANGLE_SIZE / 2);
 
         // direction angle = t
         // tan t = y / x
@@ -29,24 +74,17 @@ impl Boid {
         // | x cos t  -y sin t |
         // | x sin t   y cos t |
 
-        let angle = (self.dir_y / self.dir_x).atan() + FRAC_PI_2;
+        let angle = (self.dir.y / self.dir.x).atan() + FRAC_PI_2;
         let s = angle.sin();
         let c = angle.cos();
 
         let ps: Vec<Point> = [top, bot_left, bot_right, top].iter().map(|p| {
-            let x = (p.x() - self.position.x()) as f32;
-            let y = (p.y() - self.position.y()) as f32;
-            Point::new((x * c - y * s) as i32 + self.position.x(),
-                       (x * s + y * c) as i32 + self.position.y())
+            let x = (p.x() - self.pos.x()) as f32;
+            let y = (p.y() - self.pos.y()) as f32;
+            Point::new((x * c - y * s) as i32 + self.pos.x(),
+                       (x * s + y * c) as i32 + self.pos.y())
         }).collect();
 
         canvas.draw_lines(&ps[..]).unwrap();
     }
-
-    pub fn step(&mut self) {
-        self.position = self.position.offset(
-            (self.dir_x * SPEED) as i32,
-            (self.dir_y * SPEED) as i32
-        );
-    }
 }
diff --git a/src/main.rs b/src/main.rs
index 4c6ce8f..2c633cc 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -1,21 +1,39 @@
 extern crate sdl2;
+extern crate rand;
 
 use sdl2::event::Event;
 use sdl2::keyboard::Keycode;
 use sdl2::pixels::Color;
-use sdl2::rect::Point;
 
+use rand::Rng;
+
+pub mod vector2;
 pub mod boid;
 use boid::Boid;
 
+const HEIGHT:            i32   = 480;
+const WIDTH:             i32   = 640;
+const BOIDS_NUM:         usize = 100;
+// const NEIGHBOURS_RADIUS: i32   = ((HEIGHT + WIDTH) / 2) / 10;
+
 fn main() {
     let sdl = sdl2::init().unwrap();
     let video_subsys = sdl.video().unwrap();
 
-    let window = video_subsys.window("boids", 640, 480).build().unwrap();
+    let window = video_subsys.window("boids", WIDTH as u32, HEIGHT as u32).build().unwrap();
     let mut canvas = window.into_canvas().build().unwrap();
 
-    let mut b = Boid::new(Point::new(50, 50));
+    let mut boids: Vec<Boid> = Vec::with_capacity(BOIDS_NUM);
+
+    let mut rng = rand::thread_rng();
+    for _ in 0..BOIDS_NUM {
+        boids.push(Boid::new(
+            rng.gen_range(0, WIDTH as i32),
+            rng.gen_range(0, HEIGHT as i32),
+            rng.gen_range(-2.0, 2.0),
+            rng.gen_range(-2.0, 2.0),
+        ));
+    }
 
     let mut event_pump = sdl.event_pump().unwrap();
     'main: loop {
@@ -31,11 +49,17 @@ fn main() {
         canvas.set_draw_color(Color::RGB(0, 0, 0));
         canvas.clear();
         canvas.set_draw_color(Color::RGB(200, 200, 200));
-        b.draw(&mut canvas);
+        for b in boids.iter() {
+            b.draw(&mut canvas);
+        }
 
         canvas.present();
 
-        b.step();
+        let prev_boids = boids.clone();
+
+        for b in boids.iter_mut() {
+            b.step(&prev_boids, WIDTH, HEIGHT);
+        }
 
         std::thread::sleep(std::time::Duration::new(0, 50_000_000));
     }
diff --git a/src/vector2.rs b/src/vector2.rs
new file mode 100644
index 0000000..e991a2a
--- /dev/null
+++ b/src/vector2.rs
@@ -0,0 +1,55 @@
+use sdl2::rect::Point;
+
+#[derive(PartialEq, Clone, Copy)]
+pub struct Vector2 {
+    pub y: f32,
+    pub x: f32,
+}
+
+impl Vector2 {
+    pub fn new(x: f32, y: f32) -> Vector2 {
+        Vector2 { x, y }
+    }
+
+    pub fn from_point(point: Point) -> Vector2 {
+        Vector2::new(point.x as f32, point.y as f32)
+    }
+
+    pub fn norm(&self) -> f32 {
+        (self.x * self.x + self.y * self.y).sqrt()
+    }
+
+    pub fn normalize(&mut self) {
+        *self = *self / self.norm();
+    }
+}
+
+use std::ops::{Add, Mul, MulAssign, Div};
+
+impl Add for Vector2 {
+    type Output = Self;
+    fn add(self, other: Self) -> Self::Output {
+        Vector2::new(self.x + other.x, self.y + other.y)
+    }
+}
+
+impl Mul<f32> for Vector2 {
+    type Output = Vector2;
+    fn mul(self, scalar: f32) -> Self::Output {
+        Vector2::new(self.x * scalar, self.y * scalar)
+    }
+}
+
+impl MulAssign<f32> for Vector2 {
+    fn mul_assign(&mut self, scalar: f32) {
+        self.x *= scalar;
+        self.y *= scalar;
+    }
+}
+
+impl Div<f32> for Vector2 {
+    type Output = Vector2;
+    fn div(self, scalar: f32) -> Self::Output {
+        Vector2::new(self.x / scalar, self.y / scalar)
+    }
+}