mirror of
https://github.com/simon987/wavelib.git
synced 2025-04-19 10:06:45 +00:00
681 lines
13 KiB
C
681 lines
13 KiB
C
/*
|
|
Copyright (c) 2018, Rafat Hussain
|
|
*/
|
|
#include "wtmath.h"
|
|
|
|
void dwt_per_stride(double *inp, int N, double *lpd,double*hpd,int lpd_len,double *cA, int len_cA, double *cD, int istride, int ostride) {
|
|
int l, l2, isodd, i, t, len_avg,is,os;
|
|
|
|
len_avg = lpd_len;
|
|
l2 = len_avg / 2;
|
|
isodd = N % 2;
|
|
|
|
for (i = 0; i < len_cA; ++i) {
|
|
t = 2 * i + l2;
|
|
os = i *ostride;
|
|
cA[os] = 0.0;
|
|
cD[os] = 0.0;
|
|
for (l = 0; l < len_avg; ++l) {
|
|
if ((t - l) >= l2 && (t - l) < N) {
|
|
is = (t - l) * istride;
|
|
cA[os] += lpd[l] * inp[is];
|
|
cD[os] += hpd[l] * inp[is];
|
|
}
|
|
else if ((t - l) < l2 && (t - l) >= 0) {
|
|
is = (t - l) * istride;
|
|
cA[os] += lpd[l] * inp[is];
|
|
cD[os] += hpd[l] * inp[is];
|
|
}
|
|
else if ((t - l) < 0 && isodd == 0) {
|
|
is = (t - l + N) * istride;
|
|
cA[os] += lpd[l] * inp[is];
|
|
cD[os] += hpd[l] * inp[is];
|
|
}
|
|
else if ((t - l) < 0 && isodd == 1) {
|
|
if ((t - l) != -1) {
|
|
is = (t - l + N + 1) * istride;
|
|
cA[os] += lpd[l] * inp[is];
|
|
cD[os] += hpd[l] * inp[is];
|
|
}
|
|
else {
|
|
is = (N - 1) * istride;
|
|
cA[os] += lpd[l] * inp[is];
|
|
cD[os] += hpd[l] * inp[is];
|
|
}
|
|
}
|
|
else if ((t - l) >= N && isodd == 0) {
|
|
is = (t - l - N) * istride;
|
|
cA[os] += lpd[l] * inp[is];
|
|
cD[os] += hpd[l] * inp[is];
|
|
}
|
|
else if ((t - l) >= N && isodd == 1) {
|
|
is = (t - l - (N + 1)) * istride;
|
|
if (t - l != N) {
|
|
cA[os] += lpd[l] * inp[is];
|
|
cD[os] += hpd[l] * inp[is];
|
|
}
|
|
else {
|
|
is = (N - 1) * istride;
|
|
cA[os] += lpd[l] * inp[is];
|
|
cD[os] += hpd[l] * inp[is];
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
void dwt_sym_stride(double *inp, int N, double *lpd, double*hpd, int lpd_len, double *cA, int len_cA, double *cD, int istride, int ostride) {
|
|
int i, l, t, len_avg;
|
|
int is, os;
|
|
len_avg = lpd_len;
|
|
|
|
for (i = 0; i < len_cA; ++i) {
|
|
t = 2 * i + 1;
|
|
os = i *ostride;
|
|
cA[os] = 0.0;
|
|
cD[os] = 0.0;
|
|
for (l = 0; l < len_avg; ++l) {
|
|
if ((t - l) >= 0 && (t - l) < N) {
|
|
is = (t - l) * istride;
|
|
cA[os] += lpd[l] * inp[is];
|
|
cD[os] += hpd[l] * inp[is];
|
|
}
|
|
else if ((t - l) < 0) {
|
|
is = (-t + l - 1) * istride;
|
|
cA[os] += lpd[l] * inp[is];
|
|
cD[os] += hpd[l] * inp[is];
|
|
}
|
|
else if ((t - l) >= N) {
|
|
is = (2 * N - t + l - 1) * istride;
|
|
cA[os] += lpd[l] * inp[is];
|
|
cD[os] += hpd[l] * inp[is];
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
|
|
}
|
|
|
|
void modwt_per_stride(int M, double *inp, int N, double *filt, int lpd_len, double *cA, int len_cA, double *cD, int istride, int ostride) {
|
|
int l, i, t, len_avg;
|
|
int is, os;
|
|
len_avg = lpd_len;
|
|
|
|
|
|
for (i = 0; i < len_cA; ++i) {
|
|
t = i;
|
|
os = i *ostride;
|
|
is = t *istride;
|
|
cA[os] = filt[0] * inp[is];
|
|
cD[os] = filt[len_avg] * inp[is];
|
|
for (l = 1; l < len_avg; l++) {
|
|
t -= M;
|
|
while (t >= len_cA) {
|
|
t -= len_cA;
|
|
}
|
|
while (t < 0) {
|
|
t += len_cA;
|
|
}
|
|
os = i * ostride;
|
|
is = t * istride;
|
|
cA[os] += filt[l] * inp[is];
|
|
cD[os] += filt[len_avg + l] * inp[is];
|
|
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
void swt_per_stride(int M, double *inp, int N, double *lpd, double*hpd, int lpd_len, double *cA, int len_cA, double *cD, int istride, int ostride) {
|
|
int l, l2, isodd, i, t, len_avg, j;
|
|
int is, os;
|
|
len_avg = M * lpd_len;
|
|
l2 = len_avg / 2;
|
|
isodd = N % 2;
|
|
|
|
for (i = 0; i < len_cA; ++i) {
|
|
t = i + l2;
|
|
os = i *ostride;
|
|
cA[os] = 0.0;
|
|
cD[os] = 0.0;
|
|
l = -1;
|
|
for (j = 0; j < len_avg; j += M) {
|
|
l++;
|
|
while (j >= len_cA) {
|
|
j -= len_cA;
|
|
}
|
|
if ((t - j) >= l2 && (t - j) < N) {
|
|
is = (t - j)*istride;
|
|
cA[os] += lpd[l] * inp[is];
|
|
cD[os] += hpd[l] * inp[is];
|
|
}
|
|
else if ((t - j) < l2 && (t - j) >= 0) {
|
|
is = (t - j)*istride;
|
|
cA[os] += lpd[l] * inp[is];
|
|
cD[os] += hpd[l] * inp[is];
|
|
}
|
|
else if ((t - j) < 0) {
|
|
is = (t - j + N)*istride;
|
|
cA[os] += lpd[l] * inp[is];
|
|
cD[os] += hpd[l] * inp[is];
|
|
}
|
|
else if ((t - j) >= N && isodd == 0) {
|
|
is = (t - j - N)*istride;
|
|
cA[os] += lpd[l] * inp[is];
|
|
cD[os] += hpd[l] * inp[is];
|
|
}
|
|
else if ((t - j) >= N && isodd == 1) {
|
|
if (t - l != N) {
|
|
is = (t - j - (N + 1))*istride;
|
|
cA[os] += lpd[l] * inp[is];
|
|
cD[os] += hpd[l] * inp[is];
|
|
}
|
|
else {
|
|
is = (N - 1)*istride;
|
|
cA[os] += lpd[l] * inp[is];
|
|
cD[os] += hpd[l] * inp[N - 1];
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
void idwt_per_stride(double *cA, int len_cA, double *cD, double *lpr, double *hpr, int lpr_len, double *X, int istride, int ostride) {
|
|
int len_avg, i, l, m, n, t, l2;
|
|
int is, ms, ns;
|
|
|
|
len_avg = lpr_len;
|
|
l2 = len_avg / 2;
|
|
m = -2;
|
|
n = -1;
|
|
|
|
for (i = 0; i < len_cA + l2 - 1; ++i) {
|
|
m += 2;
|
|
n += 2;
|
|
ms = m * ostride;
|
|
ns = n * ostride;
|
|
X[ms] = 0.0;
|
|
X[ns] = 0.0;
|
|
for (l = 0; l < l2; ++l) {
|
|
t = 2 * l;
|
|
if ((i - l) >= 0 && (i - l) < len_cA) {
|
|
is = (i - l) * istride;
|
|
X[ms] += lpr[t] * cA[is] + hpr[t] * cD[is];
|
|
X[ns] += lpr[t + 1] * cA[is] + hpr[t + 1] * cD[is];
|
|
}
|
|
else if ((i - l) >= len_cA && (i - l) < len_cA + len_avg - 1) {
|
|
is = (i - l - len_cA) * istride;
|
|
X[ms] += lpr[t] * cA[is] + hpr[t] * cD[is];
|
|
X[ns] += lpr[t + 1] * cA[is] + hpr[t + 1] * cD[is];
|
|
}
|
|
else if ((i - l) < 0 && (i - l) > -l2) {
|
|
is = (len_cA + i - l) * istride;
|
|
X[ms] += lpr[t] * cA[is] + hpr[t] * cD[is];
|
|
X[ns] += lpr[t + 1] * cA[is] + hpr[t + 1] * cD[is];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void idwt_sym_stride(double *cA, int len_cA, double *cD, double *lpr, double *hpr, int lpr_len, double *X, int istride, int ostride) {
|
|
int len_avg, i, l, m, n, t, v;
|
|
int ms, ns, is;
|
|
len_avg = lpr_len;
|
|
m = -2;
|
|
n = -1;
|
|
|
|
for (v = 0; v < len_cA; ++v) {
|
|
i = v;
|
|
m += 2;
|
|
n += 2;
|
|
ms = m * ostride;
|
|
ns = n * ostride;
|
|
X[ms] = 0.0;
|
|
X[ns] = 0.0;
|
|
for (l = 0; l < len_avg / 2; ++l) {
|
|
t = 2 * l;
|
|
if ((i - l) >= 0 && (i - l) < len_cA) {
|
|
is = (i - l) * istride;
|
|
X[ms] += lpr[t] * cA[is] + hpr[t] * cD[is];
|
|
X[ns] += lpr[t + 1] * cA[is] + hpr[t + 1] * cD[is];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void imodwt_per_stride(int M, double *cA, int len_cA, double *cD, double *filt,int lf,double *X,int istride, int ostride) {
|
|
int len_avg, i, l, t;
|
|
int is, os;
|
|
|
|
len_avg = lf;
|
|
|
|
for (i = 0; i < len_cA; ++i) {
|
|
t = i;
|
|
os = i * ostride;
|
|
is = t *istride;
|
|
X[os] = (filt[0] * cA[is]) + (filt[len_avg] * cD[is]);
|
|
for (l = 1; l < len_avg; l++) {
|
|
t += M;
|
|
while (t >= len_cA) {
|
|
t -= len_cA;
|
|
}
|
|
while (t < 0) {
|
|
t += len_cA;
|
|
}
|
|
is = t *istride;
|
|
X[os] += (filt[l] * cA[is]) + (filt[len_avg + l] * cD[is]);
|
|
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
void idwt2_shift(int shift, int rows, int cols, double *lpr, double *hpr, int lf, double *A,double *H, double *V,double *D, double *oup) {
|
|
int i, k, N, ir, ic, J, dim1, dim2;
|
|
int istride, ostride;
|
|
double *cL, *cH, *X_lp;
|
|
|
|
|
|
N = rows > cols ? 2 * rows : 2 * cols;
|
|
|
|
J = 1;
|
|
dim1 = 2 * rows;
|
|
dim2 = 2 * cols;
|
|
|
|
X_lp = (double*)malloc(sizeof(double)* (N + 2 * lf - 1));
|
|
cL = (double*)calloc(dim1*dim2, sizeof(double));
|
|
cH = (double*)calloc(dim1*dim2, sizeof(double));
|
|
|
|
ir = rows;
|
|
ic = cols;
|
|
istride = ic;
|
|
ostride = 1;
|
|
for (i = 0; i < ic; ++i) {
|
|
idwt_per_stride(A+i, ir, H+i, lpr, hpr, lf, X_lp, istride, ostride);
|
|
|
|
for (k = lf / 2 - 1; k < 2 * ir + lf / 2 - 1; ++k) {
|
|
cL[(k - lf / 2 + 1)*ic + i] = X_lp[k];
|
|
}
|
|
|
|
idwt_per_stride(V+i, ir, D+i, lpr, hpr, lf, X_lp, istride, ostride);
|
|
|
|
for (k = lf / 2 - 1; k < 2 * ir + lf / 2 - 1; ++k) {
|
|
cH[(k - lf / 2 + 1)*ic + i] = X_lp[k];
|
|
}
|
|
}
|
|
ir *= 2;
|
|
istride = 1;
|
|
ostride = 1;
|
|
|
|
for (i = 0; i < ir; ++i) {
|
|
idwt_per_stride(cL + i*ic, ic, cH + i*ic, lpr, hpr, lf, X_lp, istride, ostride);
|
|
|
|
for (k = lf / 2 - 1; k < 2 * ic + lf / 2 - 1; ++k) {
|
|
oup[(k - lf / 2 + 1) + i*ic * 2] = X_lp[k];
|
|
}
|
|
}
|
|
ic *= 2;
|
|
|
|
|
|
if (shift == -1) {
|
|
//Save the last column
|
|
for (i = 0; i < ir; ++i) {
|
|
cL[i] = oup[(i + 1)*ic - 1];
|
|
}
|
|
// Save the last row
|
|
memcpy(cH, oup + (ir - 1)*ic, sizeof(double)*ic);
|
|
for (i = ir - 1; i > 0; --i) {
|
|
memcpy(oup + i*ic + 1, oup + (i - 1)*ic, sizeof(double)*(ic - 1));
|
|
}
|
|
oup[0] = cL[ir - 1];
|
|
for (i = 1; i < ir; ++i) {
|
|
oup[i*ic] = cL[i - 1];
|
|
}
|
|
|
|
for (i = 1; i < ic; ++i) {
|
|
oup[i] = cH[i - 1];
|
|
}
|
|
}
|
|
|
|
|
|
free(X_lp);
|
|
free(cL);
|
|
free(cH);
|
|
|
|
}
|
|
|
|
int upsamp(double *x, int lenx, int M, double *y) {
|
|
int N, i, j, k;
|
|
|
|
if (M < 0) {
|
|
return -1;
|
|
}
|
|
|
|
if (M == 0) {
|
|
for (i = 0; i < lenx; ++i) {
|
|
y[i] = x[i];
|
|
}
|
|
return lenx;
|
|
}
|
|
|
|
N = M * (lenx - 1) + 1;
|
|
j = 1;
|
|
k = 0;
|
|
|
|
for (i = 0; i < N; ++i) {
|
|
j--;
|
|
y[i] = 0.0;
|
|
if (j == 0) {
|
|
y[i] = x[k];
|
|
k++;
|
|
j = M;
|
|
}
|
|
}
|
|
|
|
return N;
|
|
}
|
|
|
|
int upsamp2(double *x, int lenx, int M, double *y) {
|
|
int N, i, j, k;
|
|
// upsamp2 returns even numbered output. Last value is set to zero
|
|
if (M < 0) {
|
|
return -1;
|
|
}
|
|
|
|
if (M == 0) {
|
|
for (i = 0; i < lenx; ++i) {
|
|
y[i] = x[i];
|
|
}
|
|
return lenx;
|
|
}
|
|
|
|
N = M * lenx;
|
|
j = 1;
|
|
k = 0;
|
|
|
|
for (i = 0; i < N; ++i) {
|
|
j--;
|
|
y[i] = 0.0;
|
|
if (j == 0) {
|
|
y[i] = x[k];
|
|
k++;
|
|
j = M;
|
|
}
|
|
}
|
|
|
|
return N;
|
|
}
|
|
|
|
int downsamp(double *x, int lenx, int M, double *y) {
|
|
int N, i;
|
|
|
|
if (M < 0) {
|
|
return -1;
|
|
}
|
|
if (M == 0) {
|
|
for (i = 0; i < lenx; ++i) {
|
|
y[i] = x[i];
|
|
}
|
|
return lenx;
|
|
}
|
|
|
|
N = (lenx - 1) / M + 1;
|
|
|
|
for (i = 0; i < N; ++i) {
|
|
y[i] = x[i*M];
|
|
}
|
|
|
|
return N;
|
|
}
|
|
/*
|
|
int per_ext(double *sig, int len, int a,double *oup) {
|
|
int i,len2;
|
|
// oup is of length len + (len%2) + 2 * a
|
|
for (i = 0; i < len; ++i) {
|
|
oup[a + i] = sig[i];
|
|
}
|
|
len2 = len;
|
|
if ((len % 2) != 0) {
|
|
len2 = len + 1;
|
|
oup[a + len] = sig[len - 1];
|
|
}
|
|
for (i = 0; i < a; ++i) {
|
|
oup[a-1-i] = sig[len - 1 - i];
|
|
oup[len2 + a + i] = sig[i];
|
|
}
|
|
|
|
return len2;
|
|
|
|
}
|
|
*/
|
|
|
|
int per_ext(double *sig, int len, int a, double *oup) {
|
|
int i, len2;
|
|
double temp1;
|
|
double temp2;
|
|
for (i = 0; i < len; ++i) {
|
|
oup[a + i] = sig[i];
|
|
}
|
|
len2 = len;
|
|
if ((len % 2) != 0) {
|
|
len2 = len + 1;
|
|
oup[a + len] = sig[len - 1];
|
|
}
|
|
for (i = 0; i < a; ++i) {
|
|
temp1 = oup[a + i];
|
|
temp2 = oup[a + len2 - 1 - i];
|
|
oup[a - 1 - i] = temp2;
|
|
oup[len2 + a + i] = temp1;
|
|
}
|
|
return len2;
|
|
}
|
|
/*
|
|
int symm_ext(double *sig, int len, int a, double *oup) {
|
|
int i, len2;
|
|
// oup is of length len + 2 * a
|
|
for (i = 0; i < len; ++i) {
|
|
oup[a + i] = sig[i];
|
|
}
|
|
len2 = len;
|
|
for (i = 0; i < a; ++i) {
|
|
oup[a - 1 - i] = sig[i];
|
|
oup[len2 + a + i] = sig[len - 1 - i];
|
|
}
|
|
|
|
return len2;
|
|
|
|
}
|
|
*/
|
|
|
|
int symm_ext(double *sig, int len, int a, double *oup) {
|
|
int i, len2;
|
|
double temp1;
|
|
double temp2;
|
|
// oup is of length len + 2 * a
|
|
for (i = 0; i < len; ++i) {
|
|
oup[a + i] = sig[i];
|
|
}
|
|
len2 = len;
|
|
for (i = 0; i < a; ++i) {
|
|
temp1 = oup[a + i];
|
|
temp2 = oup[a + len2 - 1 - i];
|
|
oup[a - 1 - i] = temp1;
|
|
oup[len2 + a + i] = temp2;
|
|
}
|
|
|
|
return len2;
|
|
|
|
}
|
|
|
|
static int isign(int N) {
|
|
int M;
|
|
if (N >= 0) {
|
|
M = 1;
|
|
}
|
|
else {
|
|
M = -1;
|
|
}
|
|
|
|
return M;
|
|
}
|
|
|
|
static int iabs(int N) {
|
|
if (N >= 0) {
|
|
return N;
|
|
}
|
|
else {
|
|
return -N;
|
|
}
|
|
}
|
|
|
|
void circshift(double *array, int N, int L) {
|
|
int i;
|
|
double *temp;
|
|
if (iabs(L) > N) {
|
|
L = isign(L) * (iabs(L) % N);
|
|
}
|
|
if (L < 0) {
|
|
L = (N + L) % N;
|
|
}
|
|
|
|
temp = (double*)malloc(sizeof(double) * L);
|
|
|
|
for (i = 0; i < L; ++i) {
|
|
temp[i] = array[i];
|
|
}
|
|
|
|
for (i = 0; i < N - L; ++i) {
|
|
array[i] = array[i + L];
|
|
}
|
|
|
|
for (i = 0; i < L; ++i) {
|
|
array[N - L + i] = temp[i];
|
|
}
|
|
|
|
free(temp);
|
|
}
|
|
|
|
int testSWTlength(int N, int J) {
|
|
int ret,div,i;
|
|
ret = 1;
|
|
|
|
div = 1;
|
|
for (i = 0; i < J; ++i) {
|
|
div *= 2;
|
|
}
|
|
|
|
if (N % div) {
|
|
ret = 0;
|
|
}
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
int wmaxiter(int sig_len, int filt_len) {
|
|
int lev;
|
|
double temp;
|
|
|
|
temp = log((double)sig_len / ((double)filt_len - 1.0)) / log(2.0);
|
|
lev = (int)temp;
|
|
|
|
return lev;
|
|
}
|
|
|
|
static double entropy_s(double *x,int N) {
|
|
int i;
|
|
double val,x2;
|
|
|
|
val = 0.0;
|
|
|
|
for(i = 0; i < N; ++i) {
|
|
if (x[i] != 0) {
|
|
x2 = x[i] * x[i];
|
|
val -= x2 * log(x2);
|
|
}
|
|
}
|
|
return val;
|
|
}
|
|
|
|
static double entropy_t(double *x,int N, double t) {
|
|
int i;
|
|
double val,x2;
|
|
if (t < 0) {
|
|
printf("Threshold value must be >= 0");
|
|
exit(1);
|
|
}
|
|
val = 0.0;
|
|
|
|
for(i = 0; i < N; ++i) {
|
|
x2 = fabs(x[i]);
|
|
if (x2 > t) {
|
|
val += 1;
|
|
}
|
|
|
|
}
|
|
|
|
return val;
|
|
|
|
}
|
|
|
|
static double entropy_n(double *x,int N,double p) {
|
|
int i;
|
|
double val,x2;
|
|
if (p < 1) {
|
|
printf("Norm power value must be >= 1");
|
|
exit(1);
|
|
}
|
|
val = 0.0;
|
|
for(i = 0; i < N; ++i) {
|
|
x2 = fabs(x[i]);
|
|
val += pow(x2,(double)p);
|
|
|
|
}
|
|
|
|
return val;
|
|
}
|
|
|
|
static double entropy_l(double *x,int N) {
|
|
int i;
|
|
double val,x2;
|
|
|
|
val = 0.0;
|
|
|
|
for(i = 0; i < N; ++i) {
|
|
if (x[i] != 0) {
|
|
x2 = x[i] * x[i];
|
|
val += log(x2);
|
|
}
|
|
}
|
|
return val;
|
|
}
|
|
|
|
double costfunc(double *x, int N ,char *entropy,double p) {
|
|
double val;
|
|
|
|
if (!strcmp(entropy, "shannon")) {
|
|
val = entropy_s(x, N);
|
|
}
|
|
else if (!strcmp(entropy, "threshold")) {
|
|
val = entropy_t(x, N,p);
|
|
}
|
|
else if (!strcmp(entropy, "norm")) {
|
|
val = entropy_n(x, N,p);
|
|
}
|
|
else if (!strcmp(entropy, "logenergy") || !strcmp(entropy, "log energy") || !strcmp(entropy, "energy")) {
|
|
val = entropy_l(x, N);
|
|
}
|
|
else {
|
|
printf("Entropy must be one of shannon, threshold, norm or energy");
|
|
exit(-1);
|
|
}
|
|
|
|
return val;
|
|
}
|