From c1a5d8e2e4c0a9aa10f530fcc8b084edd0cb81ff Mon Sep 17 00:00:00 2001 From: Philipp Kramer Date: Thu, 12 Jan 2023 13:00:53 +0100 Subject: [PATCH] change perlin noise for simplex noise --- protesis_03_dxl/SimplexNoise.cpp | 114 ++++++++++++++++++++++++++++ protesis_03_dxl/SimplexNoise.h | 54 +++++++++++++ protesis_03_dxl/protesis_03_dxl.ino | 3 +- protesis_03_dxl/servo-control.cpp | 34 ++++++--- protesis_03_dxl/servo-control.hpp | 3 + 5 files changed, 196 insertions(+), 12 deletions(-) create mode 100644 protesis_03_dxl/SimplexNoise.cpp create mode 100644 protesis_03_dxl/SimplexNoise.h diff --git a/protesis_03_dxl/SimplexNoise.cpp b/protesis_03_dxl/SimplexNoise.cpp new file mode 100644 index 0000000..63c4110 --- /dev/null +++ b/protesis_03_dxl/SimplexNoise.cpp @@ -0,0 +1,114 @@ +/* +=============================================================================== +Ported the SimplexNoise algorithm from the C++ versions mentioned below +to a reuseable Arduino Library. + +By Jordan Shaw / http://jordanshaw.com / 2017-02 + +A C++ port of a speed-improved simplex noise algorithm for 2D in Java. +Based on example code by Stefan Gustavson (stegu@itn.liu.se). +Optimisations by Peter Eastman (peastman@drizzle.stanford.edu). +Better rank ordering method by Stefan Gustavson in 2012. +C++ port and minor type and algorithm changes by Josh Koch (jdk1337@gmail.com). +This could be speeded up even further, but it's useful as it is. +Version 2012-04-12 +The original Java code was placed in the public domain by its original author, +Stefan Gustavson. You may use it as you see fit, +but attribution is appreciated. + +Original gist url: https://gist.github.com/Slipyx/2372043 + +=============================================================================== +*/ + +#include +#include "Arduino.h" +#include "SimplexNoise.h" + +// Private static member definitions +const double SimplexNoise::F2 = 0.5 * (sqrt( 3.0 ) - 1.0); +const double SimplexNoise::G2 = (3.0 - sqrt( 3.0 )) / 6.0; +const uint8_t SimplexNoise::p[256] = { + 151,160,137,91,90,15,131,13,201,95,96,53,194,233,7,225,140,36,103,30,69, + 142,8,99,37,240,21,10,23,190,6,148,247,120,234,75,0,26,197,62,94,252,219, + 203,117,35,11,32,57,177,33,88,237,149,56,87,174,20,125,136,171,168,68,175, + 74,165,71,134,139,48,27,166,77,146,158,231,83,111,229,122,60,211,133,230, + 220,105,92,41,55,46,245,40,244,102,143,54,65,25,63,161,1,216,80,73,209,76, + 132,187,208,89,18,169,200,196,135,130,116,188,159,86,164,100,109,198,173, + 186,3,64,52,217,226,250,124,123,5,202,38,147,118,126,255,82,85,212,207,206, + 59,227,47,16,58,17,182,189,28,42,223,183,170,213,119,248,152,2,44,154,163, + 70,221,153,101,155,167,43,172,9,129,22,39,253,19,98,108,110,79,113,224,232, + 178,185,112,104,218,246,97,228,251,34,242,193,238,210,144,12,191,179,162, + 241,81,51,145,235,249,14,239,107,49,192,214,31,181,199,106,157,184,84,204, + 176,115,121,50,45,127,4,150,254,138,236,205,93,222,114,67,29,24,72,243,141, + 128,195,78,66,215,61,156,180 +}; +const Grad SimplexNoise::grad3[12] = { + Grad(1,1,0),Grad(-1,1,0),Grad(1,-1,0),Grad(-1,-1,0),Grad(1,0,1), + Grad(-1,0,1),Grad(1,0,-1),Grad(-1,0,-1),Grad(0,1,1),Grad(0,-1,1), + Grad(0,1,-1),Grad(0,-1,-1) +}; +uint8_t SimplexNoise::perm[512] = {0}; +uint8_t SimplexNoise::permMod12[512] = {0}; + +// Initialize permutaion arrays +void SimplexNoise::init() { + for ( uint16_t i = 0; i < 512; ++i ) { + perm[i] = p[i & 255]; + permMod12[i] = static_cast(perm[i] % 12); + } + delete[] &p; // lol what +} + +// Fast floor +int32_t SimplexNoise::fastFloor( double x ) { + int32_t xi = static_cast(x); + return x < xi ? xi - 1 : xi; +} + +double SimplexNoise::dot( const Grad& g, double x, double y ) { + return g.x * x + g.y * y; +} + +// 2D SimplexNoise noise +double SimplexNoise::noise( double xin, double yin ) { + double s = (xin + yin) * F2; + int32_t i = fastFloor( xin + s ); + int32_t j = fastFloor( yin + s ); + double t = (i + j) * G2; + double x0 = xin - (i - t); + double y0 = yin - (j - t); + uint8_t i1 = 0, j1 = 1; + if ( x0 > y0 ) { + i1 = 1; + j1 = 0; + } + double x1 = x0 - i1 + G2; + double y1 = y0 - j1 + G2; + double x2 = x0 - 1.0 + 2.0 * G2; + double y2 = y0 - 1.0 + 2.0 * G2; + uint8_t ii = i & 255; + uint8_t jj = j & 255; + uint8_t gi0 = permMod12[ii + perm[jj]]; + uint8_t gi1 = permMod12[ii + i1 + perm[jj + j1]]; + uint8_t gi2 = permMod12[ii + 1 + perm[jj + 1]]; + double n0 = 0.0; + double t0 = 0.5 - x0 * x0 - y0 * y0; + if ( t0 >= 0.0 ) { + t0 *= t0; + n0 = t0 * t0 * dot( grad3[gi0], x0, y0 ); + } + double n1 = 0.0; + double t1 = 0.5 - x1 * x1 - y1 * y1; + if ( t1 >= 0.0 ) { + t1 *= t1; + n1 = t1 * t1 * dot( grad3[gi1], x1, y1 ); + } + double n2 = 0.0; + double t2 = 0.5 - x2 * x2 - y2 * y2; + if ( t2 >= 0.0 ) { + t2 *= t2; + n2 = t2 * t2 * dot( grad3[gi2], x2, y2 ); + } + return 70.0 * (n0 + n1 + n2); +} \ No newline at end of file diff --git a/protesis_03_dxl/SimplexNoise.h b/protesis_03_dxl/SimplexNoise.h new file mode 100644 index 0000000..4ed97af --- /dev/null +++ b/protesis_03_dxl/SimplexNoise.h @@ -0,0 +1,54 @@ +/* +=============================================================================== +Ported the SimplexNoise algorithm from the C++ versions mentioned below +to a reuseable Arduino Library. + +By Jordan Shaw / http://jordanshaw.com / 2017-02 + +A C++ port of a speed-improved simplex noise algorithm for 2D in Java. +Based on example code by Stefan Gustavson (stegu@itn.liu.se). +Optimisations by Peter Eastman (peastman@drizzle.stanford.edu). +Better rank ordering method by Stefan Gustavson in 2012. +C++ port and minor type and algorithm changes by Josh Koch (jdk1337@gmail.com). +This could be speeded up even further, but it's useful as it is. +Version 2012-04-12 +The original Java code was placed in the public domain by its original author, +Stefan Gustavson. You may use it as you see fit, +but attribution is appreciated. + +Original gist url: https://gist.github.com/Slipyx/2372043 + +=============================================================================== +*/ + +#ifndef SimplexNoise_h +#define SimplexNoise_h + +#include +#include "Arduino.h" + +class Grad { +public: + Grad( int8_t x, int8_t y, int8_t z ) : x(x), y(y), z(z) {} + int8_t x, y, z; +}; + +class SimplexNoise { +public: + // Initialize permutation arrays + static void init(); + // 2D simplex noise + static double noise( double xin, double yin ); + +private: + static int32_t fastFloor( double x ); + static double dot( const Grad& g, double x, double y ); + static const double F2; + static const double G2; + static const Grad grad3[12]; + static const uint8_t p[256]; + static uint8_t perm[512]; + static uint8_t permMod12[512]; +}; + +#endif \ No newline at end of file diff --git a/protesis_03_dxl/protesis_03_dxl.ino b/protesis_03_dxl/protesis_03_dxl.ino index 5051884..4bb8aba 100644 --- a/protesis_03_dxl/protesis_03_dxl.ino +++ b/protesis_03_dxl/protesis_03_dxl.ino @@ -4,6 +4,7 @@ #include "servo-control.hpp" #include "movement.hpp" #include "noise.hpp" +#include "SimplexNoise.h" //Library from https://github.com/jshaw/SimplexNoise #define DENOISE_PREVIOUS_INERTIA_RATE 0.9999 #define DENOISE_CURRENT_INERTIA_RATE 0.0001 @@ -96,7 +97,7 @@ void loop() { if (loopmillis-last_loop_readpos_millis>100 && loopmillis-last_servo_send_millis>=4) { //limit frequency and minimum delay after send positions last_loop_readpos_millis=loopmillis; - read_servos_positions(); //takes around 8000us for all servos + read_servos_positions(); //takes around 8000us for all servos. read and send over serial. } diff --git a/protesis_03_dxl/servo-control.cpp b/protesis_03_dxl/servo-control.cpp index b38cd7f..2660d3f 100644 --- a/protesis_03_dxl/servo-control.cpp +++ b/protesis_03_dxl/servo-control.cpp @@ -7,6 +7,8 @@ #include "servo-control.hpp" +SimplexNoise sn; + int angle_to_dxl_servo_position(const int deg); int rpm_to_dxl_servo_speed(const int rpm); @@ -33,22 +35,25 @@ void matrix_weights_update(){ - matrix_weights[0][W_COS]=40.0; - matrix_weights[1][W_SIN]=-20.0; //testing on servo - /* + matrix_weights[0][W_COS]=30 *map_mode; matrix_weights[0][W_NOISE]=90.0 *map_mode; + matrix_weights[0][W_NOISESLOW]=0.0 *map_mode; matrix_weights[1][W_SIN]=30 *map_mode; - matrix_weights[1][W_NOISE]=90.0 *map_mode; + matrix_weights[1][W_NOISE]=70.0 *map_mode; + matrix_weights[1][W_NOISESLOW]=20.0 *map_mode; matrix_weights[2][W_COS]=-30 *map_mode; - matrix_weights[2][W_NOISE]=90.0 *map_mode; + matrix_weights[2][W_NOISE]=50.0 *map_mode; + matrix_weights[2][W_NOISESLOW]=40.0 *map_mode; matrix_weights[3][W_SIN]=-30 *map_mode; - matrix_weights[3][W_NOISE]=90.0 *map_mode; + matrix_weights[3][W_NOISE]=20.0 *map_mode; + matrix_weights[3][W_NOISESLOW]=70.0 *map_mode; matrix_weights[4][W_COS]=30 *map_mode; - matrix_weights[4][W_NOISE]=90.0 *map_mode; + matrix_weights[4][W_NOISE]=0.0 *map_mode; + matrix_weights[4][W_NOISESLOW]=60.0 *map_mode; matrix_weights[0][W_ROLL]=1.0* (1-map_mode); @@ -56,7 +61,7 @@ void matrix_weights_update(){ matrix_weights[2][W_ROLL]=1.0* (1-map_mode); matrix_weights[3][W_PITCH]=1.0* (1-map_mode); matrix_weights[4][W_ROLL]=1.0* (1-map_mode); - */ + /* matrix_weights[0][W_NOISE]=90.0*map_mode; @@ -81,6 +86,7 @@ void matrix_weights_update(){ matrix_weights[i][W_NOISE]*=sleeping; } + @@ -265,12 +271,18 @@ void map_servos_by_weights_with_initial_position_and_move(const int roll, const float vsin=sin((millis()+millis_add)/1000.0); float vcos=cos((millis()+millis_add)/1000.0); + //unsigned long blastart=micros(); for (uint8_t i=0;i