Fixing separation direction and modulo operator on floatHEADmaster

5 files changed, 113 insertions, 54 deletions

diff --git a/README.md b/README.md
new file mode 100644
index 0000000..f9241c9
--- /dev/null
+++ b/README.md
@@ -0,0 +1,5 @@
+# boids
+
+Bird flocking simulation.
+
+![record](./record.gif)
diff --git a/record.gif b/record.gif
new file mode 100644
index 0000000..9738304
--- /dev/null
+++ b/record.gif
diff --git a/src/boid.rs b/src/boid.rs
index 5c66272..af0604b 100644
--- a/src/boid.rs
+++ b/src/boid.rs
@@ -1,4 +1,4 @@
-use std::f32::consts::{FRAC_PI_2};
+use std::f64::consts::{FRAC_PI_2};
 
 use sdl2::rect::Point;
 use sdl2::render::Canvas;
@@ -6,67 +6,82 @@ use sdl2::video::Window;
 
 use crate::vector2::Vector2;
 
-const NEIGHBOURS_RADIUS: i32 = 50;
+const NEIGHBOURS_RADIUS: f64 = 30.0;
 
 #[derive(PartialEq, Clone)]
 pub struct Boid {
     dir: Vector2,
-    pos: Point,
+    pos: Vector2,
 }
 
-const TRIANGLE_SIZE: i32 = 20;
-const SPEED: f32 = 15.0;
+const TRIANGLE_SIZE: f64 = 8.0;
+const SPEED: f64 = 1.0;
 
 impl Boid {
-    pub fn new(x: i32, y: i32, dir_x: f32, dir_y: f32) -> Boid {
+    pub fn new(x: f64, y: f64, dir_x: f64, dir_y: f64) -> Boid {
         let d = Vector2::new(dir_x, dir_y);
-        Boid { dir: d / d.norm(), pos: Point::new(x, y) }
+        Boid { dir: d / d.norm(), pos: Vector2::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 };
+        if ns.len() == 0 {
+            self.update_pos(width, height);
+            return ;
+        }
 
         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();
+            .fold(Vector2::new(0.0, 0.0), |acc, x| acc + x.pos) / ns.len() as f64;
+        let mut center_dir = center - self.pos;
 
         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;
-
+            .fold(Vector2::new(0.0, 0.0), |acc, x| acc + x.dir) / ns.len() as f64;
+
+        let mut sep_dir = ns.iter()
+            .fold(Vector2::new(0.0, 0.0), |acc, x| {
+                acc + ((self.pos - x.pos) / (self.dist(x) * self.dist(x)))
+            }) / ns.len() as f64;
+
+        let max_speed = 4.0;
+        align_dir.set_mag(max_speed);
+        center_dir.set_mag(max_speed);
+        sep_dir.set_mag(max_speed);
+
+        let mut alignment_force = align_dir - self.dir;
+        let mut center_force = center_dir - self.dir;
+        // let mut sep_force = sep_dir - self.dir;
+
+        let max_force = 1.0;
+        alignment_force.limit(max_force);
+        center_force.limit(max_force);
+        sep_dir.limit(max_force);
+
+        self.update_pos(width, height);
+        let acceleration = alignment_force + center_force + sep_dir;
+        self.dir += acceleration;
+        self.dir.limit(max_speed);
+    }
 
-        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 update_pos(&mut self, width: i32, height: i32) {
+        self.pos += self.dir * SPEED;
+        self.pos.x = self.pos.x.rem_euclid(width as f64);
+        self.pos.y = self.pos.y.rem_euclid(height as f64);
     }
 
     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 {
+    fn dist(&self, other: &Boid) -> f64 {
         let p = self.pos - other.pos;
-        ((p.x * p.x + p.y * p.y) as f32).sqrt() as i32
+        (p.x * p.x + p.y * p.y).sqrt()
     }
 
     pub fn draw(&self, canvas: &mut Canvas<Window>) {
-        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);
+        let top       = self.pos + Vector2::new(0.0, -TRIANGLE_SIZE);
+        let bot_left  = self.pos + Vector2::new(-TRIANGLE_SIZE / 3.0, TRIANGLE_SIZE / 2.0);
+        let bot_right = self.pos + Vector2::new(TRIANGLE_SIZE / 3.0, TRIANGLE_SIZE / 2.0);
 
         // direction angle = t
         // tan t = y / x
@@ -79,10 +94,10 @@ impl Boid {
         let c = angle.cos();
 
         let ps: Vec<Point> = [top, bot_left, bot_right, top].iter().map(|p| {
-            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())
+            let x = (p.x - self.pos.x) as f64;
+            let y = (p.y - self.pos.y) as f64;
+            Point::new(((x * c - y * s) + self.pos.x) as i32,
+                       ((x * s + y * c) + self.pos.y) as i32)
         }).collect();
 
         canvas.draw_lines(&ps[..]).unwrap();
diff --git a/src/main.rs b/src/main.rs
index 2c633cc..05a007d 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -28,10 +28,10 @@ fn main() {
     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),
+            rng.gen_range(0.0, WIDTH as f64),
+            rng.gen_range(0.0, HEIGHT as f64),
+            rng.gen_range(0.1, 1.0),
+            rng.gen_range(0.1, 1.0),
         ));
     }
 
