From ef1ba40bfbb3ba79ab4b7bcde3ae1b088b6b8384 Mon Sep 17 00:00:00 2001 From: Dsplib Date: Tue, 26 Feb 2019 23:37:58 +0300 Subject: [PATCH] added matrix_lu decomposition --- dspl/src/array.c | 28 +++++++ dspl/src/dspl_internal.h | 3 + dspl/src/matrix.c | 157 ++++++++++++++++++++++++++++++++++++++- include/dspl.c | 8 ++ include/dspl.h | 29 +++++++- release/include/dspl.c | 8 ++ release/include/dspl.h | 29 +++++++- 7 files changed, 257 insertions(+), 5 deletions(-) diff --git a/dspl/src/array.c b/dspl/src/array.c index 25bff5a..e6eae58 100644 --- a/dspl/src/array.c +++ b/dspl/src/array.c @@ -103,6 +103,34 @@ int DSPL_API decimate_cmplx(complex_t* x, int n, int dec, return RES_OK; } +/****************************************************************************** +Find max(|a|) +*******************************************************************************/ +int DSPL_API find_max_abs(double* a, int n, double* m, int* ind) +{ + int k, i; + double t; + if(!a) + return ERROR_PTR; + if(n < 1) + return ERROR_SIZE; + t = fabs(a[0]); + i = 0; + for(k = 1; k < n; k++) + { + if(fabs(a[k]) > t) + { + t = a[k]; + i = k; + } + } + if(m) + *m = t; + if(ind) + *ind = i; + return RES_OK; +} + /****************************************************************************** Flip real array in place diff --git a/dspl/src/dspl_internal.h b/dspl/src/dspl_internal.h index ee677ea..e87085a 100644 --- a/dspl/src/dspl_internal.h +++ b/dspl/src/dspl_internal.h @@ -88,6 +88,9 @@ int win_rect (double *w, int n); int fir_linphase_lpf(int ord, double wp, int wintype, double winparam, double* h); + +#define MATRIX_SINGULAR_THRESHOLD 1E-14 + #endif diff --git a/dspl/src/matrix.c b/dspl/src/matrix.c index affce68..d40f1f1 100644 --- a/dspl/src/matrix.c +++ b/dspl/src/matrix.c @@ -59,7 +59,43 @@ int DSPL_API matrix_create(matrix_t* a, int n, int m, int type) } - +/******************************************************************************* +matrix_create eye +*******************************************************************************/ +int DSPL_API matrix_create_eye(matrix_t* a, int n, int type) +{ + double *pr; + complex_t *pc; + int err, m, k; + + err = matrix_create(a, n, n, type); + if(err != RES_OK) + return RES_OK; + + k = 0; + if((a->type & DAT_MASK) == DAT_DOUBLE) + { + pr = (double*) a->dat; + memset(pr, 0, n*n*sizeof(double)); + for(m = 0; m < n; m++) + { + pr[k] = 1.0; + k += n+1; + } + } + + if((a->type & DAT_MASK) == DAT_COMPLEX) + { + pc = (complex_t*) a->dat; + memset(pc, 0, n*n*sizeof(complex_t)); + for(m = 0; m < n; m++) + { + RE(pc[k]) = 1.0; + k += n+1; + } + } + return RES_OK; +} @@ -76,7 +112,68 @@ void DSPL_API matrix_free(matrix_t* a) } +/******************************************************************************* +matrix LU decomposition +*******************************************************************************/ +int DSPL_API matrix_lu(matrix_t* a, matrix_t* L, matrix_t* U, matrix_t* P) +{ + int err, k, n, m, N, ind; + double *ra, *rl, *ru, mu, ukk, gmax; + + if(!a || !L || !U || !P) + return ERROR_PTR; + + if(a->n != a->m || a->n < 1) + return ERROR_MATRIX_SIZE; + + N = a->n; + err = matrix_create(L, N, N, a->type); + if(err != RES_OK) + return err; + + err = matrix_create(U, N, N, a->type); + if(err != RES_OK) + return err; + err = matrix_create_eye(P, N, a->type); + if(err != RES_OK) + return err; + if((a->type & DAT_MASK) == DAT_DOUBLE) + { + rl = (double*)L->dat; + ru = (double*)U->dat; + + memcpy(ru, (double*)a->dat, N*N*sizeof(double)); + memset(rl, 0, N*N*sizeof(double)); + + find_max_abs(ru, N*N, &gmax, NULL); + for(k = 0; k < N; k++) + { + find_max_abs(ru+k*N+k, N-k, NULL, &ind); + ind += k; + matrix_swap_rows(U, k, ind); + matrix_swap_rows(L, k, ind); + matrix_swap_rows(P, k, ind); + ukk = ru[N*k+k]; + if(fabs(ukk / gmax) < MATRIX_SINGULAR_THRESHOLD) + return ERROR_MATRIX_SINGULAR; + + for(m = k+1; m < N; m++) + { + mu = ru[m+k*N] / ukk; + rl[m+k*N] = mu; + for(n = k; n < N; n++) + { + ru[m + n*N] -= ru[k + n*N] * mu; + } + } + } + for(n =0; n < N; n++) + rl[n+n*N] = 1.0; + } + + return RES_OK; +} @@ -125,7 +222,65 @@ int DSPL_API matrix_print(matrix_t* a, const char* name, const char* format) } +/******************************************************************************* +matrix swap 2 elements +*******************************************************************************/ +int DSPL_API matrix_swap(matrix_t* a, int r0, int c0, int r1, int c1) +{ + double tr; + complex_t tc; + double *pr; + complex_t *pc; + if(!a) + return ERROR_PTR; + if(r0 >= a->n || r1 >= a->n || c0 >= a->m || c1 >= a->m) + return ERROR_MATRIX_INDEX; + + if((a->type & DAT_MASK) == DAT_DOUBLE) + { + pr = (double*)(a->dat); + tr = pr[r0 + c0 * a->n]; + pr[r0 + c0 * a->n] = pr[r1 + c1 * a->n]; + pr[r1 + c1 * a->n] = tr; + } + + if((a->type & DAT_MASK) == DAT_COMPLEX) + { + pc = (complex_t*)(a->dat); + RE(tc) = RE(pc[r0 + c0 * a->n]); + IM(tc) = IM(pc[r0 + c0 * a->n]); + + RE(pc[r0 + c0 * a->n]) = RE(pc[r1 + c1 * a->n]); + IM(pc[r0 + c0 * a->n]) = IM(pc[r1 + c1 * a->n]); + + RE(pc[r1 + c1 * a->n]) = RE(tc); + IM(pc[r1 + c1 * a->n]) = IM(tc); + } + return RES_OK; +} + + + +/******************************************************************************* +matrix swap 2 rows +*******************************************************************************/ +int DSPL_API matrix_swap_rows(matrix_t* a, int r0, int r1) +{ + int c, err; + if(!a) + return ERROR_PTR; + if(r0 >= a->n || r1 >= a->n) + return ERROR_MATRIX_INDEX; + + for(c = 0; c < a->m; c++) + { + err = matrix_swap(a, r0, c, r1, c); + if(err != RES_OK) + break; + } + return err; +} diff --git a/include/dspl.c b/include/dspl.c index 16ec5ed..1c37b91 100644 --- a/include/dspl.c +++ b/include/dspl.c @@ -83,6 +83,7 @@ p_fft_shift fft_shift ; p_fft_shift_cmplx fft_shift_cmplx ; p_filter_iir filter_iir ; p_filter_zp2ab filter_zp2ab ; +p_find_max_abs find_max_abs ; p_fir_linphase fir_linphase ; p_flipip flipip ; p_flipip_cmplx flipip_cmplx ; @@ -104,8 +105,11 @@ p_low2bp low2bp ; p_low2high low2high ; p_low2low low2low ; p_matrix_create matrix_create ; +p_matrix_create_eye matrix_create_eye ; p_matrix_free matrix_free ; p_matrix_print matrix_print ; +p_matrix_swap matrix_swap ; +p_matrix_swap_rows matrix_swap_rows ; p_matrix_transpose matrix_transpose ; p_matrix_transpose_hermite matrix_transpose_hermite ; p_poly_z2a_cmplx poly_z2a_cmplx ; @@ -234,6 +238,7 @@ void* dspl_load() LOAD_FUNC(fft_shift_cmplx); LOAD_FUNC(filter_iir); LOAD_FUNC(filter_zp2ab); + LOAD_FUNC(find_max_abs); LOAD_FUNC(fir_linphase); LOAD_FUNC(flipip); LOAD_FUNC(flipip_cmplx); @@ -255,8 +260,11 @@ void* dspl_load() LOAD_FUNC(low2high); LOAD_FUNC(low2low); LOAD_FUNC(matrix_create); + LOAD_FUNC(matrix_create_eye); LOAD_FUNC(matrix_free); LOAD_FUNC(matrix_print); + LOAD_FUNC(matrix_swap); + LOAD_FUNC(matrix_swap_rows); LOAD_FUNC(matrix_transpose); LOAD_FUNC(matrix_transpose_hermite); LOAD_FUNC(poly_z2a_cmplx); diff --git a/include/dspl.h b/include/dspl.h index 24ca435..670dfd1 100644 --- a/include/dspl.h +++ b/include/dspl.h @@ -111,7 +111,9 @@ typedef struct /* K 0x11xxxxxx*/ /* L 0x12xxxxxx*/ /* M 0x13xxxxxx*/ -#define ERROR_MATRIX_SIZE 0x13011909 +#define ERROR_MATRIX_INDEX 0x13010914 +#define ERROR_MATRIX_SINGULAR 0x13011914 +#define ERROR_MATRIX_SIZE 0x13011926 /* N 0x14xxxxxx*/ #define ERROR_NEGATIVE 0x14050701 /* O 0x15xxxxxx*/ @@ -472,6 +474,11 @@ DECLARE_FUNC(int, filter_zp2ab, complex_t* COMMA double* COMMA double*); //------------------------------------------------------------------------------ +DECLARE_FUNC(int, find_max_abs, double* a + COMMA int n + COMMA double* m + COMMA int* ind); +//------------------------------------------------------------------------------ DECLARE_FUNC(int, fir_linphase, int ord COMMA double w0 COMMA double w1 @@ -613,14 +620,32 @@ DECLARE_FUNC(int, matrix_create, matrix_t* a COMMA int n COMMA int m COMMA int type); - +//------------------------------------------------------------------------------ +DECLARE_FUNC(int, matrix_create_eye, matrix_t* a + COMMA int n + COMMA int type); //------------------------------------------------------------------------------ DECLARE_FUNC(void, matrix_free, matrix_t* a); //------------------------------------------------------------------------------ +DECLARE_FUNC(int, matrix_lu, matrix_t* a + COMMA matrix_t* L + COMMA matrix_t* U + COMMA matrix_t* P); +//------------------------------------------------------------------------------ DECLARE_FUNC(int, matrix_print, matrix_t* a COMMA const char* name COMMA const char* format); //------------------------------------------------------------------------------ +DECLARE_FUNC(int, matrix_swap, matrix_t* a + COMMA int r0 + COMMA int c0 + COMMA int r1 + COMMA int c1); +//------------------------------------------------------------------------------ +DECLARE_FUNC(int, matrix_swap_rows, matrix_t* a + COMMA int r0 + COMMA int r1); +//------------------------------------------------------------------------------ DECLARE_FUNC(int, matrix_transpose, matrix_t* a COMMA matrix_t* b); //------------------------------------------------------------------------------ diff --git a/release/include/dspl.c b/release/include/dspl.c index 16ec5ed..1c37b91 100644 --- a/release/include/dspl.c +++ b/release/include/dspl.c @@ -83,6 +83,7 @@ p_fft_shift fft_shift ; p_fft_shift_cmplx fft_shift_cmplx ; p_filter_iir filter_iir ; p_filter_zp2ab filter_zp2ab ; +p_find_max_abs find_max_abs ; p_fir_linphase fir_linphase ; p_flipip flipip ; p_flipip_cmplx flipip_cmplx ; @@ -104,8 +105,11 @@ p_low2bp low2bp ; p_low2high low2high ; p_low2low low2low ; p_matrix_create matrix_create ; +p_matrix_create_eye matrix_create_eye ; p_matrix_free matrix_free ; p_matrix_print matrix_print ; +p_matrix_swap matrix_swap ; +p_matrix_swap_rows matrix_swap_rows ; p_matrix_transpose matrix_transpose ; p_matrix_transpose_hermite matrix_transpose_hermite ; p_poly_z2a_cmplx poly_z2a_cmplx ; @@ -234,6 +238,7 @@ void* dspl_load() LOAD_FUNC(fft_shift_cmplx); LOAD_FUNC(filter_iir); LOAD_FUNC(filter_zp2ab); + LOAD_FUNC(find_max_abs); LOAD_FUNC(fir_linphase); LOAD_FUNC(flipip); LOAD_FUNC(flipip_cmplx); @@ -255,8 +260,11 @@ void* dspl_load() LOAD_FUNC(low2high); LOAD_FUNC(low2low); LOAD_FUNC(matrix_create); + LOAD_FUNC(matrix_create_eye); LOAD_FUNC(matrix_free); LOAD_FUNC(matrix_print); + LOAD_FUNC(matrix_swap); + LOAD_FUNC(matrix_swap_rows); LOAD_FUNC(matrix_transpose); LOAD_FUNC(matrix_transpose_hermite); LOAD_FUNC(poly_z2a_cmplx); diff --git a/release/include/dspl.h b/release/include/dspl.h index 24ca435..670dfd1 100644 --- a/release/include/dspl.h +++ b/release/include/dspl.h @@ -111,7 +111,9 @@ typedef struct /* K 0x11xxxxxx*/ /* L 0x12xxxxxx*/ /* M 0x13xxxxxx*/ -#define ERROR_MATRIX_SIZE 0x13011909 +#define ERROR_MATRIX_INDEX 0x13010914 +#define ERROR_MATRIX_SINGULAR 0x13011914 +#define ERROR_MATRIX_SIZE 0x13011926 /* N 0x14xxxxxx*/ #define ERROR_NEGATIVE 0x14050701 /* O 0x15xxxxxx*/ @@ -472,6 +474,11 @@ DECLARE_FUNC(int, filter_zp2ab, complex_t* COMMA double* COMMA double*); //------------------------------------------------------------------------------ +DECLARE_FUNC(int, find_max_abs, double* a + COMMA int n + COMMA double* m + COMMA int* ind); +//------------------------------------------------------------------------------ DECLARE_FUNC(int, fir_linphase, int ord COMMA double w0 COMMA double w1 @@ -613,14 +620,32 @@ DECLARE_FUNC(int, matrix_create, matrix_t* a COMMA int n COMMA int m COMMA int type); - +//------------------------------------------------------------------------------ +DECLARE_FUNC(int, matrix_create_eye, matrix_t* a + COMMA int n + COMMA int type); //------------------------------------------------------------------------------ DECLARE_FUNC(void, matrix_free, matrix_t* a); //------------------------------------------------------------------------------ +DECLARE_FUNC(int, matrix_lu, matrix_t* a + COMMA matrix_t* L + COMMA matrix_t* U + COMMA matrix_t* P); +//------------------------------------------------------------------------------ DECLARE_FUNC(int, matrix_print, matrix_t* a COMMA const char* name COMMA const char* format); //------------------------------------------------------------------------------ +DECLARE_FUNC(int, matrix_swap, matrix_t* a + COMMA int r0 + COMMA int c0 + COMMA int r1 + COMMA int c1); +//------------------------------------------------------------------------------ +DECLARE_FUNC(int, matrix_swap_rows, matrix_t* a + COMMA int r0 + COMMA int r1); +//------------------------------------------------------------------------------ DECLARE_FUNC(int, matrix_transpose, matrix_t* a COMMA matrix_t* b); //------------------------------------------------------------------------------