106 lines
3.2 KiB
C++
106 lines
3.2 KiB
C++
#include <Arduino.h>
|
|
#include "constants.h"
|
|
#include "config.h"
|
|
#include "util.h"
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
//float first_pass[PANEL_WIDTH][PANEL_HEIGHT];
|
|
void gaussian_blur(float *field[PANEL_WIDTH][PANEL_HEIGHT]) {
|
|
float kernel[GAUSS_WIDTH];
|
|
float first_pass[PANEL_WIDTH][PANEL_HEIGHT];
|
|
memset(first_pass, 0.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 = *field[x][y] * DECAY_FACTOR * 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 += *field[x + x_offset][y] * kernel[x_offset];
|
|
additions++;
|
|
}
|
|
if (is_in_bounds(x-x_offset, y)) {
|
|
sum += *field[x - x_offset][y] * kernel[x_offset];
|
|
additions++;
|
|
}
|
|
}
|
|
first_pass[x][y] = sum/GAUSS_WIDTH;
|
|
}
|
|
}
|
|
// 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++;
|
|
}
|
|
}
|
|
*field[x][y] = sum/GAUSS_WIDTH;
|
|
}
|
|
}
|
|
}
|
|
|
|
void box_blur(float field[PANEL_WIDTH][PANEL_HEIGHT]) {
|
|
auto first_pass = new float[PANEL_WIDTH][PANEL_HEIGHT]();
|
|
// horizontal pass
|
|
for (int x = 0; x < PANEL_WIDTH; x++) {
|
|
for(int y = 0; y < PANEL_HEIGHT; y++) {
|
|
float sum;
|
|
sum = field[x][y] * DECAY_FACTOR; //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 += field[x + x_offset][y];
|
|
additions++;
|
|
}
|
|
if (is_in_bounds(x-x_offset, y)) {
|
|
sum += field[x - x_offset][y];
|
|
additions++;
|
|
}
|
|
}
|
|
first_pass[x][y] = (sum/additions);
|
|
}
|
|
}
|
|
// 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++;
|
|
}
|
|
}
|
|
|
|
float result = (sum/additions); // * DECAY_FACTOR;
|
|
//attractant[x][y] = (((8.0*attractant[x][y]) + result) / 9.0);
|
|
field[x][y] = result;
|
|
}
|
|
}
|
|
delete[] first_pass;
|
|
}
|