From fe795b536fb97c2d6edc1830078a1f214076f91d Mon Sep 17 00:00:00 2001
From: mindv0rtex <mindv0rtex@users.noreply.github.com>
Date: Thu, 25 Feb 2021 23:17:13 -0500
Subject: [PATCH] Approximate trigonometry without LUTs.

---
 src/main.rs  |  1 +
 src/model.rs | 28 ++++++++++++++++++++++++++--
 src/trig.rs  | 27 +++++++++++++++++++++++++++
 3 files changed, 54 insertions(+), 2 deletions(-)
 create mode 100644 src/trig.rs

diff --git a/src/main.rs b/src/main.rs
index d134d4d..53d85b3 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -1,6 +1,7 @@
 mod blur;
 mod grid;
 mod model;
+mod trig;
 
 fn main() {
     let model = model::Model::new(4, 4, 20, 1);
diff --git a/src/model.rs b/src/model.rs
index 0b552c0..12ff604 100644
--- a/src/model.rs
+++ b/src/model.rs
@@ -1,4 +1,4 @@
-use crate::grid::Grid;
+use crate::{grid::Grid, trig};
 
 use rand::{thread_rng, Rng};
 
@@ -12,6 +12,7 @@ struct Agent {
 }
 
 impl Agent {
+    /// Construct a new agent with random parameters.
     fn new(width: usize, height: usize) -> Self {
         let mut rng = rand::thread_rng();
         let (x, y, angle) = rng.gen::<(f32, f32, f32)>();
@@ -48,7 +49,7 @@ impl PopulationConfig {
     const DECAY_FACTOR_MIN: f32 = 0.1;
     const DECAY_FACTOR_MAX: f32 = 0.1;
 
-    /// Generate a random configuration.
+    /// Construct a random configuration.
     pub fn new() -> Self {
         let mut rng = rand::thread_rng();
 
@@ -83,6 +84,7 @@ pub struct Model {
 }
 
 impl Model {
+    /// Construct a new model with random initial conditions and random configuration.
     pub fn new(width: usize, height: usize, n_particles: usize, diffusivity: usize) -> Self {
         Model {
             agents: (0..n_particles)
@@ -94,4 +96,26 @@ impl Model {
             iteration: 0,
         }
     }
+
+    pub fn step(&mut self) {
+        let sensor_distance = self.config.sensor_distance;
+        let sensor_angle = self.config.sensor_angle;
+
+        for agent in self.agents.iter_mut() {
+            let xc = agent.x + trig::cos(agent.angle) * sensor_distance;
+            let yc = agent.y + trig::sin(agent.angle) * sensor_distance;
+            let xl = agent.x + trig::cos(agent.angle - sensor_angle) * sensor_distance;
+            let yl = agent.y + trig::sin(agent.angle - sensor_angle) * sensor_distance;
+            let xr = agent.x + trig::cos(agent.angle + sensor_angle) * sensor_distance;
+            let yr = agent.y + trig::sin(agent.angle + sensor_angle) * sensor_distance;
+
+            // Sense
+            let trail_c = self.grid.get(xc, yc);
+            let trail_l = self.grid.get(xl, yl);
+            let trail_r = self.grid.get(xr, yr);
+            // Rotate
+            // Move
+        }
+        // Deposit + Diffuse + Decay
+    }
 }
diff --git a/src/trig.rs b/src/trig.rs
new file mode 100644
index 0000000..b4282ea
--- /dev/null
+++ b/src/trig.rs
@@ -0,0 +1,27 @@
+/// From https://bits.stephan-brumme.com/absFloat.html
+pub(crate) fn abs(x: f32) -> f32 {
+    f32::from_bits(x.to_bits() & 0x7FFF_FFFF)
+}
+
+/// Branchless floor implementation
+pub(crate) fn floor(x: f32) -> f32 {
+    let mut x_trunc = (x as i32) as f32;
+    x_trunc -= (x < x_trunc) as i32 as f32;
+    x_trunc
+}
+
+/// Approximates `cos(x)` in radians with the maximum error of `0.002`
+/// https://stackoverflow.com/posts/28050328/revisions
+pub(crate) fn cos(mut x: f32) -> f32 {
+    const ALPHA: f32 = 0.5 * std::f32::consts::FRAC_1_PI;
+    x *= ALPHA;
+    x -= 0.25_f32 + floor(x + 0.25_f32);
+    x *= 16.0_f32 * (abs(x) - 0.5_f32);
+    x += 0.225_f32 * x * (abs(x) - 1.0_f32);
+    x
+}
+
+/// Approximates `sin(x)` in radians with the maximum error of `0.002`
+pub(crate) fn sin(x: f32) -> f32 {
+    cos(x - std::f32::consts::FRAC_PI_2)
+}