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