hkl/hkl-matrix.c

   1 /* This file is part of the hkl library.
   2  *
   3  * The hkl library is free software: you can redistribute it and/or modify
   4  * it under the terms of the GNU General Public License as published by
   5  * the Free Software Foundation, either version 3 of the License, or
   6  * (at your option) any later version.
   7  *
   8  * The hkl library is distributed in the hope that it will be useful,
   9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11  * GNU General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License
  14  * along with the hkl library.  If not, see <http://www.gnu.org/licenses/>.
  15  *
  16  * Copyright (C) 2003-2014 Synchrotron SOLEIL
  17  *                         L'Orme des Merisiers Saint-Aubin
  18  *                         BP 48 91192 GIF-sur-YVETTE CEDEX
  19  *
  20  * Authors: Picca Frédéric-Emmanuel <picca@synchrotron-soleil.fr>
  21  */
  22 #include <math.h>                       // for cos, fabs, atan2, sin, asin
  23 #include <stdio.h>                      // for fprintf, FILE
  24 #include <stdlib.h>                     // for free
  25 #include <string.h>                     // for memcpy
  26 #include "hkl-macros-private.h"         // for HKL_MALLOC
  27 #include "hkl-matrix-private.h"         // for _HklMatrix
  28 #include "hkl-vector-private.h"         // for HklVector, etc
  29 #include "hkl.h"                        // for HklMatrix, HKL_EPSILON, etc
  30
  31 /**
  32  * hkl_matrix_new: (skip)
  33  *
  34  * Returns: a new uninitialized HklMatrix
  35  */
  36 HklMatrix *hkl_matrix_new()
  37 {
  38         return HKL_MALLOC(HklMatrix);
  39 }
  40
  41 /**
  42  * hkl_matrix_new_full: (skip)
  43  * @m11: the matrix 11 value
  44  * @m12: the matrix 12 value
  45  * @m13: the matrix 13 value
  46  * @m21: the matrix 21 value
  47  * @m22: the matrix 22 value
  48  * @m23: the matrix 23 value
  49  * @m31: the matrix 31 value
  50  * @m32: the matrix 32 value
  51  * @m33: the matrix 33 value
  52  *
  53  * @todo test
  54  * Returns: a new HklMAtrix
  55  **/
  56 HklMatrix *hkl_matrix_new_full(double m11, double m12, double m13,
  57                                double m21, double m22, double m23,
  58                                double m31, double m32, double m33)
  59 {
  60         HklMatrix *self = hkl_matrix_new();
  61         hkl_matrix_init(self,
  62                         m11, m12, m13,
  63                         m21, m22, m23,
  64                         m31, m32, m33);
  65
  66         return self;
  67 }
  68
  69 /**
  70  * hkl_matrix_new_euler:
  71  * @euler_x: the eulerian value along X
  72  * @euler_y: the eulerian value along Y
  73  * @euler_z: the eulerian value along Z
  74  *
  75  * Returns: Create a rotation #HklMatrix from three eulerians angles.
  76  **/
  77 HklMatrix *hkl_matrix_new_euler(double euler_x, double euler_y, double euler_z)
  78 {
  79         HklMatrix *self = hkl_matrix_new();
  80         hkl_matrix_init_from_euler(self, euler_x, euler_y, euler_z);
  81
  82         return self;
  83 }
  84
  85 /**
  86  * hkl_matrix_dup: (skip)
  87  * @self:
  88  *
  89  *
  90  *
  91  * Returns:
  92  **/
  93 HklMatrix *hkl_matrix_dup(const HklMatrix* self)
  94 {
  95         HklMatrix *dup;
  96
  97         dup = HKL_MALLOC(HklMatrix);
  98         memcpy(dup, self, sizeof(*self));
  99
 100         return dup;
 101 }
 102
 103 /**
 104  * hkl_matrix_free: (skip)
 105  * @self:
 106  *
 107  *
 108  **/
 109 void hkl_matrix_free(HklMatrix *self)
 110 {
 111         free(self);
 112 }
 113
 114 /**
 115  * hkl_matrix_init:
 116  * @self: the #HklMatrix to initialize
 117  * @m11: the matrix 11 value
 118  * @m12: the matrix 12 value
 119  * @m13: the matrix 13 value
 120  * @m21: the matrix 21 value
 121  * @m22: the matrix 22 value
 122  * @m23: the matrix 23 value
 123  * @m31: the matrix 31 value
 124  * @m32: the matrix 32 value
 125  * @m33: the matrix 33 value
 126  *
 127  *
 128  **/
 129 void hkl_matrix_init(HklMatrix *self,
 130                      double m11, double m12, double m13,
 131                      double m21, double m22, double m23,
 132                      double m31, double m32, double m33)
 133 {
 134         double (*M)[3] = self->data;
 135
 136         M[0][0] = m11, M[0][1] = m12, M[0][2] = m13;
 137         M[1][0] = m21, M[1][1] = m22, M[1][2] = m23;
 138         M[2][0] = m31, M[2][1] = m32, M[2][2] = m33;
 139 }
 140
 141 /**
 142  * hkl_matrix_matrix_set: (skip)
 143  * @self: the this ptr
 144  * @m: the matrix to set
 145  *
 146  * @todo test
 147  **/
 148 void hkl_matrix_matrix_set(HklMatrix *self, const HklMatrix *m)
 149 {
 150         if (self == m)
 151                 return;
 152
 153         memcpy(self->data, m->data, sizeof(double) * 9);
 154 }
 155
 156 /**
 157  * hkl_matrix_get:
 158  * @self: the this ptr
 159  * @i: the i coordinate
 160  * @j: the j coordinate
 161  *
 162  * @todo test
 163  * Return value: the Mij value
 164  **/
 165 double hkl_matrix_get(const HklMatrix *self, unsigned int i, unsigned int j)
 166 {
 167         return self->data[i][j];
 168 }
 169
 170 /**
 171  * hkl_matrix_fprintf:
 172  * @file: the FILE stream
 173  * @self: the #HklMatrix to print into the file stream
 174  *
 175  * printf an #HklMatrix into a FILE stream.
 176  **/
 177 void hkl_matrix_fprintf(FILE *file, const HklMatrix *self)
 178 {
 179         double const (*M)[3] = self->data;
 180
 181         fprintf(file, "|%f, %f, %f|\n", M[0][0], M[0][1], M[0][2]);
 182         fprintf(file, "|%f, %f, %f|\n", M[1][0], M[1][1], M[1][2]);
 183         fprintf(file, "|%f, %f, %f|\n", M[2][0], M[2][1], M[2][2]);
 184 }
 185
 186 /**
 187  * hkl_matrix_init_from_two_vector:
 188  * @self: The #HklMatrix to initialize
 189  * @v1: the first #HklVector
 190  * @v2: the second #HklVector
 191  *
 192  * Create an #HklMatrix which represent a direct oriented base of the space
 193  * the first row correspond to the |v1|, the second row |v2| and the last one
 194  * is |v1 ^ v2|
 195  **/
 196 void hkl_matrix_init_from_two_vector(HklMatrix *self,
 197                                      const HklVector *v1, const HklVector *v2)
 198 {
 199         HklVector x, y, z;
 200         double (*M)[3] = self->data;
 201
 202         x = *v1;
 203         hkl_vector_normalize(&x);
 204
 205         z = *v1;
 206         hkl_vector_vectorial_product(&z, v2);
 207         hkl_vector_normalize(&z);
 208
 209         y = z;
 210         hkl_vector_vectorial_product(&y, &x);
 211
 212         M[0][0] = x.data[0], M[0][1] = y.data[0], M[0][2] = z.data[0];
 213         M[1][0] = x.data[1], M[1][1] = y.data[1], M[1][2] = z.data[1];
 214         M[2][0] = x.data[2], M[2][1] = y.data[2], M[2][2] = z.data[2];
 215 }
 216
 217 /**
 218  * hkl_matrix_init_from_euler:
 219  * @self: the #HklMatrix to initialize
 220  * @euler_x: the eulerian value along X
 221  * @euler_y: the eulerian value along Y
 222  * @euler_z: the eulerian value along Z
 223  *
 224  * Create a rotation #HklMatrix from three eulerians angles.
 225  **/
 226 void hkl_matrix_init_from_euler(HklMatrix *self,
 227                                 double euler_x, double euler_y, double euler_z)
 228 {
 229         double (*M)[3] = self->data;
 230
 231         double A = cos(euler_x);
 232         double B = sin(euler_x);
 233         double C = cos(euler_y);
 234         double D = sin(euler_y);
 235         double E = cos(euler_z);
 236         double F = sin(euler_z);
 237         double AD = A *D;
 238         double BD = B *D;
 239
 240         M[0][0] = C*E;
 241         M[0][1] =-C*F;
 242         M[0][2] = D;
 243         M[1][0] = BD *E + A *F;
 244         M[1][1] =-BD *F + A *E;
 245         M[1][2] =-B *C;
 246         M[2][0] =-AD *E + B *F;
 247         M[2][1] = AD *F + B *E;
 248         M[2][2] = A *C;
 249 }
 250
 251 /**
 252  * hkl_matrix_to_euler:
 253  * @self: the rotation #HklMatrix use to compute the eulerians angles
 254  * @euler_x: the eulerian value along X
 255  * @euler_y: the eulerian value along Y
 256  * @euler_z: the eulerian value along Z
 257  *
 258  * compute the three eulerians values for a given rotation #HklMatrix
 259  **/
 260 void hkl_matrix_to_euler(const HklMatrix *self,
 261                          double *euler_x, double *euler_y, double *euler_z)
 262 {
 263         double tx, ty;
 264         double C;
 265         double const (*M)[3] = self->data;
 266
 267         *euler_y = asin( self->data[0][2] );      /*Calculate Y-axis angle */
 268         C = cos( *euler_y );
 269         if (fabs(C) > HKL_EPSILON) {
 270                 /*Gimball lock? */
 271                 tx       =  M[2][2] / C; /*No, so get X-axis angle */
 272                 ty       = -M[1][2] / C;
 273                 *euler_x = atan2( ty, tx );
 274                 tx       =  M[0][0] / C; /*Get Z-axis angle */
 275                 ty       = -M[0][1] / C;
 276                 *euler_z = atan2( ty, tx );
 277         } else {
 278                 /*Gimball lock has occurred */
 279                 *euler_x = 0.;              /*Set X-axis angle to zero */
 280                 tx       =  M[1][1];    /*And calculate Z-axis angle */
 281                 ty       =  M[1][0];
 282                 *euler_z = atan2( ty, tx );
 283         }
 284 }
 285
 286 /**
 287  * hkl_matrix_cmp:
 288  * @self: the first #HklMatrix
 289  * @m: the #HklMatrix to compare with
 290  *
 291  * compare two #HklMatrix.
 292  *
 293  * Returns: return TRUE if | self - m | > HKL_EPSILON
 294  **/
 295 int hkl_matrix_cmp(const HklMatrix *self, const HklMatrix *m)
 296 {
 297         unsigned int i;
 298         unsigned int j;
 299         for(i=0;i<3;i++)
 300                 for(j=0;j<3;j++)
 301                         if( fabs(self->data[i][j] - m->data[i][j]) > HKL_EPSILON )
 302                                 return FALSE;
 303         return TRUE;
 304 }
 305
 306
 307 /**
 308  * hkl_matrix_times_matrix:
 309  * @self: the #HklMatrix to modify
 310  * @m: the #HklMatrix to multiply by
 311  *
 312  * compute the matrix multiplication self = self * m
 313  **/
 314 void hkl_matrix_times_matrix(HklMatrix *self, const HklMatrix *m)
 315 {
 316         HklMatrix const tmp = *self;
 317         double (*M)[3] = self->data;
 318         double const (*Tmp)[3] = tmp.data;
 319         double const (*M1)[3];
 320         if (self == m)
 321                 M1 = tmp.data;
 322         else
 323                 M1 = m->data;
 324
 325         M[0][0] = Tmp[0][0]*M1[0][0] + Tmp[0][1]*M1[1][0] + Tmp[0][2]*M1[2][0];
 326         M[0][1] = Tmp[0][0]*M1[0][1] + Tmp[0][1]*M1[1][1] + Tmp[0][2]*M1[2][1];
 327         M[0][2] = Tmp[0][0]*M1[0][2] + Tmp[0][1]*M1[1][2] + Tmp[0][2]*M1[2][2];
 328
 329         M[1][0] = Tmp[1][0]*M1[0][0] + Tmp[1][1]*M1[1][0] + Tmp[1][2]*M1[2][0];
 330         M[1][1] = Tmp[1][0]*M1[0][1] + Tmp[1][1]*M1[1][1] + Tmp[1][2]*M1[2][1];
 331         M[1][2] = Tmp[1][0]*M1[0][2] + Tmp[1][1]*M1[1][2] + Tmp[1][2]*M1[2][2];
 332
 333         M[2][0] = Tmp[2][0]*M1[0][0] + Tmp[2][1]*M1[1][0] + Tmp[2][2]*M1[2][0];
 334         M[2][1] = Tmp[2][0]*M1[0][1] + Tmp[2][1]*M1[1][1] + Tmp[2][2]*M1[2][1];
 335         M[2][2] = Tmp[2][0]*M1[0][2] + Tmp[2][1]*M1[1][2] + Tmp[2][2]*M1[2][2];
 336 }
 337
 338
 339 /**
 340  * hkl_matrix_times_vector:
 341  * @self: the #HklMatrix use to multiply the #HklVector
 342  * @v: the #HklVector multiply by the #HklMatrix
 343  *
 344  * multiply an #HklVector by an #HklMatrix
 345  **/
 346 void hkl_matrix_times_vector(const HklMatrix *self, HklVector *v)
 347 {
 348         HklVector tmp;
 349         double *Tmp;
 350         double *V = v->data;
 351         double const (*M)[3] = self->data;
 352
 353         tmp = *v;
 354         Tmp = tmp.data;
 355
 356         V[0] = Tmp[0]*M[0][0] + Tmp[1]*M[0][1] + Tmp[2]*M[0][2];
 357         V[1] = Tmp[0]*M[1][0] + Tmp[1]*M[1][1] + Tmp[2]*M[1][2];
 358         V[2] = Tmp[0]*M[2][0] + Tmp[1]*M[2][1] + Tmp[2]*M[2][2];
 359 }
 360
 361
 362 /**
 363  * hkl_matrix_transpose:
 364  * @self: the #HklMatrix to transpose
 365  *
 366  * transpose an #HklMatrix
 367  **/
 368 void hkl_matrix_transpose(HklMatrix *self)
 369 {
 370 #define SWAP(a, b) {double tmp=a; a=b; b=tmp;}
 371         SWAP(self->data[1][0], self->data[0][1]);
 372         SWAP(self->data[2][0], self->data[0][2]);
 373         SWAP(self->data[2][1], self->data[1][2]);
 374 }
 375
 376 /**
 377  * hkl_matrix_det:
 378  * @self: the #HklMatrix use to compute the determinant
 379  *
 380  * compute the determinant of an #HklMatrix
 381  *
 382  * Returns: the determinant of the self #HklMatrix
 383  * Todo: test
 384  **/
 385 double hkl_matrix_det(const HklMatrix *self)
 386 {
 387         double det;
 388         double const (*M)[3] = self->data;
 389
 390         det  =  M[0][0] * (M[1][1] * M[2][2] - M[2][1] * M[1][2]);
 391         det += -M[0][1] * (M[1][0] * M[2][2] - M[2][0] * M[1][2]);
 392         det +=  M[0][2] * (M[1][0] * M[2][1] - M[2][0] * M[1][1]);
 393
 394         return det;
 395 }
 396
 397 /**
 398  * hkl_matrix_solve:
 399  * @self: The #HklMatrix of the system
 400  * @x: the #HklVector to compute.
 401  * @b: the #hklVector of the system to solve.
 402  *
 403  * solve the system self . X = b
 404  *
 405  * Returns: -1 if the système has no solution, 0 otherwise.
 406  * Todo: test
 407  **/
 408 int hkl_matrix_solve(const HklMatrix *self, HklVector *x, const HklVector *b)
 409 {
 410         double det;
 411         double const (*M)[3] = self->data;
 412         double *X = x->data;
 413         double const *B = b->data;
 414
 415         det = hkl_matrix_det(self);
 416         if (fabs(det) < HKL_EPSILON)
 417                 return -1;
 418         else {
 419                 X[0] =   B[0] * (M[1][1]*M[2][2] - M[1][2]*M[2][1]);
 420                 X[0] += -B[1] * (M[0][1]*M[2][2] - M[0][2]*M[2][1]);
 421                 X[0] +=  B[2] * (M[0][1]*M[1][2] - M[0][2]*M[1][1]);
 422
 423                 X[1] =  -B[0] * (M[1][0]*M[2][2] - M[1][2]*M[2][0]);
 424                 X[1] +=  B[1] * (M[0][0]*M[2][2] - M[0][2]*M[2][0]);
 425                 X[1] += -B[2] * (M[0][0]*M[1][2] - M[0][2]*M[1][0]);
 426
 427                 X[2] =   B[0] * (M[1][0]*M[2][1] - M[1][1]*M[2][0]);
 428                 X[2] += -B[1] * (M[0][0]*M[2][1] - M[0][1]*M[2][0]);
 429                 X[2] +=  B[2] * (M[0][0]*M[1][1] - M[0][1]*M[1][0]);
 430
 431                 hkl_vector_div_double(x, det);
 432         }
 433         return 0;
 434 }
 435
 436 /**
 437  * hkl_matrix_is_null:
 438  * @self: the #HklMatrix to test
 439  *
 440  * is all #hklMatrix elementes bellow #HKL_EPSILON
 441  *
 442  * Returns: TRUE if the self #HklMatrix is null
 443  * Todo: test
 444  **/
 445 int hkl_matrix_is_null(const HklMatrix *self)
 446 {
 447         unsigned int i;
 448         unsigned int j;
 449         for (i=0;i<3;i++)
 450                 for (j=0;j<3;j++)
 451                         if ( fabs(self->data[i][j]) > HKL_EPSILON )
 452                                 return FALSE;
 453         return TRUE;
 454 }