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-2010 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 <stdlib.h>
  23 #include <math.h>
  24 #include <string.h>
  25
  26 #include <hkl/hkl-macros.h>
  27 #include <hkl/hkl-matrix.h>
  28 #include <hkl/hkl-vector.h>
  29
  30 /**
  31  * hkl_matrix_dup: (skip)
  32  * @self:
  33  *
  34  *
  35  *
  36  * Returns:
  37  **/
  38 HklMatrix *hkl_matrix_dup(const HklMatrix* self)
  39 {
  40         HklMatrix *dup;
  41
  42         dup = HKL_MALLOC(HklMatrix);
  43         memcpy(dup, self, sizeof(*self));
  44
  45         return dup;
  46 }
  47
  48 /**
  49  * hkl_matrix_free: (skip)
  50  * @self:
  51  *
  52  *
  53  **/
  54 void hkl_matrix_free(HklMatrix *self)
  55 {
  56         if(!self)
  57                 return;
  58
  59         free(self);
  60 }
  61
  62 /**
  63  * hkl_matrix_init:
  64  * @self: the #HklMatrix to initialize
  65  * @m11: the matrix 11 value
  66  * @m12: the matrix 12 value
  67  * @m13: the matrix 13 value
  68  * @m21: the matrix 21 value
  69  * @m22: the matrix 22 value
  70  * @m23: the matrix 23 value
  71  * @m31: the matrix 31 value
  72  * @m32: the matrix 32 value
  73  * @m33: the matrix 33 value
  74  *
  75  *
  76  **/
  77 void hkl_matrix_init(HklMatrix *self,
  78                      double m11, double m12, double m13,
  79                      double m21, double m22, double m23,
  80                      double m31, double m32, double m33)
  81 {
  82         double (*M)[3] = self->data;
  83
  84         M[0][0] = m11, M[0][1] = m12, M[0][2] = m13;
  85         M[1][0] = m21, M[1][1] = m22, M[1][2] = m23;
  86         M[2][0] = m31, M[2][1] = m32, M[2][2] = m33;
  87 }
  88
  89 /**
  90  * hkl_matrix_fprintf:
  91  * @file: the FILE stream
  92  * @self: the #HklMatrix to print into the file stream
  93  *
  94  * printf an #HklMatrix into a FILE stream.
  95  **/
  96 void hkl_matrix_fprintf(FILE *file, const HklMatrix *self)
  97 {
  98         double const (*M)[3] = self->data;
  99
 100         fprintf(file, "|%f, %f, %f|\n", M[0][0], M[0][1], M[0][2]);
 101         fprintf(file, "|%f, %f, %f|\n", M[1][0], M[1][1], M[1][2]);
 102         fprintf(file, "|%f, %f, %f|\n", M[2][0], M[2][1], M[2][2]);
 103 }
 104
 105 /**
 106  * hkl_matrix_init_from_two_vector:
 107  * @self: The #HklMatrix to initialize
 108  * @v1: the first #HklVector
 109  * @v2: the second #HklVector
 110  *
 111  * Create an #HklMatrix which represent a direct oriented base of the space
 112  * the first row correspond to the |v1|, the second row |v2| and the last one
 113  * is |v1 ^ v2|
 114  **/
 115 void hkl_matrix_init_from_two_vector(HklMatrix *self,
 116                                      const HklVector *v1, const HklVector *v2)
 117 {
 118         HklVector x, y, z;
 119         double (*M)[3] = self->data;
 120
 121         x = *v1;
 122         hkl_vector_normalize(&x);
 123
 124         z = *v1;
 125         hkl_vector_vectorial_product(&z, v2);
 126         hkl_vector_normalize(&z);
 127
 128         y = z;
 129         hkl_vector_vectorial_product(&y, &x);
 130
 131         M[0][0] = x.data[0], M[0][1] = y.data[0], M[0][2] = z.data[0];
 132         M[1][0] = x.data[1], M[1][1] = y.data[1], M[1][2] = z.data[1];
 133         M[2][0] = x.data[2], M[2][1] = y.data[2], M[2][2] = z.data[2];
 134 }
 135
 136 /**
 137  * hkl_matrix_init_from_euler:
 138  * @self: the #HklMatrix to initialize
 139  * @euler_x: the eulerian value along X
 140  * @euler_y: the eulerian value along Y
 141  * @euler_z: the eulerian value along Z
 142  *
 143  * Create a rotation #HklMatrix from three eulerians angles.
 144  **/
 145 void hkl_matrix_init_from_euler(HklMatrix *self,
 146                                 double euler_x, double euler_y, double euler_z)
 147 {
 148         double (*M)[3] = self->data;
 149
 150         double A = cos(euler_x);
 151         double B = sin(euler_x);
 152         double C = cos(euler_y);
 153         double D = sin(euler_y);
 154         double E = cos(euler_z);
 155         double F = sin(euler_z);
 156         double AD = A *D;
 157         double BD = B *D;
 158
 159         M[0][0] = C*E;
 160         M[0][1] =-C*F;
 161         M[0][2] = D;
 162         M[1][0] = BD *E + A *F;
 163         M[1][1] =-BD *F + A *E;
 164         M[1][2] =-B *C;
 165         M[2][0] =-AD *E + B *F;
 166         M[2][1] = AD *F + B *E;
 167         M[2][2] = A *C;
 168 }
 169
 170 /**
 171  * hkl_matrix_to_euler:
 172  * @self: the rotation #HklMatrix use to compute the eulerians angles
 173  * @euler_x: the eulerian value along X
 174  * @euler_y: the eulerian value along Y
 175  * @euler_z: the eulerian value along Z
 176  *
 177  * compute the three eulerians values for a given rotation #HklMatrix
 178  **/
 179 void hkl_matrix_to_euler(const HklMatrix *self,
 180                          double *euler_x, double *euler_y, double *euler_z)
 181 {
 182         double tx, ty;
 183         double C;
 184         double const (*M)[3] = self->data;
 185
 186         *euler_y = asin( self->data[0][2] );      /*Calculate Y-axis angle */
 187         C = cos( *euler_y );
 188         if (fabs(C) > HKL_EPSILON) {
 189                 /*Gimball lock? */
 190                 tx       =  M[2][2] / C; /*No, so get X-axis angle */
 191                 ty       = -M[1][2] / C;
 192                 *euler_x = atan2( ty, tx );
 193                 tx       =  M[0][0] / C; /*Get Z-axis angle */
 194                 ty       = -M[0][1] / C;
 195                 *euler_z = atan2( ty, tx );
 196         } else {
 197                 /*Gimball lock has occurred */
 198                 *euler_x = 0.;              /*Set X-axis angle to zero */
 199                 tx       =  M[1][1];    /*And calculate Z-axis angle */
 200                 ty       =  M[1][0];
 201                 *euler_z = atan2( ty, tx );
 202         }
 203 }
 204
 205 /**
 206  * hkl_matrix_cmp:
 207  * @self: the first #HklMatrix
 208  * @m: the #HklMatrix to compare with
 209  *
 210  * compare two #HklMatrix.
 211  *
 212  * Returns: return HKL_TRUE if | self - m | > HKL_EPSILON
 213  **/
 214 int hkl_matrix_cmp(const HklMatrix *self, const HklMatrix *m)
 215 {
 216         unsigned int i;
 217         unsigned int j;
 218         for(i=0;i<3;i++)
 219                 for(j=0;j<3;j++)
 220                         if( fabs(self->data[i][j] - m->data[i][j]) > HKL_EPSILON )
 221                                 return HKL_FALSE;
 222         return HKL_TRUE;
 223 }
 224
 225
 226 /**
 227  * hkl_matrix_times_matrix:
 228  * @self: the #HklMatrix to modify
 229  * @m: the #HklMatrix to multiply by
 230  *
 231  * compute the matrix multiplication self = self * m
 232  **/
 233 void hkl_matrix_times_matrix(HklMatrix *self, const HklMatrix *m)
 234 {
 235         HklMatrix const tmp = *self;
 236         double (*M)[3] = self->data;
 237         double const (*Tmp)[3] = tmp.data;
 238         double const (*M1)[3];
 239         if (self == m)
 240                 M1 = tmp.data;
 241         else
 242                 M1 = m->data;
 243
 244         M[0][0] = Tmp[0][0]*M1[0][0] + Tmp[0][1]*M1[1][0] + Tmp[0][2]*M1[2][0];
 245         M[0][1] = Tmp[0][0]*M1[0][1] + Tmp[0][1]*M1[1][1] + Tmp[0][2]*M1[2][1];
 246         M[0][2] = Tmp[0][0]*M1[0][2] + Tmp[0][1]*M1[1][2] + Tmp[0][2]*M1[2][2];
 247
 248         M[1][0] = Tmp[1][0]*M1[0][0] + Tmp[1][1]*M1[1][0] + Tmp[1][2]*M1[2][0];
 249         M[1][1] = Tmp[1][0]*M1[0][1] + Tmp[1][1]*M1[1][1] + Tmp[1][2]*M1[2][1];
 250         M[1][2] = Tmp[1][0]*M1[0][2] + Tmp[1][1]*M1[1][2] + Tmp[1][2]*M1[2][2];
 251
 252         M[2][0] = Tmp[2][0]*M1[0][0] + Tmp[2][1]*M1[1][0] + Tmp[2][2]*M1[2][0];
 253         M[2][1] = Tmp[2][0]*M1[0][1] + Tmp[2][1]*M1[1][1] + Tmp[2][2]*M1[2][1];
 254         M[2][2] = Tmp[2][0]*M1[0][2] + Tmp[2][1]*M1[1][2] + Tmp[2][2]*M1[2][2];
 255 }
 256
 257
 258 /**
 259  * hkl_matrix_times_vector:
 260  * @self: the #HklMatrix use to multiply the #HklVector
 261  * @v: the #HklVector multiply by the #HklMatrix
 262  *
 263  * multiply an #HklVector by an #HklMatrix
 264  **/
 265 void hkl_matrix_times_vector(const HklMatrix *self, HklVector *v)
 266 {
 267         HklVector tmp;
 268         double *Tmp;
 269         double *V = v->data;
 270         double const (*M)[3] = self->data;
 271
 272         tmp = *v;
 273         Tmp = tmp.data;
 274
 275         V[0] = Tmp[0]*M[0][0] + Tmp[1]*M[0][1] + Tmp[2]*M[0][2];
 276         V[1] = Tmp[0]*M[1][0] + Tmp[1]*M[1][1] + Tmp[2]*M[1][2];
 277         V[2] = Tmp[0]*M[2][0] + Tmp[1]*M[2][1] + Tmp[2]*M[2][2];
 278 }
 279
 280
 281 /**
 282  * hkl_matrix_transpose:
 283  * @self: the #HklMatrix to transpose
 284  *
 285  * transpose an #HklMatrix
 286  **/
 287 void hkl_matrix_transpose(HklMatrix *self)
 288 {
 289 #define SWAP(a, b) {double tmp=a; a=b; b=tmp;}
 290         SWAP(self->data[1][0], self->data[0][1]);
 291         SWAP(self->data[2][0], self->data[0][2]);
 292         SWAP(self->data[2][1], self->data[1][2]);
 293 }
 294
 295 /**
 296  * hkl_matrix_det:
 297  * @self: the #HklMatrix use to compute the determinant
 298  *
 299  * compute the determinant of an #HklMatrix
 300  *
 301  * Returns: the determinant of the self #HklMatrix
 302  * Todo: test
 303  **/
 304 double hkl_matrix_det(const HklMatrix *self)
 305 {
 306         double det;
 307         double const (*M)[3] = self->data;
 308
 309         det  =  M[0][0] * (M[1][1] * M[2][2] - M[2][1] * M[1][2]);
 310         det += -M[0][1] * (M[1][0] * M[2][2] - M[2][0] * M[1][2]);
 311         det +=  M[0][2] * (M[1][0] * M[2][1] - M[2][0] * M[1][1]);
 312
 313         return det;
 314 }
 315
 316 /**
 317  * hkl_matrix_solve:
 318  * @self: The #HklMatrix of the system
 319  * @x: the #HklVector to compute.
 320  * @b: the #hklVector of the system to solve.
 321  *
 322  * solve the system self . X = b
 323  *
 324  * Returns: -1 if the système has no solution, 0 otherwise.
 325  * Todo: test
 326  **/
 327 int hkl_matrix_solve(const HklMatrix *self, HklVector *x, const HklVector *b)
 328 {
 329         double det;
 330         double const (*M)[3] = self->data;
 331         double *X = x->data;
 332         double const *B = b->data;
 333
 334         det = hkl_matrix_det(self);
 335         if (fabs(det) < HKL_EPSILON)
 336                 return -1;
 337         else {
 338                 X[0] =   B[0] * (M[1][1]*M[2][2] - M[1][2]*M[2][1]);
 339                 X[0] += -B[1] * (M[0][1]*M[2][2] - M[0][2]*M[2][1]);
 340                 X[0] +=  B[2] * (M[0][1]*M[1][2] - M[0][2]*M[1][1]);
 341
 342                 X[1] =  -B[0] * (M[1][0]*M[2][2] - M[1][2]*M[2][0]);
 343                 X[1] +=  B[1] * (M[0][0]*M[2][2] - M[0][2]*M[2][0]);
 344                 X[1] += -B[2] * (M[0][0]*M[1][2] - M[0][2]*M[1][0]);
 345
 346                 X[2] =   B[0] * (M[1][0]*M[2][1] - M[1][1]*M[2][0]);
 347                 X[2] += -B[1] * (M[0][0]*M[2][1] - M[0][1]*M[2][0]);
 348                 X[2] +=  B[2] * (M[0][0]*M[1][1] - M[0][1]*M[1][0]);
 349
 350                 hkl_vector_div_double(x, det);
 351         }
 352         return 0;
 353 }
 354
 355 /**
 356  * hkl_matrix_is_null:
 357  * @self: the #HklMatrix to test
 358  *
 359  * is all #hklMatrix elementes bellow #HKL_EPSILON
 360  *
 361  * Returns: HKL_TRUE if the self #HklMatrix is null
 362  * Todo: test
 363  **/
 364 int hkl_matrix_is_null(const HklMatrix *self)
 365 {
 366         unsigned int i;
 367         unsigned int j;
 368         for (i=0;i<3;i++)
 369                 for (j=0;j<3;j++)
 370                         if ( fabs(self->data[i][j]) > HKL_EPSILON )
 371                                 return HKL_FALSE;
 372         return HKL_TRUE;
 373 }