diff options
Diffstat (limited to 'lib/center_of_gravity.c')
| -rw-r--r-- | lib/center_of_gravity.c | 56 |
1 files changed, 56 insertions, 0 deletions
diff --git a/lib/center_of_gravity.c b/lib/center_of_gravity.c new file mode 100644 index 0000000..3186c44 --- /dev/null +++ b/lib/center_of_gravity.c @@ -0,0 +1,56 @@ +// Center of Gravity +// +// center_of_gravity - COG +// +// 1 - 2 * SUM(i*x_i) / ((k+1) * SUM(x_i)), oscillates around 0 +// +// Inputs: 1 +// Outputs: 1 (COG) +// Constants: 1 (period) + +#include "ts.h" + +typedef struct { + uint_t k; +} ts_center_of_gravity_params_t; + +ts_parameter_t const ts_center_of_gravity_p = { .n_inputs = 1, .n_outputs = 1, .n_constants = 1, .params_size = sizeof(ts_center_of_gravity_params_t) }; + +uint_t ts_center_of_gravity_offset( ts_constant_t const *c ) +{ + ( void )c; + return ( c ? ((ts_center_of_gravity_params_t *)c)->k - 1 : 0 ); +} + +int ts_center_of_gravity( + fp_t * const *y, uint_t y_n, + fp_t const * const *x, uint_t x_n, + uint_t n, ts_constant_t const *c +) +{ + TS_PROLOGUE( ts_center_of_gravity_p ); + if ( !c ) return -1; + uint_t k = ((ts_center_of_gravity_params_t *)c)->k; + if (k == 0 || n < k) return -1; + + for( uint_t i = k - 1; i < n; ++i ) + { + fp_t sum_ix = 0; + fp_t sum_x = 0; + for( uint_t j = 0; j < k; ++j ) + { + fp_t idx = (fp_t)(j + 1); + fp_t val = x[0][i - k + 1 + j]; + sum_ix += idx * val; + sum_x += val; + } + if( sum_x == 0 ) + y[0][i] = 0; + else + y[0][i] = 1.0 - 2.0 * sum_ix / ( ( (fp_t)k + 1.0 ) * sum_x ); + } + return 0; +} + + + |