organism/src/main.cpp

@@ -118,7 +118,7 @@ void draw_bg() {
 void draw_attractant() {
   for(int x = 0; x<PANEL_WIDTH; x++) {
     for(int y = 0; y<PANEL_HEIGHT; y++){
-      if (attractant[x][y] > 20) {
+      if (attractant[x][y] > 0) {
         matrix->drawPixel(x, y, matrix->color565(
               int((255.0 / 255.0) * attractant[x][y]),
               int((80.0 / 255.0) * attractant[x][y]),
@@ -130,6 +130,108 @@ void draw_attractant() {
   }
 }
 
+void setupGaussianKernel(float* kernel, int width, float sigma) {
+  float sum = 0.0;
+  for(int i = 0; i < width; i++) {
+    //kernel[width+i] = exp( -(i*i) / (2 * sigma * sigma)) / (PI * 2 * sigma * sigma);
+    kernel[i] = exp(-0.5f * (i * i) / (sigma * sigma));
+    sum += kernel[width];
+  }
+  for(int i; i < width; i++) {
+    kernel[i] = sum/width;
+  }
+  Serial.printf("%f %f %f\n", kernel[0], kernel[1], kernel[2]);
+}
+
+#define GAUSS_WIDTH 5
+#define GAUSS_SIGMA 1.0
+#define DECAY_FACTOR 0.9
+void gaussian_blur() {
+  float kernel[GAUSS_WIDTH];
+  uint8_t first_pass[PANEL_WIDTH][PANEL_HEIGHT];
+  memset(first_pass, (uint8_t) 0, sizeof first_pass);
+  setupGaussianKernel(kernel, GAUSS_WIDTH, GAUSS_SIGMA);
+  // horizontal pass
+  for (int x = 0; x < PANEL_WIDTH; x++) {
+    for(int y = 0; y < PANEL_HEIGHT; y++) {
+      float sum = attractant[x][y] * kernel[0]; //0.0;
+      int additions = 1;
+      for (int x_offset = 1; x_offset < GAUSS_WIDTH; x_offset++) {
+        if (is_in_bounds(x+x_offset, y)) {
+          sum += attractant[x + x_offset][y] * kernel[x_offset];
+          additions++;
+        }
+        if (is_in_bounds(x-x_offset, y)) {
+          sum += attractant[x - x_offset][y] * kernel[x_offset];
+          additions++;
+        }
+      }
+      first_pass[x][y] = (int) round(sum/GAUSS_WIDTH * DECAY_FACTOR);
+    }
+  }
+  // vertical pass
+  for (int x = 0; x < PANEL_WIDTH; x++) {
+    for(int y = 0; y < PANEL_HEIGHT; y++) {
+      float sum = first_pass[x][y] * kernel[0]; //0.0;
+      int additions = 1;
+      for (int y_offset = 1; y_offset < GAUSS_WIDTH; y_offset++) {
+        if (is_in_bounds(x, y + y_offset)) {
+          sum += first_pass[x][y + y_offset] * kernel[y_offset];
+          additions++;
+        }
+        if (is_in_bounds(x, y - y_offset)) {
+          sum += first_pass[x][y - y_offset] * kernel[y_offset];
+          additions++;
+        }
+      }
+      attractant[x][y] = (int) round((sum/GAUSS_WIDTH) * DECAY_FACTOR);
+    } 
+  }
+}
+
+#define BLUR_KERNEL_SIZE 3
+void box_blur() {
+  uint8_t first_pass[PANEL_WIDTH][PANEL_HEIGHT];
+  memset(first_pass, (uint8_t) 0, sizeof first_pass);
+  
+  // horizontal pass
+  for (int x = 0; x < PANEL_WIDTH; x++) {
+    for(int y = 0; y < PANEL_HEIGHT; y++) {
+      int sum = attractant[x][y]; //0.0;
+      int additions = 1;
+      for (int x_offset = 1; x_offset < BLUR_KERNEL_SIZE; x_offset++) {
+        if (is_in_bounds(x+x_offset, y)) {
+          sum += attractant[x + x_offset][y];
+          additions++;
+        }
+        if (is_in_bounds(x-x_offset, y)) {
+          sum += attractant[x - x_offset][y];
+          additions++;
+        }
+      }
+      first_pass[x][y] = (int) round((sum/additions) * DECAY_FACTOR);
+    }
+  }
+  // vertical pass
+  for (int x = 0; x < PANEL_WIDTH; x++) {
+    for(int y = 0; y < PANEL_HEIGHT; y++) {
+      float sum = first_pass[x][y]; //0.0;
+      int additions = 1;
+      for (int y_offset = 1; y_offset < BLUR_KERNEL_SIZE; y_offset++) {
+        if (is_in_bounds(x, y + y_offset)) {
+          sum += first_pass[x][y + y_offset];
+          additions++;
+        }
+        if (is_in_bounds(x, y - y_offset)) {
+          sum += first_pass[x][y - y_offset];
+          additions++;
+        }
+      }
+      attractant[x][y] = (int) round((sum/additions) * DECAY_FACTOR);
+    } 
+  }
+}
+
 void loop() {
   matrix ->flipDMABuffer();
   //matrix->clearScreen();
@@ -176,9 +278,12 @@ void loop() {
       agents[a].heading += AGENT_ANGLE * (PI * 2);
     }
   }
-
   delay(10);
   iterations++;
+  if (iterations % 64 == 0) {
+    //gaussian_blur();
+    box_blur();
+  }
   if (iterations >= NUM_ITERATIONS) {
     init_attractant();
     init_agents();