libdspl-2.0
Digital Signal Processing Algorithm Library
matrix.c
1 /*
2 * Copyright (c) 2015-2019 Sergey Bakhurin
3 * Digital Signal Processing Library [http://dsplib.org]
4 *
5 * This file is part of libdspl-2.0.
6 *
7 * is free software: you can redistribute it and/or modify
8 * it under the terms of the GNU Lesser General Public License as published by
9 * the Free Software Foundation, either version 3 of the License, or
10 * (at your option) any later version.
11 *
12 * DSPL is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU Lesser General Public License
18 * along with Foobar. If not, see <http://www.gnu.org/licenses/>.
19 */
20 #include <stdio.h>
21 #include <stdlib.h>
22 #include <string.h>
23 #include <float.h>
24 #include "dspl.h"
25 #include "dspl_internal.h"
26 #include "blas.h"
27 
28 
29 
30 
31 #ifdef DOXYGEN_ENGLISH
32 
86 #endif
87 #ifdef DOXYGEN_RUSSIAN
88 
143 #endif
144 int DSPL_API matrix_eig_cmplx(complex_t* a, int n, complex_t* v, int* info)
145 {
146  int err;
147  int sdim = 0;
148  int ldvs = 1;
149  int lwork = 2*n;
150  if(!a || !v)
151  return ERROR_PTR;
152 
153  if(n<1)
154  return ERROR_MATRIX_SIZE;
155 
156  complex_t *work=(complex_t*)malloc(lwork*sizeof(complex_t));
157  double *rwork = (double*)malloc(n*sizeof(double));
158 
159  zgees_("N", "N", NULL, &n, a, &n, &sdim, v, NULL, &ldvs, work, &lwork,
160  rwork, NULL, &err);
161 
162  if(err!=0)
163  {
164  if(info)
165  *info = err;
166  err = ERROR_LAPACK;
167  }
168  else
169  err = RES_OK;
170 
171  free(work);
172  free(rwork);
173  return err;
174 }
175 
176 
177 
178 #ifdef DOXYGEN_ENGLISH
179 
202 #endif
203 #ifdef DOXYGEN_RUSSIAN
204 
228 #endif
229 int DSPL_API matrix_eye(double* a, int n, int m)
230 {
231  int p, k;
232  if(!a)
233  return ERROR_PTR;
234  if (n < 1 || m < 1)
235  return ERROR_MATRIX_SIZE;
236 
237  k = 0;
238  memset(a, 0, n*m*sizeof(double));
239  for(p = 0; p < m; p++)
240  {
241  a[k] = 1.0;
242  k += n+1;
243  }
244 
245  return RES_OK;
246 }
247 
248 
249 #ifdef DOXYGEN_ENGLISH
250 
273 #endif
274 #ifdef DOXYGEN_RUSSIAN
275 
299 #endif
300 int DSPL_API matrix_eye_cmplx(complex_t* a, int n, int m)
301 {
302  int p, k;
303  if(!a)
304  return ERROR_PTR;
305  if (n < 1 || m < 1)
306  return ERROR_MATRIX_SIZE;
307 
308  k = 0;
309  memset(a, 0, n*m*sizeof(complex_t));
310  for(p = 0; p < m; p++)
311  {
312  RE(a[k]) = 1.0;
313  k += n+1;
314  }
315 
316  return RES_OK;
317 }
318 
319 
320 
321 
322 
323 
324 
325 #ifdef DOXYGEN_ENGLISH
326 
404 #endif
405 #ifdef DOXYGEN_RUSSIAN
406 
485 #endif
486 int DSPL_API matrix_mul(double* a, int na, int ma,
487  double* b, int nb, int mb,
488  double* c)
489 {
490 
491  double alpha = 1;
492  double beta = 0.0;
493 
494  if(!a || !b || !c)
495  return ERROR_PTR;
496  if(na < 1 || ma < 1 || nb < 1 || mb < 1 || ma != nb)
497  return ERROR_MATRIX_SIZE;
498 
499  /* BLAS DGEMM */
500  dgemm_("N", "N", &na, &mb, &ma, &alpha, a, &na, b, &nb, &beta, c, &na);
501 
502  return RES_OK;
503 }
504 
505 
506 
507 
508 #ifdef DOXYGEN_ENGLISH
509 
510 #endif
511 #ifdef DOXYGEN_RUSSIAN
512 
513 #endif
514 int DSPL_API matrix_pinv(double* a, int n, int m, double* tol,
515  double* inv, int* info)
516 {
517  int err, mn, i, j;
518  double eps;
519 
520 
521  double* u = NULL;
522  double* vt = NULL;
523  double* v = NULL;
524  double* ut = NULL;
525  double* s = NULL;
526 
527 
528  mn = (m > n) ? n : m;
529 
530  u = (double*) malloc(n*n*sizeof(double));
531  if(!u)
532  {
533  err = ERROR_MALLOC;
534  goto exit_label;
535  }
536 
537  vt = (double*) malloc(m*m*sizeof(double));
538  if(!vt)
539  {
540  err = ERROR_MALLOC;
541  goto exit_label;
542  }
543 
544  s = (double*) malloc(mn*sizeof(double));
545  if(!s)
546  {
547  err = ERROR_MALLOC;
548  goto exit_label;
549  }
550 
551  err = matrix_svd(a, n, m, u, s, vt, info);
552  if(err != RES_OK)
553  goto exit_label;
554 
555 
556 
557  if(tol)
558  eps = *tol;
559  else
560  {
561  double smax;
562  double mx = (n > m) ? (double)n : (double)m;
563  err = minmax(s, mn, NULL, &smax);
564  eps = DBL_EPSILON * mx * smax;
565  }
566 
567  for(i = 0; i < mn; i++)
568  if(s[i] > eps)
569  s[i] = 1.0 / s[i];
570  else
571  s[i] = 0.0;
572 
573  v = (double*) malloc(m*m*sizeof(double));
574  if(!v)
575  {
576  err = ERROR_MALLOC;
577  goto exit_label;
578  }
579  err = matrix_transpose(vt, m, m, v);
580  if(err != RES_OK)
581  goto exit_label;
582 
583  ut = (double*) malloc(n*n*sizeof(double));
584  if(!ut)
585  {
586  err = ERROR_MALLOC;
587  goto exit_label;
588  }
589  err = matrix_transpose(u, n, n, ut);
590  if(err != RES_OK)
591  goto exit_label;
592 
593  for(i = 0; i < mn; i++)
594  for(j = 0; j < m; j++)
595  v[j + i*m] *= s[i];
596 
597  if(mn < m)
598  memset(v+ mn*m, 0, (m-mn)*sizeof(double));
599 
600  err = matrix_mul(v, m, n, ut, n, n, inv);
601 
602 exit_label:
603  if(u)
604  free(u);
605  if(vt)
606  free(vt);
607  if(s)
608  free(s);
609  if(v)
610  free(v);
611  if(ut)
612  free(ut);
613 
614  return err;
615 }
616 
617 
618 
619 
620 
621 
622 
623 
624 #ifdef DOXYGEN_ENGLISH
625 
626 #endif
627 #ifdef DOXYGEN_RUSSIAN
628 
629 #endif
630 int DSPL_API matrix_print(double* a, int n, int m,
631  const char* name, const char* format)
632 {
633  int p,q;
634 
635  if(!a)
636  return ERROR_PTR;
637  if(n < 1 || m < 1)
638  return ERROR_SIZE;
639 
640  printf("\n%s = [ %% size [%d x %d] type: real", name, n, m);
641 
642  for(p = 0; p < n; p++)
643  {
644  printf("\n");
645  for(q = 0; q < m; q++)
646  {
647  printf(format, a[q*n + p]);
648  if(q == m-1)
649  printf(";");
650  else
651  printf(", ");
652  }
653  }
654  printf("];\n");
655 
656  return RES_OK;
657 }
658 
659 
660 #ifdef DOXYGEN_ENGLISH
661 
662 #endif
663 #ifdef DOXYGEN_RUSSIAN
664 
665 #endif
666 int DSPL_API matrix_print_cmplx(complex_t* a, int n, int m,
667  const char* name, const char* format)
668 {
669  int p,q;
670 
671  if(!a)
672  return ERROR_PTR;
673  if(n < 1 || m < 1)
674  return ERROR_MATRIX_SIZE;
675 
676  if(!a)
677  return ERROR_PTR;
678  if(n < 1 || m < 1)
679  return ERROR_SIZE;
680 
681  printf("\n%s = [ %% size [%d x %d] type: complex", name, n, m);
682 
683  for(p = 0; p < n; p++)
684  {
685  printf("\n");
686  for(q = 0; q < m; q++)
687  {
688  printf(format, RE(a[q*n + p]), IM(a[q*n + p]));
689  if(q == m-1)
690  printf(";");
691  else
692  printf(", ");
693  }
694  }
695  printf("];\n");
696 
697  return RES_OK;
698 }
699 
700 
701 
702 #ifdef DOXYGEN_ENGLISH
703 
704 #endif
705 #ifdef DOXYGEN_RUSSIAN
706 
707 #endif
708 int DSPL_API matrix_svd(double* a, int n, int m,
709  double* u, double* s, double* vt, int* info)
710 {
711  char jobz = 'A';
712  double* work = NULL;
713  int* iwork = NULL;
714  int lwork, mn, mx, err;
715  int pi;
716 
717  if(!a || !u || !s || !vt)
718  return ERROR_PTR;
719  if(n < 1 || m < 1)
720  return ERROR_SIZE;
721 
722  if(n > m)
723  {
724  mn = m;
725  mx = n;
726  }
727  else
728  {
729  mn = n;
730  mx = m;
731  }
732 
733  err = RES_OK;
734 
735  lwork = 4 * mn * mn + 6 * mn + mx;
736  work = (double*) malloc(lwork*sizeof(double));
737  iwork = (int*) malloc(8*mn*sizeof(int));
738  dgesdd_(&jobz, &n, &m, a, &n, s, u, &n, vt, &m, work, &lwork, iwork, &pi);
739 
740  if(info)
741  *info = pi;
742 
743  if(pi)
744  err = ERROR_LAPACK;
745  if(work)
746  free(work);
747  if(iwork)
748  free(iwork);
749  return err;
750 }
751 
752 
753 #ifdef DOXYGEN_ENGLISH
754 
755 #endif
756 #ifdef DOXYGEN_RUSSIAN
757 
758 #endif
759 int DSPL_API matrix_transpose(double* a, int n, int m, double* b)
760 {
761  int p, q, i, j, aind, bind;
762  if(!a || !b)
763  return ERROR_PTR;
764  if(n < 1 || m < 1)
765  return ERROR_MATRIX_SIZE;
766 
767 
768  for(p = 0; p < n - DSPL_MATRIX_BLOCK; p+=DSPL_MATRIX_BLOCK)
769  {
770  for(q = 0; q < m - DSPL_MATRIX_BLOCK; q+=DSPL_MATRIX_BLOCK)
771  {
772  for(i = 0; i < DSPL_MATRIX_BLOCK; i++)
773  {
774  for(j = 0; j < DSPL_MATRIX_BLOCK; j++)
775  {
776  aind = (q+j) * n + p + i;
777  bind = (p+i) * m + q + j;
778  b[bind] = a[aind];
779  }
780  }
781  }
782  }
783  for(i = p; i < n; i++)
784  for(j = 0; j < m; j++)
785  b[i*m + j] = a[j*n+i];
786 
787  for(i = 0; i < p; i++)
788  for(j = q; j < m; j++)
789  b[i*m + j] = a[j*n+i];
790 
791  return RES_OK;
792 }
793 
794 
795 
796 
797 
798 #ifdef DOXYGEN_ENGLISH
799 
800 #endif
801 #ifdef DOXYGEN_RUSSIAN
802 
803 #endif
804 int DSPL_API matrix_transpose_cmplx(complex_t* a, int n, int m, complex_t* b)
805 {
806  int p, q, i, j, aind, bind;
807 
808  if(!a || !b)
809  return ERROR_PTR;
810  if(n < 1 || m < 1)
811  return ERROR_MATRIX_SIZE;
812 
813  for(p = 0; p < n - DSPL_MATRIX_BLOCK; p+=DSPL_MATRIX_BLOCK)
814  {
815  for(q = 0; q < m - DSPL_MATRIX_BLOCK; q+=DSPL_MATRIX_BLOCK)
816  {
817  for(i = 0; i < DSPL_MATRIX_BLOCK; i++)
818  {
819  for(j = 0; j < DSPL_MATRIX_BLOCK; j++)
820  {
821  aind = (q+j) * n + p + i;
822  bind = (p+i) * m + q + j;
823  RE(b[bind]) = RE(a[aind]);
824  IM(b[bind]) = IM(a[aind]);
825  }
826  }
827  }
828  }
829  for(i = p; i < n; i++)
830  {
831  for(j = 0; j < m; j++)
832  {
833  RE(b[i*m + j]) = RE(a[j*n+i]);
834  IM(b[i*m + j]) = IM(a[j*n+i]);
835  }
836  }
837 
838  for(i = 0; i < p; i++)
839  {
840  for(j = q; j < m; j++)
841  {
842  RE(b[i*m + j]) = RE(a[j*n+i]);
843  IM(b[i*m + j]) = IM(a[j*n+i]);
844  }
845  }
846  return RES_OK;
847 }
848 
849 
850 
851 
852 
853 #ifdef DOXYGEN_ENGLISH
854 
855 #endif
856 #ifdef DOXYGEN_RUSSIAN
857 
858 #endif
859 int DSPL_API matrix_transpose_hermite(complex_t* a, int n, int m, complex_t* b)
860 {
861  int p, q, i, j, aind, bind;
862 
863  if(!a || !b)
864  return ERROR_PTR;
865  if(n < 1 || m < 1)
866  return ERROR_MATRIX_SIZE;
867 
868  for(p = 0; p < n - DSPL_MATRIX_BLOCK; p+=DSPL_MATRIX_BLOCK)
869  {
870  for(q = 0; q < m - DSPL_MATRIX_BLOCK; q+=DSPL_MATRIX_BLOCK)
871  {
872  for(i = 0; i < DSPL_MATRIX_BLOCK; i++)
873  {
874  for(j = 0; j < DSPL_MATRIX_BLOCK; j++)
875  {
876  aind = (q+j) * n + p + i;
877  bind = (p+i) * m + q + j;
878  RE(b[bind]) = RE(a[aind]);
879  IM(b[bind]) = -IM(a[aind]);
880  }
881  }
882  }
883  }
884  for(i = p; i < n; i++)
885  {
886  for(j = 0; j < m; j++)
887  {
888  RE(b[i*m + j]) = RE(a[j*n+i]);
889  IM(b[i*m + j]) = -IM(a[j*n+i]);
890  }
891  }
892 
893  for(i = 0; i < p; i++)
894  {
895  for(j = q; j < m; j++)
896  {
897  RE(b[i*m + j]) = RE(a[j*n+i]);
898  IM(b[i*m + j]) = -IM(a[j*n+i]);
899  }
900  }
901 
902  return RES_OK;
903 }
904 
905 
906 
907 
908 #ifdef DOXYGEN_ENGLISH
909 
910 #endif
911 #ifdef DOXYGEN_RUSSIAN
912 
913 #endif
914 int DSPL_API vector_dot(double* x, double* y, int n, double* p)
915 {
916  int inc = 1;
917 
918  if(!x || !y || !p)
919  return ERROR_PTR;
920  if(n<1)
921  return ERROR_SIZE;
922 
923  *p = ddot_(&n, x, &inc, y, &inc);
924 
925  return RES_OK;
926 }
927 
#define ERROR_LAPACK
The built-in LAPACK function returned an error code. This error is returned by the function if it use...
Definition: dspl.h:597
#define RE(x)
Macro sets real part of the complex number.
Definition: dspl.h:420
#define ERROR_PTR
Pointer error. This error means that one of the required pointers (memory to be allocated for) is tra...
Definition: dspl.h:610
#define ERROR_SIZE
Error array size. This error occurs when in addition to the pointer the wrong input is passed to the ...
Definition: dspl.h:618
int DSPL_API matrix_eig_cmplx(complex_t *a, int n, complex_t *v, int *info)
Eigenvalues calculation of the complex square matrix a.
Definition: matrix.c:144
double complex_t[2]
Complex data type.
Definition: dspl.h:86
int DSPL_API matrix_eye(double *a, int n, int m)
The real identity matrix size n x m generation.
Definition: matrix.c:229
#define RES_OK
The function completed correctly. No errors.
Definition: dspl.h:558
int DSPL_API matrix_eye_cmplx(complex_t *a, int n, int m)
The complex identity matrix size n x m generation.
Definition: matrix.c:300
#define IM(x)
Macro sets imaginary part of the complex number.
Definition: dspl.h:478
#define ERROR_MATRIX_SIZE
Matrix size error.
Definition: dspl.h:600
int DSPL_API matrix_mul(double *a, int na, int ma, double *b, int nb, int mb, double *c)
Matrix multiplication.
Definition: matrix.c:486
#define ERROR_MALLOC
Dynamic memory allocation error. This error means that an error occurred while dynamically allocating...
Definition: dspl.h:599