hkl/hkl-quaternion.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-vector.h>
  28 #include <hkl/hkl-matrix.h>
  29 #include <hkl/hkl-quaternion.h>
  30
  31 /* public */
  32
  33 /**
  34  * hkl_quaternion_init:
  35  * @self: the #HklQuaternion to initialize
  36  * @a: the 1st element value
  37  * @b: the 2nd element value
  38  * @c: the 3rd element value
  39  * @d: the 4th element value
  40  *
  41  * initialize the four elements of an #HklQuaternion
  42  **/
  43 void hkl_quaternion_init(HklQuaternion *self,
  44                          double a, double b, double c, double d)
  45 {
  46         self->data[0] = a;
  47         self->data[1] = b;
  48         self->data[2] = c;
  49         self->data[3] = d;
  50 }
  51
  52 /**
  53  * hkl_quaternion_fprintf:
  54  * @file: the file to send the #HklQuaternion into
  55  * @self: the #HklQuaternion to write into the file stream.
  56  *
  57  *  print an #HklQuaternion into a FILE stream
  58  **/
  59 void hkl_quaternion_fprintf(FILE *file, HklQuaternion const *self)
  60 {
  61         double const *Q;
  62
  63         Q = self->data;
  64         fprintf(file, "<%f, %f, %f, %f>", Q[0], Q[1], Q[2], Q[3]);
  65 }
  66
  67 /**
  68  * hkl_quaternion_init_from_vector:
  69  * @self: the #HklQuaternion to set
  70  * @v: the #HklVector used to set the self #HklQuaternion
  71  *
  72  * initialize an #HklQuaternion from an #HklVector
  73  **/
  74 void hkl_quaternion_init_from_vector(HklQuaternion *self, HklVector const *v)
  75 {
  76         self->data[0] = 0;
  77         memcpy(&self->data[1], &v->data[0], sizeof(v->data));
  78 }
  79
  80 /**
  81  * hkl_quaternion_init_from_angle_and_axe:
  82  * @self: the #HklQuaternion to set
  83  * @angle: the angles of the rotation
  84  * @v: the axe of rotation
  85  *
  86  * initialize an #HklQuaternion from a vector and a angle.
  87  **/
  88 inline void hkl_quaternion_init_from_angle_and_axe(HklQuaternion *self,
  89                                                    double angle, HklVector const *v)
  90 {
  91         double norm;
  92         double c;
  93         double s;
  94
  95         // check that parameters are ok.
  96         norm = hkl_vector_norm2(v);
  97
  98         c = cos(angle / 2.);
  99         s = sin(angle / 2.) / norm;
 100
 101         self->data[0] = c;
 102         self->data[1] = s * v->data[0];
 103         self->data[2] = s * v->data[1];
 104         self->data[3] = s * v->data[2];
 105 }
 106
 107 /**
 108  * hkl_quaternion_cmp:
 109  * @self: the first #HklQuaternion
 110  * @q: the second #HklQuaternion
 111  *
 112  * compare two #HklQuaternion.
 113  *
 114  * Returns: #HKL_TRUE if both are equal, #HKL_FAIL otherwise.
 115  **/
 116 int hkl_quaternion_cmp(HklQuaternion const *self, HklQuaternion const *q)
 117 {
 118         unsigned int i;
 119
 120         for (i=0;i<4;i++)
 121                 if ( fabs(self->data[i] - q->data[i]) > HKL_EPSILON )
 122                         return HKL_FALSE;
 123         return HKL_TRUE;
 124 }
 125
 126 /**
 127  * hkl_quaternion_minus_quaternion:
 128  * @self: the #HklQuaternion to modify.
 129  * @q: the #HklQuaternion to substract
 130  *
 131  * substract two #HklQuaternions
 132  * Todo: test
 133  **/
 134 void hkl_quaternion_minus_quaternion(HklQuaternion *self, const HklQuaternion *q)
 135 {
 136         unsigned int i;
 137
 138         for (i=0;i<4;i++)
 139                 self->data[i] -= q->data[i];
 140 }
 141
 142 /**
 143  * hkl_quaternion_times_quaternion:
 144  * @self: the #HklQuaternion to modify
 145  * @q: the #HklQuaternion to multiply by
 146  *
 147  * multiply two quaternions
 148  **/
 149 void hkl_quaternion_times_quaternion(HklQuaternion *self, const HklQuaternion *q)
 150 {
 151         HklQuaternion Tmp;
 152         double *Q;
 153
 154         Tmp = *self;
 155         Q = Tmp.data;
 156         if (self == q){
 157                 self->data[0] = Q[0]*Q[0] - Q[1]*Q[1] - Q[2]*Q[2] - Q[3]*Q[3];
 158                 self->data[1] = Q[0]*Q[1] + Q[1]*Q[0] + Q[2]*Q[3] - Q[3]*Q[2];
 159                 self->data[2] = Q[0]*Q[2] - Q[1]*Q[3] + Q[2]*Q[0] + Q[3]*Q[1];
 160                 self->data[3] = Q[0]*Q[3] + Q[1]*Q[2] - Q[2]*Q[1] + Q[3]*Q[0];
 161         }else{
 162                 double const *Q1 = q->data;
 163                 self->data[0] = Q[0]*Q1[0] - Q[1]*Q1[1] - Q[2]*Q1[2] - Q[3]*Q1[3];
 164                 self->data[1] = Q[0]*Q1[1] + Q[1]*Q1[0] + Q[2]*Q1[3] - Q[3]*Q1[2];
 165                 self->data[2] = Q[0]*Q1[2] - Q[1]*Q1[3] + Q[2]*Q1[0] + Q[3]*Q1[1];
 166                 self->data[3] = Q[0]*Q1[3] + Q[1]*Q1[2] - Q[2]*Q1[1] + Q[3]*Q1[0];
 167         }
 168 }
 169
 170 /**
 171  * hkl_quaternion_norm2:
 172  * @self: the quaternion use to compute the norm
 173  *
 174  * compute the norm2 of an #HklQuaternion
 175  *
 176  * Returns: the self #hklquaternion norm
 177  **/
 178 double hkl_quaternion_norm2(const HklQuaternion *self)
 179 {
 180         double sum2 = 0;
 181         unsigned int i;
 182         for (i=0;i<4;i++)
 183                 sum2 += self->data[i] *self->data[i];
 184         return sqrt(sum2);
 185 }
 186
 187 /**
 188  * hkl_quaternion_conjugate:
 189  * @self: the #HklQuaternion to conjugate
 190  *
 191  * compute the conjugate of a quaternion
 192  **/
 193 void hkl_quaternion_conjugate(HklQuaternion *self)
 194 {
 195         unsigned int i;
 196         for (i=1;i<4;i++)
 197                 self->data[i] = -self->data[i];
 198 }
 199
 200 /**
 201  * hkl_quaternion_to_matrix:
 202  * @self: the #HklQuaternion use to compute the #HklMatrix
 203  * @m: the #HklMatrix return.
 204  *
 205  * Compute the rotation matrix of a Quaternion.
 206  *
 207  * compute the rotation matrix corresponding to the unitary quaternion.
 208  * \f$ q = a + b \cdot i + c \cdot j + d \cdot k \f$
 209  *
 210  * \f$
 211  * \left(
 212  *  \begin{array}{ccc}
 213  *    a^2+b^2-c^2-d^2 & 2bc-2ad         & 2ac+2bd\\
 214  *    2ad+2bc         & a^2-b^2+c^2-d^2 & 2cd-2ab\\
 215  *    2bd-2ac         & 2ab+2cd         & a^2-b^2-c^2+d^2
 216  *  \end{array}
 217  * \right)
 218  * \f$
 219  * Todo: optimize
 220  */
 221 void hkl_quaternion_to_matrix(const HklQuaternion *self, HklMatrix *m)
 222 {
 223         double const *Q;
 224
 225         // check that parameters are ok.
 226         hkl_assert(fabs(hkl_quaternion_norm2(self) - 1) < HKL_EPSILON);
 227
 228         Q = self->data;
 229
 230         m->data[0][0] = Q[0]*Q[0] + Q[1]*Q[1] - Q[2]*Q[2] - Q[3]*Q[3];
 231         m->data[0][1] = 2 * (Q[1]*Q[2] - Q[0]*Q[3]);
 232         m->data[0][2] = 2 * (Q[0]*Q[2] + Q[1]*Q[3]);
 233
 234         m->data[1][0] = 2 * (Q[0]*Q[3] + Q[1]*Q[2]);
 235         m->data[1][1] = Q[0]*Q[0] - Q[1]*Q[1] + Q[2]*Q[2] - Q[3]*Q[3];
 236         m->data[1][2] = 2 * (Q[2]*Q[3] - Q[0]*Q[1]);
 237
 238         m->data[2][0] = 2 * (Q[1]*Q[3] - Q[0]*Q[2]);
 239         m->data[2][1] = 2 * (Q[0]*Q[1] + Q[2]*Q[3]);
 240         m->data[2][2] = Q[0]*Q[0] - Q[1]*Q[1] - Q[2]*Q[2] + Q[3]*Q[3];
 241 }
 242
 243 /**
 244  * hkl_quaternion_to_angle_and_axe:
 245  * @self: The #HklQuaternion use to compute the angle and the roation axis.
 246  * @angle: the returned angle of the rotation.
 247  * @v: the returned axis of the rotation.
 248  *
 249  * compute the axe and angle of the unitary quaternion angle [-pi, pi]
 250  * if q is the (1, 0, 0, 0) quaternion return the (0,0,0) axe and a 0 angle
 251  */
 252 void hkl_quaternion_to_angle_and_axe(HklQuaternion const *self,
 253                                      double *angle, HklVector *v)
 254 {
 255         double angle_2;
 256         double cos_angle_2;
 257         double sin_angle_2;
 258
 259         // check that parameters are ok. (norm must be equal to 1)
 260         hkl_assert(fabs(hkl_quaternion_norm2(self) - 1) < HKL_EPSILON);
 261
 262         // compute the angle
 263         cos_angle_2 = self->data[0];
 264         angle_2 = acos(cos_angle_2);
 265         *angle = 2 *angle_2;
 266         // we want an angle between -pi, pi
 267         if (*angle > M_PI) *angle -= 2 *M_PI;
 268
 269         // compute the axe
 270         sin_angle_2 = sin(angle_2);
 271         if (fabs(sin_angle_2) > HKL_EPSILON) {
 272                 // compute the axe using the vector part of the unitary quaterninon
 273                 memcpy(v->data, &self->data[1], sizeof(v->data));
 274                 hkl_vector_div_double(v, sin_angle_2);
 275         } else {
 276                 *angle = 0;
 277                 memset(v->data, 0, sizeof(v->data));
 278         }
 279 }