src/hkl-vector.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-2009 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 <assert.h>
  24 #include <string.h>
  25
  26 #include <hkl/hkl-vector.h>
  27 #include <hkl/hkl-matrix.h>
  28 #include <hkl/hkl-quaternion.h>
  29
  30 void hkl_vector_init(HklVector *v, double x, double y, double z)
  31 {
  32         v->data[0] = x;
  33         v->data[1] = y;
  34         v->data[2] = z;
  35 }
  36
  37 void hkl_vector_fprintf(FILE *file, HklVector const *self)
  38 {
  39         fprintf(file, "|%f, %f, %f|", self->data[0], self->data[1], self->data[2]);
  40 }
  41
  42 int hkl_vector_cmp(HklVector const *self, HklVector const *vector)
  43 {
  44         unsigned int i;
  45
  46         for (i=0; i<3; i++)
  47                 if ( fabs(self->data[i] - vector->data[i]) > HKL_EPSILON )
  48                         return HKL_TRUE;
  49         return HKL_FALSE;
  50 }
  51
  52 /**not yet used*/
  53 int hkl_vector_is_opposite(HklVector const *self, HklVector const *vector)
  54 {
  55         unsigned int i;
  56
  57         for (i=0; i<3; i++)
  58                 if ( fabs(self->data[i] + vector->data[i]) > HKL_EPSILON )
  59                         return HKL_FALSE;
  60         return HKL_TRUE;
  61 }
  62
  63 void hkl_vector_add_vector(HklVector *self, HklVector const *vector)
  64 {
  65         unsigned int i;
  66         for (i=0;i<3;i++)
  67                 self->data[i] += vector->data[i];
  68 }
  69
  70 void hkl_vector_minus_vector(HklVector *self, HklVector const *vector)
  71 {
  72         unsigned int i;
  73         for (i=0;i<3;i++)
  74                 self->data[i] -= vector->data[i];
  75 }
  76
  77 void hkl_vector_div_double(HklVector *self, double const d)
  78 {
  79         unsigned int i;
  80         for (i=0;i<3;i++)
  81                 self->data[i] /= d;
  82 }
  83
  84 void hkl_vector_times_double(HklVector *self, double const d)
  85 {
  86         unsigned int i;
  87         for (i=0;i<3;i++)
  88                 self->data[i] *= d;
  89 }
  90
  91 void hkl_vector_times_vector(HklVector *self, HklVector const *vector)
  92 {
  93         unsigned int i;
  94         for (i=0;i<3;i++)
  95                 self->data[i] *= vector->data[i];
  96 }
  97
  98 void hkl_vector_times_smatrix(HklVector *self, HklMatrix const *m)
  99 {
 100         HklVector tmp;
 101         tmp = *self;
 102
 103         self->data[0] = tmp.data[0] *m->data[0][0] + tmp.data[1] *m->data[1][0] + tmp.data[2] *m->data[2][0];
 104         self->data[1] = tmp.data[0] *m->data[0][1] + tmp.data[1] *m->data[1][1] + tmp.data[2] *m->data[2][1];
 105         self->data[2] = tmp.data[0] *m->data[0][2] + tmp.data[1] *m->data[1][2] + tmp.data[2] *m->data[2][2];
 106 }
 107
 108 double hkl_vector_sum(HklVector const *self)
 109 {
 110         return self->data[0] + self->data[1] + self->data[2];
 111 }
 112
 113 double hkl_vector_scalar_product(HklVector const *self, HklVector const *vector)
 114 {
 115         unsigned int i;
 116         double scalar = 0;
 117
 118         for (i=0;i<3;i++)
 119                 scalar += self->data[i] *vector->data[i];
 120         return scalar;
 121 }
 122
 123 void hkl_vector_vectorial_product(HklVector *self, HklVector const *vector)
 124 {
 125         HklVector tmp;
 126
 127         tmp = *self;
 128         self->data[0] = tmp.data[1] * vector->data[2] - tmp.data[2] * vector->data[1];
 129         self->data[1] = tmp.data[2] * vector->data[0] - tmp.data[0] * vector->data[2];
 130         self->data[2] = tmp.data[0] * vector->data[1] - tmp.data[1] * vector->data[0];
 131 }
 132
 133
 134 double hkl_vector_angle(HklVector const *self, HklVector const *vector)
 135 {
 136         double angle;
 137         double cos_angle;
 138         double norm;
 139         double norm_self;
 140         double norm_vector;
 141
 142         norm_self = hkl_vector_norm2(self);
 143         norm_vector = hkl_vector_norm2(vector);
 144
 145         // check the validity of the parameters
 146         assert(norm_self > HKL_EPSILON);
 147         assert(norm_vector > HKL_EPSILON);
 148
 149         norm = norm_self *norm_vector;
 150
 151         cos_angle = hkl_vector_scalar_product(self, vector) / norm;
 152
 153         // problem with round
 154         if (cos_angle >= 1 )
 155                 angle = 0;
 156         else
 157                 if (cos_angle <= -1 )
 158                         angle = M_PI;
 159                 else
 160                         angle = acos(cos_angle);
 161         return angle;
 162 }
 163
 164 double hkl_vector_oriented_angle(HklVector const *self,
 165                                  HklVector const *vector,
 166                                  HklVector const *ref)
 167 {
 168         double angle;
 169         HklVector tmp;
 170         HklVector ref_u;
 171
 172         angle = hkl_vector_angle(self, vector);
 173         tmp = *self;
 174         hkl_vector_vectorial_product(&tmp, vector);
 175         hkl_vector_normalize(&tmp);
 176         ref_u = *ref;
 177         hkl_vector_normalize(&ref_u);
 178         if (hkl_vector_is_opposite(&tmp, &ref_u))
 179                 angle = -angle;
 180         return angle;
 181 }
 182
 183 /**
 184  *@brief normalize a hkl_vector
 185  *@return true if the hkl_vector can be normalized, false otherwise
 186  *@todo check the status
 187  */
 188 int hkl_vector_normalize(HklVector *self)
 189 {
 190         int status = HKL_FAIL;
 191
 192         double norm = hkl_vector_norm2(self);
 193         if ( norm > HKL_EPSILON )
 194         {
 195                 hkl_vector_div_double(self, norm);
 196                 status = HKL_SUCCESS;
 197         }
 198
 199         return status;
 200 }
 201
 202 int hkl_vector_is_colinear(HklVector const *self, HklVector const *vector)
 203 {
 204         int is_colinear = 0;
 205         HklVector tmp = *self;
 206
 207         hkl_vector_vectorial_product(&tmp, vector);
 208         if (hkl_vector_norm2(&tmp) < HKL_EPSILON)
 209                 is_colinear = 1;
 210
 211         return is_colinear;
 212 }
 213
 214
 215 void hkl_vector_randomize(HklVector *self)
 216 {
 217         self->data[0] = -1 + 2 *rand()/(RAND_MAX+1.0);
 218         self->data[1] = -1 + 2 *rand()/(RAND_MAX+1.0);
 219         self->data[2] = -1 + 2 *rand()/(RAND_MAX+1.0);
 220 }
 221
 222 void hkl_vector_randomize_vector(HklVector *self, HklVector const *vector)
 223 {
 224         do
 225                 hkl_vector_randomize(self);
 226         while (!hkl_vector_cmp(self, vector));
 227 }
 228
 229 void hkl_vector_randomize_vector_vector(HklVector *self,
 230                                         HklVector const *vector1,
 231                                         HklVector const *vector2)
 232 {
 233         do
 234                 hkl_vector_randomize(self);
 235         while (!hkl_vector_cmp(self, vector1) || !hkl_vector_cmp(self, vector2));
 236 }
 237
 238 /**rotate a vector around another vector with an angle */
 239 void hkl_vector_rotated_around_vector(HklVector *self,
 240                                       HklVector const *axe, double angle)
 241 {
 242         double c = cos(angle);
 243         double s = sin(angle);
 244         HklVector axe_n;
 245         HklVector tmp;
 246
 247         axe_n = *axe;
 248         hkl_vector_normalize(&axe_n);
 249
 250         tmp = *self;
 251
 252         self->data[0]  = (c + (1 - c) * axe_n.data[0] * axe_n.data[0])                     * tmp.data[0];
 253         self->data[0] += ((1 - c)     * axe_n.data[0] * axe_n.data[1] - axe_n.data[2] * s) * tmp.data[1];
 254         self->data[0] += ((1 - c)     * axe_n.data[0] * axe_n.data[2] + axe_n.data[1] * s) * tmp.data[2];
 255
 256         self->data[1]  = ((1 - c)     * axe_n.data[0] * axe_n.data[1] + axe_n.data[2] * s) * tmp.data[0];
 257         self->data[1] += (c + (1 - c) * axe_n.data[1] * axe_n.data[1])                     * tmp.data[1];
 258         self->data[1] += ((1 - c)     * axe_n.data[1] * axe_n.data[2] - axe_n.data[0] * s) * tmp.data[2];
 259
 260         self->data[2]  = ((1 - c)     * axe_n.data[0] * axe_n.data[2] - axe_n.data[1] * s) * tmp.data[0];
 261         self->data[2] += ((1 - c)     * axe_n.data[1] * axe_n.data[2] + axe_n.data[0] * s) * tmp.data[1];
 262         self->data[2] += (c + (1 - c) * axe_n.data[2] * axe_n.data[2])                     * tmp.data[2];
 263 }
 264
 265 double hkl_vector_norm2(HklVector const *self)
 266 {
 267         return sqrt(self->data[0] * self->data[0]
 268                     + self->data[1] * self->data[1]
 269                     + self->data[2] * self->data[2]);
 270 }
 271
 272 /**
 273  * apply a quaternion rotation to a vector
 274  * @todo test
 275  */
 276 void hkl_vector_rotated_quaternion(HklVector *self, HklQuaternion const *qr)
 277 {
 278         HklQuaternion q;
 279         HklQuaternion tmp;
 280
 281         // compute qr * qv * *qr
 282         q = *qr;
 283         hkl_quaternion_from_vector(&tmp, self);
 284
 285         hkl_quaternion_times_quaternion(&q, &tmp);
 286         tmp = *qr;
 287         hkl_quaternion_conjugate(&tmp);
 288         hkl_quaternion_times_quaternion(&q, &tmp);
 289
 290         // copy the vector part of the quaternion in the vector
 291         memcpy(self->data, &q.data[1], sizeof(self->data));
 292 }
 293
 294 /**
 295  * @brief check if the hkl_vector is null
 296  * @return true if all |elements| are below HKL_EPSILON, false otherwise
 297  * @todo test
 298  */
 299 int hkl_vector_is_null(HklVector const *self)
 300 {
 301         unsigned int i;
 302         for (i=0; i<3; i++)
 303                 if ( fabs(self->data[i]) > HKL_EPSILON )
 304                         return HKL_FALSE;
 305         return HKL_TRUE;
 306 }
 307
 308 void hkl_vector_project_on_plan(HklVector *self,
 309                                 HklVector const *plan)
 310 {
 311         HklVector tmp;
 312
 313         tmp = *plan;
 314         hkl_vector_normalize(&tmp);
 315         hkl_vector_times_double(&tmp, hkl_vector_scalar_product(self, &tmp));
 316         hkl_vector_minus_vector(self, &tmp);
 317 }