diff --git a/src/3rdparty/libMultivariateOpt/libgenmath/genmath.vcxproj b/src/3rdparty/libMultivariateOpt/libgenmath/genmath.vcxproj index 77ae7c1..11e17e6 100644 --- a/src/3rdparty/libMultivariateOpt/libgenmath/genmath.vcxproj +++ b/src/3rdparty/libMultivariateOpt/libgenmath/genmath.vcxproj @@ -19,6 +19,7 @@ + @@ -27,6 +28,7 @@ + diff --git a/src/3rdparty/libMultivariateOpt/libgenmath/iirZpSmooth.c b/src/3rdparty/libMultivariateOpt/libgenmath/iirZpSmooth.c new file mode 100644 index 0000000..d3b77f7 --- /dev/null +++ b/src/3rdparty/libMultivariateOpt/libgenmath/iirZpSmooth.c @@ -0,0 +1,91 @@ +#include +#include +#include +#include +#include "iirZpSmooth.h" +typedef struct +{ + unsigned int pos1, pos2, m, n, target1, target2, tmpLen, segSize; + float alpha, minusAlpha, avgGain; +} edgPreSmooth; +#ifndef min +#define min(a,b) (((a)<(b))?(a):(b)) +#endif +size_t edgePreservingSmoothInit(void *handle, unsigned int m, double ms, double fs) +{ + unsigned int n = min(m, (unsigned int)(ms * (fs / 1000.0))); + if (!handle) + return sizeof(edgPreSmooth) + (m + n - 1) * sizeof(float); + edgPreSmooth *sm = (edgPreSmooth*)handle; + sm->n = n; + sm->m = m; + sm->tmpLen = m + n - 1; + sm->alpha = (float)(1.0 - exp(-1.0 / ((ms / 1000.0) * fs))); + sm->minusAlpha = 1.0f - sm->alpha; + sm->pos1 = n >> 1; + if (n % 2) + { + sm->pos2 = sm->pos1 + 1; + sm->target1 = m + n - sm->pos1; + sm->target2 = m + n - 1 - sm->pos1; + } + else + { + sm->pos2 = sm->pos1; + sm->target1 = m + n + 1 - sm->pos1; + sm->target2 = m + n - sm->pos1; + } + sm->segSize = max(1, sm->pos1 / 4); + sm->avgGain = 1.0f / (float)sm->segSize; + return 0; +} +unsigned int edgePreservingSmoothGetInputLen(void *handle) +{ + edgPreSmooth *sm = (edgPreSmooth*)handle; + return sm->m; +} +void emaZpSmooth(void *handle, float *x, float *y, char boundaryMode) +{ + edgPreSmooth *sm = (edgPreSmooth*)handle; + unsigned int i; + float *tmp = (float*)((char*)handle + sizeof(edgPreSmooth)); + if (!boundaryMode) + { + float frontAvg = 0.0f; + float backAvg = 0.0f; + for (i = 0; i < sm->segSize; i++) + { + frontAvg += x[i]; + backAvg += x[sm->m - 1 - i]; + } + frontAvg = frontAvg * sm->avgGain; + backAvg = backAvg * sm->avgGain; + for (i = 0; i < sm->pos1; i++) + { + tmp[i] = frontAvg; + tmp[sm->m - 1 + i + sm->pos2] = backAvg; + } + } + else + { + for (i = 0; i < sm->pos1; i++) + { + tmp[i] = 2.0f * x[0] - x[sm->pos2 - 1 - i]; + tmp[sm->m - 1 + i + sm->pos2] = 2.0f * x[sm->m - 1] - x[sm->m - 2 - i]; + } + } + memcpy(tmp + sm->pos1, x, sm->m * sizeof(float)); + float ema = 0.0f; + for (i = 0; i < sm->tmpLen; i++) + { + ema = tmp[i] * sm->alpha + ema * sm->minusAlpha; + tmp[i] = ema; + } + for (i = sm->tmpLen; i-- >= sm->target1; ) + ema = tmp[i] * sm->alpha + ema * sm->minusAlpha; + for (i = sm->target2; i-- > sm->pos1; ) + { + ema = tmp[i] * sm->alpha + ema * sm->minusAlpha; + y[i - sm->pos1] = ema; + } +} \ No newline at end of file diff --git a/src/3rdparty/libMultivariateOpt/libgenmath/iirZpSmooth.h b/src/3rdparty/libMultivariateOpt/libgenmath/iirZpSmooth.h new file mode 100644 index 0000000..ac28654 --- /dev/null +++ b/src/3rdparty/libMultivariateOpt/libgenmath/iirZpSmooth.h @@ -0,0 +1,3 @@ +extern size_t edgePreservingSmoothInit(void *sm, unsigned int m, double ms, double fs); +extern unsigned int edgePreservingSmoothGetInputLen(void *handle); +extern void emaZpSmooth(void *sm, float *x, float *y, char boundaryMode); \ No newline at end of file diff --git a/src/3rdparty/libMultivariateOpt/libgenmath/sortingIdx.c b/src/3rdparty/libMultivariateOpt/libgenmath/sortingIdx.c index f7b4807..e12234a 100644 --- a/src/3rdparty/libMultivariateOpt/libgenmath/sortingIdx.c +++ b/src/3rdparty/libMultivariateOpt/libgenmath/sortingIdx.c @@ -1,3 +1,4 @@ +#include static void merge(unsigned int idx_data[], double x_data[], int offset, int np, int nq, unsigned int iwork_data[], double xwork_data[]) { int exitg1; @@ -88,10 +89,10 @@ void sort(double x_data[], const unsigned int xSize, unsigned int idx_data[]) idx_data[0] = 0; return; } - double vwork_data[3777]; - double xwork_data[3777]; - unsigned int iidx_data[3777]; - unsigned int iwork_data[3777]; + double *vwork_data = (double*)malloc(xSize * sizeof(double)); + double *xwork_data = (double*)malloc(xSize * sizeof(double)); + unsigned int *iidx_data = (unsigned int*)malloc(xSize * sizeof(unsigned int)); + unsigned int *iwork_data = (unsigned int*)malloc(xSize * sizeof(unsigned int)); double x4[4]; unsigned int idx4[4]; signed char perm[4]; @@ -258,4 +259,8 @@ void sort(double x_data[], const unsigned int xSize, unsigned int idx_data[]) x_data[k] = vwork_data[k]; idx_data[k] = iidx_data[k] - 1; } + free(vwork_data); + free(xwork_data); + free(iidx_data); + free(iwork_data); } \ No newline at end of file