#include #include #include #include #include #include "vec_match.h" #define SQUARE(a) ((a)*(a)) bool vec_match(const float *data, const float *ref, const vec_match_cfg_t *cfg, float *fuzzy_match_error, float *abs_match_error) { int a, b; int err_cnt = 0; float env_err = 0; float abs_err = 0; for (int i = 0; i < (int)cfg->length; i++) { float peak = FLT_MIN; float base = FLT_MAX; // find highest value in the surrounding drift_x points a = i - cfg->drift_x; b = i + cfg->drift_x; if (a < 0) a = 0; if (b >= (int)cfg->length) b = cfg->length - 1; for (int j = a; j <= b; j++) { if (peak < ref[j]) peak = ref[j]; if (base > ref[j]) base = ref[j]; } // apply drift_y peak += cfg->offset_y; // add abs threshold on top base -= cfg->offset_y; // ignore abs threshold difference (float precision error) if (fabs(ref[i] - data[i]) > cfg->abs_threshold) { abs_err += SQUARE(ref[i] - data[i]); } if (data[i] >= (base - cfg->abs_threshold) && data[i] <= (peak + cfg->abs_threshold)) { // within limits continue; } else { //printf("data[%d] out of range: %f, [%f ; %f]\n", i, data[i], base, peak); if (data[i] < base) env_err += SQUARE(base - data[i]); if (data[i] > peak) env_err += SQUARE(data[i] - peak); err_cnt++; } } // write error values to provided fields if (fuzzy_match_error != NULL) *fuzzy_match_error = env_err; if (abs_match_error != NULL) *abs_match_error = abs_err; return err_cnt == 0; } uint32_t vec_pack(float *result, uint32_t result_capacity, const float *data, uint32_t data_length, float threshold) { uint32_t result_len = 0; uint32_t zeroes = 0; for (uint32_t i = 0; i < data_length; i++) { if (data[i] < threshold) { zeroes++; } else { // write zero marker to result if (zeroes) { if (result_len < result_capacity) { result[result_len] = 0.0f - zeroes; // float and negative } zeroes = 0; result_len++; //length is increased even if buffer full } if (result_len < result_capacity) { result[result_len] = data[i]; } result_len++; } } // handle trailing zeroes if (zeroes) { if (result_len < result_capacity) { result[result_len] = 0.0f - zeroes; } result_len++; } return result_len; }