@@ -61,6 +61,6 @@ fn main() {
             b.step(&prev_boids, WIDTH, HEIGHT);
         }
 
-        std::thread::sleep(std::time::Duration::new(0, 50_000_000));
+        std::thread::sleep(std::time::Duration::new(0, 10_000_000));
     }
 }
diff --git a/src/vector2.rs b/src/vector2.rs
index e991a2a..2413f42 100644
--- a/src/vector2.rs
+++ b/src/vector2.rs
@@ -2,29 +2,46 @@ use sdl2::rect::Point;
 
 #[derive(PartialEq, Clone, Copy)]
 pub struct Vector2 {
-    pub y: f32,
-    pub x: f32,
+    pub y: f64,
+    pub x: f64,
 }
 
 impl Vector2 {
-    pub fn new(x: f32, y: f32) -> Vector2 {
+    pub fn new(x: f64, y: f64) -> Vector2 {
         Vector2 { x, y }
     }
 
     pub fn from_point(point: Point) -> Vector2 {
-        Vector2::new(point.x as f32, point.y as f32)
+        Vector2::new(point.x as f64, point.y as f64)
     }
 
-    pub fn norm(&self) -> f32 {
+    pub fn norm(&self) -> f64 {
         (self.x * self.x + self.y * self.y).sqrt()
     }
 
     pub fn normalize(&mut self) {
         *self = *self / self.norm();
     }
+
+    pub fn set_mag(&mut self, mag: f64) {
+        *self = *self / self.norm() * mag;
+    }
+
+
+    pub fn limit(&mut self, max: f64) {
+        if self.norm() > max {
+            self.set_mag(max);
+        }
+    }
+
+    pub fn low(&mut self, min: f64) {
+        if self.norm() < min {
+            self.set_mag(min);
+        }
+    }
 }
 
-use std::ops::{Add, Mul, MulAssign, Div};
+use std::ops::{Add, AddAssign, Sub, Mul, MulAssign, Div};
 
 impl Add for Vector2 {
     type Output = Self;
@@ -33,23 +50,45 @@ impl Add for Vector2 {
     }
 }
 
-impl Mul<f32> for Vector2 {
+impl AddAssign for Vector2 {
+    fn add_assign(&mut self, other: Self) {
+        self.x += other.x;
+        self.y += other.y;
+    }
+}
+
+impl Sub for Vector2 {
+    type Output = Self;
+    fn sub(self, other: Self) -> Self::Output {
+        Vector2::new(self.x - other.x, self.y - other.y)
+    }
+}
+
+impl Mul<f64> for Vector2 {
     type Output = Vector2;
-    fn mul(self, scalar: f32) -> Self::Output {
+    fn mul(self, scalar: f64) -> Self::Output {
         Vector2::new(self.x * scalar, self.y * scalar)
     }
 }
 
-impl MulAssign<f32> for Vector2 {
-    fn mul_assign(&mut self, scalar: f32) {
+impl MulAssign<f64> for Vector2 {
+    fn mul_assign(&mut self, scalar: f64) {
         self.x *= scalar;
         self.y *= scalar;
     }
 }
 
-impl Div<f32> for Vector2 {
+impl Div<f64> for Vector2 {
     type Output = Vector2;
-    fn div(self, scalar: f32) -> Self::Output {
+    fn div(self, scalar: f64) -> Self::Output {
         Vector2::new(self.x / scalar, self.y / scalar)
     }
 }
+
+use std::fmt;
+
+impl fmt::Debug for Vector2 {
+    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        write!(f, "{} {}", self.x, self.y)
+    }
+}