123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375 |
- /*************************************/
- /* Auteur : Rémi Synave */
- /* Date de création : 01/03/07 */
- /* Date de modification : 15/03/15 */
- /* Version : 0.4 */
- /*************************************/
- /***************************************************************************/
- /* This file is part of a2ri. */
- /* */
- /* a2ri is free software: you can redistribute it and/or modify it */
- /* under the terms of the GNU Lesser General Public License as published */
- /* by the Free Software Foundation, either version 3 of the License, or */
- /* (at your option) any later version. */
- /* */
- /* a2ri is distributed in the hope that it will be useful, */
- /* but WITHOUT ANY WARRANTY; without even the implied warranty of */
- /* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the */
- /* GNU Lesser General Public License for more details. */
- /* */
- /* You should have received a copy of the GNU Lesser General Public */
- /* License along with a2ri. */
- /* If not, see <http://www.gnu.org/licenses/>. */
- /***************************************************************************/
- #include "matrix.h"
- /********** INTERMEDIATE TYPES AND FUNCTIONS **********/
- /* Les fonctions intermédiaires sont préfixées de IF */
- /* et les types intermédiaires de IT */
- /********** MAIN FUNCTIONS **********/
- /**
- Calcul du déterminant d'une matrice
- @param m pointeur sur une matrice
- @return le déterminant
- */
- double
- matrix_determinant (
- const gsl_matrix * const m)
- {
- double det;
- int s;
- gsl_permutation *perm = gsl_permutation_alloc (m->size1);
- a2ri_erreur_critique_si (perm == NULL,
- "erreur allocation memoire pour perm\nmatrix_determinant");
- gsl_matrix *ludecomp = gsl_matrix_alloc (m->size1, m->size1);
- a2ri_erreur_critique_si (ludecomp == NULL,
- "erreur allocation memoire pour ludecomp\nmatrix_determinant");
- gsl_matrix_memcpy (ludecomp, m);
- //get the LU decomposition
- gsl_linalg_LU_decomp (ludecomp, perm, &s);
- //get the determinant
- det = gsl_linalg_LU_det (ludecomp, s);
- gsl_matrix_free (ludecomp);
- gsl_permutation_free (perm);
- return det;
- }
- /**
- Calcul de la norme de Frobenius
- @param m pointeur sur une matrice
- @return la norme de Frobenius
- */
- double
- matrix_frobenius_norm (
- const gsl_matrix * const m)
- {
- double somme = 0.0;
- for (size_t i = 0; i < m->size1; i++)
- for (size_t j = 0; j < m->size2; j++)
- somme += gsl_matrix_get (m, i, j) * gsl_matrix_get (m, i, j);
- somme = sqrt (somme);
- return somme;
- }
- /**
- Addition de deux matrices
- @param A pointeur sur la première matrice
- @param B pointeur sur la seconde matrice
- @return pointeur sur une matrice contenant l'addition A+B
- */
- gsl_matrix *
- matrix_add (
- const gsl_matrix * const A,
- const gsl_matrix * const B)
- {
- gsl_matrix *res;
- a2ri_erreur_critique_si (A->size1 != B->size1
- || A->size2 != B->size2,
- "matrice incompatible pour l'addition");
- res = gsl_matrix_alloc (A->size1, B->size2);
- a2ri_erreur_critique_si (res == NULL,
- "erreur allocation memoire pour res\nmatrix_add");
- for (size_t i = 0; i < A->size1; i++)
- for (size_t j = 0; j < A->size2; j++)
- gsl_matrix_set (res, i, j,
- gsl_matrix_get (A, i, j) + gsl_matrix_get (B, i, j));
- return res;
- }
- /**
- Soustraction de deux matrices
- @param A pointeur sur la première matrice
- @param B pointeur sur la seconde matrice
- @return pointeur sur une matrice contenant la soustraction A-B
- */
- gsl_matrix *
- matrix_sub (
- const gsl_matrix * const A,
- const gsl_matrix * const B)
- {
- gsl_matrix *res;
- a2ri_erreur_critique_si (A->size1 != B->size1
- || A->size2 != B->size2,
- "matrice incompatible pour la soustraction");
- res = gsl_matrix_alloc (A->size1, B->size2);
- a2ri_erreur_critique_si (res == NULL,
- "erreur allocation memoire pour res\nmatrix_sub");
- for (size_t i = 0; i < A->size1; i++)
- for (size_t j = 0; j < A->size2; j++)
- gsl_matrix_set (res, i, j,
- gsl_matrix_get (A, i, j) - gsl_matrix_get (B, i, j));
- return res;
- }
- /**
- Multiplication d'une matrice par un scalaire
- @param A pointeur sur une matrice
- @param n scalaire par lequel on multiplie la matrice
- @return pointeur sur la matrice contenant n*A
- */
- gsl_matrix *
- matrix_mul_scale (
- const gsl_matrix * const A,
- double n)
- {
- gsl_matrix *res = gsl_matrix_alloc (A->size1, A->size2);
- a2ri_erreur_critique_si (res == NULL,
- "erreur allocation memoire pour res\nmatrix_scale");
- for (size_t i = 0; i < A->size1; i++)
- for (size_t j = 0; j < A->size2; j++)
- gsl_matrix_set (res, i, j, gsl_matrix_get (A, i, j) * n);
- return res;
- }
- /**
- Multiplication de deux matrices
- @param A pointeur sur la première matrice
- @param B pointeur sur la seconde matrice
- @return pointeur sur une matrice contenant la multiplication A*B
- @warning Erreur du programme si le nombre de colonne de la première matrice est différent du nombre de ligne de la seconde.
- */
- gsl_matrix *
- matrix_mul (
- const gsl_matrix * const A,
- const gsl_matrix * const B)
- {
- double alpha = 1.,
- beta = 0.;
- gsl_matrix *C = gsl_matrix_alloc (A->size1, B->size2);
- a2ri_erreur_critique_si (C == NULL,
- "erreur allocation memoire pour C\nmatrix_mul");
- gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, alpha, A, B, beta, C);
- return C;
- }
- /**
- Calcul de l'inverse d'une matrice
- @param m pointeur sur une matrice
- @return pointeur sur une matrice contenant l'inverse \f$A^{-1}\f$
- */
- gsl_matrix *
- matrix_inverse (
- const gsl_matrix * const m)
- {
- int s;
- gsl_permutation *p = gsl_permutation_alloc (m->size1);
- a2ri_erreur_critique_si (p == NULL,
- "erreur allocation memoire pour p\nmatrix_inverse");
- gsl_matrix *ludecomp = gsl_matrix_alloc (m->size1, m->size1);
- a2ri_erreur_critique_si (ludecomp == NULL,
- "erreur allocation memoire pour ludecomp\nmatrix_inverse");
- gsl_matrix *inv = gsl_matrix_alloc (m->size1, m->size1);
- a2ri_erreur_critique_si (inv == NULL,
- "erreur allocation memoire pour inv\nmatrix_inverse");
- gsl_matrix_memcpy (ludecomp, m);
- gsl_linalg_LU_decomp (ludecomp, p, &s);
- gsl_linalg_LU_invert (ludecomp, p, inv);
- gsl_matrix_free (ludecomp);
- gsl_permutation_free (p);
- return inv;
- }
- /**
- Affichage d'une matrice
- @param m pointeur sur une matrice
- @return aucun
- */
- void
- matrix_display (
- const gsl_matrix * const m)
- {
- printf ("[[\n");
- for (size_t i = 0; i < m->size1; i++)
- {
- for (size_t j = 0; j < m->size2; j++)
- printf ("%4.2lf\t", gsl_matrix_get (m, i, j));
- printf ("\n");
- }
- printf ("]]\n");
- }
- /**
- Initialisation d'une matrice à partir d'une liste de valeur
- @param data liste de valeur
- @param nbline nombre de ligne
- @param nbcol nombre de colonne
- @return pointeur sur la matrice initialisée avec la liste de valeur
- Exemple de code
- @code
- double data[]={1.0,2.0,3.0,4.0,5.0,6.0};
- gsl_matrix *mat=matrix_init(data,2,3);
- matrix_display(mat);
- @endcode
- donne le résultat suivant
- @code
- 1.0 2.0 3.0
- 4.0 5.0 6.0
- @endcode
- */
- gsl_matrix *
- matrix_init (
- double *data,
- int nbline,
- int nbcol)
- {
- gsl_matrix *m = gsl_matrix_alloc (nbline, nbcol);
- a2ri_erreur_critique_si (m == NULL,
- "erreur allocation memoire pour m\nmatrix_init");
- for (int i = 0; i < nbline; i++)
- for (int j = 0; j < nbcol; j++)
- gsl_matrix_set (m, i, j, data[i * nbcol + j]);
- return m;
- }
- /**
- Calcul de la transposé d'une matrice
- @param m pointeur sur une matrice
- @return pointeur sur une matrice contenant la transposé \f$A^T\f$
- */
- gsl_matrix *
- matrix_transpose (
- const gsl_matrix * const m)
- {
- gsl_matrix *ret = gsl_matrix_alloc (m->size2, m->size1);
- a2ri_erreur_critique_si (ret == NULL,
- "erreur allocation memoire pour ret\nmatrix_transpose");
- for (size_t i = 0; i < m->size1; i++)
- for (size_t j = 0; j < m->size2; j++)
- gsl_matrix_set (ret, j, i, gsl_matrix_get (m, i, j));
- return ret;
- }
- /**
- Calcul de la matrice de covariance
- @param data_p premier ensemble de point
- @param data_x second ensemble de point
- @param nbpoint nombre de point
- @return la matrice de covariance
- */
- //calcul de la matrice covariance
- //voir Besl p243
- gsl_matrix *
- cross_variance (
- const point3d * const data_p,
- const point3d * const data_x,
- int nbpoint)
- {
- point3d *centerP,
- *centerX,
- tempp,
- tempx;
- gsl_matrix *somme,
- *up,
- *ux,
- *mul = NULL,
- *pi,
- *xi;
- double d[3];
- somme = gsl_matrix_calloc (3, 3);
- a2ri_erreur_critique_si (somme == NULL,
- "erreur allocation memoire pour somme\ncross_variance");
- centerP = center_of_mass (data_p, nbpoint);
- centerX = center_of_mass (data_x, nbpoint);
- d[0] = centerP->x;
- d[1] = centerP->y;
- d[2] = centerP->z;
- up = matrix_init (d, 3, 1);
- d[0] = centerX->x;
- d[1] = centerX->y;
- d[2] = centerX->z;
- ux = matrix_init (d, 1, 3);
- for (int i = 0; i < nbpoint; i++)
- {
- //p_i-up
- tempp.x = data_p[i].x - gsl_matrix_get (up, 0, 0);
- tempp.y = data_p[i].y - gsl_matrix_get (up, 1, 0);
- tempp.z = data_p[i].z - gsl_matrix_get (up, 2, 0);
- //p_x-ux
- tempx.x = data_x[i].x - gsl_matrix_get (ux, 0, 0);
- tempx.y = data_x[i].y - gsl_matrix_get (ux, 0, 1);
- tempx.z = data_x[i].z - gsl_matrix_get (ux, 0, 2);
- d[0] = tempp.x;
- d[1] = tempp.y;
- d[2] = tempp.z;
- pi = matrix_init (d, 3, 1);
- d[0] = tempx.x;
- d[1] = tempx.y;
- d[2] = tempx.z;
- xi = matrix_init (d, 1, 3);
- //(p_i-up)(p_x-ux)
- mul = matrix_mul (pi, xi);
- //somme
- somme = matrix_add (somme, mul);
- gsl_matrix_free (pi);
- gsl_matrix_free (xi);
- }
- for (size_t i = 0; i < somme->size1; i++)
- for (size_t j = 0; j < somme->size2; j++)
- gsl_matrix_set (somme, i, j,
- gsl_matrix_get (somme, i, j) / (nbpoint * 1.0));
- free (centerP);
- free (centerX);
- gsl_matrix_free (up);
- gsl_matrix_free (ux);
- gsl_matrix_free (mul);
- return somme;
- }
|