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