hkl/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-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 <string.h>
  24
  25 #include <gsl/gsl_math.h>
  26
  27 #include <hkl/hkl-vector.h>
  28 #include <hkl/hkl-matrix.h>
  29 #include <hkl/hkl-quaternion.h>
  30
  31 /**
  32  * hkl_vector_init:
  33  * @self: the #HklVector to initialize.
  34  * @x: the first coordinate value
  35  * @y: the second coordinate value
  36  * @z: the third coordinate value
  37  *
  38  * initialize an #HklVector
  39  **/
  40 void hkl_vector_init(HklVector *self, double x, double y, double z)
  41 {
  42         self->data[0] = x;
  43         self->data[1] = y;
  44         self->data[2] = z;
  45 }
  46
  47 /**
  48  * hkl_vector_fprintf:
  49  * @file: the stream to print into
  50  * @self: the #HklVector to print.
  51  *
  52  * print an #HklVector into a stream
  53  **/
  54 void hkl_vector_fprintf(FILE *file, const HklVector *self)
  55 {
  56         fprintf(file, "|%f, %f, %f|", self->data[0], self->data[1], self->data[2]);
  57 }
  58
  59 /**
  60  * hkl_vector_cmp:
  61  * @self: the first vector
  62  * @vector: th vector to compare with
  63  *
  64  * compare two #HklVector. this comparison use HKL_EPSILON
  65  * to do the comparison.
  66  *
  67  * Returns: HKL_FALSE if both are equals, HKL_TRUE otherwise.
  68  **/
  69 int hkl_vector_cmp(const HklVector *self, const HklVector *vector)
  70 {
  71         unsigned int i;
  72
  73         for (i=0; i<3; i++)
  74                 if ( fabs(self->data[i] - vector->data[i]) > HKL_EPSILON )
  75                         return HKL_TRUE;
  76         return HKL_FALSE;
  77 }
  78
  79 /**not yet used*/
  80 int hkl_vector_is_opposite(const HklVector *self, const HklVector *vector)
  81 {
  82         unsigned int i;
  83
  84         for (i=0; i<3; i++)
  85                 if ( fabs(self->data[i] + vector->data[i]) > HKL_EPSILON )
  86                         return HKL_FALSE;
  87         return HKL_TRUE;
  88 }
  89
  90 /**
  91  * hkl_vector_add_vector:
  92  * @self: the modified #HklVector
  93  * @vector: the #hklvector to add
  94  *
  95  * add an #HklVector to another one.
  96  **/
  97 void hkl_vector_add_vector(HklVector *self, const HklVector *vector)
  98 {
  99         unsigned int i;
 100         for (i=0;i<3;i++)
 101                 self->data[i] += vector->data[i];
 102 }
 103
 104 /**
 105  * hkl_vector_minus_vector:
 106  * @self: the modified #HklVector
 107  * @vector: the #hklvector to substract
 108  *
 109  * substract an #HklVector to another one.
 110  **/
 111 void hkl_vector_minus_vector(HklVector *self, const HklVector *vector)
 112 {
 113         unsigned int i;
 114         for (i=0;i<3;i++)
 115                 self->data[i] -= vector->data[i];
 116 }
 117
 118 /**
 119  * hkl_vector_div_double:
 120  * @self: the #HklVector to divide.
 121  * @d: constant use to divide the #HklVector
 122  *
 123  * divide an #HklVector by constant.
 124  **/
 125 void hkl_vector_div_double(HklVector *self, const double d)
 126 {
 127         unsigned int i;
 128         for (i=0;i<3;i++)
 129                 self->data[i] /= d;
 130 }
 131
 132 /**
 133  * hkl_vector_times_double:
 134  * @self: the #HklVector to modify
 135  * @d: the multiply factor
 136  *
 137  * multiply an #HklVector by a constant value.
 138  **/
 139 void hkl_vector_times_double(HklVector *self, const double d)
 140 {
 141         unsigned int i;
 142         for (i=0;i<3;i++)
 143                 self->data[i] *= d;
 144 }
 145
 146 /**
 147  * hkl_vector_times_vector:
 148  * @self: the #HklVector to modify
 149  * @vector: the #HklVector use to modify the first one
 150  *
 151  * multiply an #HklVector by another one. This method multiply
 152  * coordinate by coordinate.
 153  **/
 154 void hkl_vector_times_vector(HklVector *self, const HklVector *vector)
 155 {
 156         unsigned int i;
 157         for (i=0;i<3;i++)
 158                 self->data[i] *= vector->data[i];
 159 }
 160
 161 /**
 162  * hkl_vector_times_smatrix:
 163  * @self: the #HklVector to multiply
 164  * @m: the #HklMatrix use to multiply the #HklVector
 165  *
 166  * multiply an #HklVector by an #HklMatrix.
 167  * compute v'= M . v
 168  **/
 169 void hkl_vector_times_matrix(HklVector *self, const HklMatrix *m)
 170 {
 171         HklVector tmp;
 172         tmp = *self;
 173
 174         self->data[0] = tmp.data[0] *m->data[0][0] + tmp.data[1] *m->data[1][0] + tmp.data[2] *m->data[2][0];
 175         self->data[1] = tmp.data[0] *m->data[0][1] + tmp.data[1] *m->data[1][1] + tmp.data[2] *m->data[2][1];
 176         self->data[2] = tmp.data[0] *m->data[0][2] + tmp.data[1] *m->data[1][2] + tmp.data[2] *m->data[2][2];
 177 }
 178
 179 /**
 180  * hkl_vector_sum:
 181  * @self: the #HklVector to sum.
 182  *
 183  * compute the #HklVector sum of all its elements.
 184  *
 185  * Returns: the sum of all elements.
 186  **/
 187 double hkl_vector_sum(const HklVector *self)
 188 {
 189         return self->data[0] + self->data[1] + self->data[2];
 190 }
 191
 192 /**
 193  * hkl_vector_scalar_product:
 194  * @self: the first #HklVector
 195  * @vector: the second #HklVector
 196  *
 197  * compute the scalar product of two #HklVector
 198  *
 199  * Returns: the scalar product.
 200  **/
 201 double hkl_vector_scalar_product(const HklVector *self, const HklVector *vector)
 202 {
 203         unsigned int i;
 204         double scalar = 0;
 205
 206         for (i=0;i<3;i++)
 207                 scalar += self->data[i] *vector->data[i];
 208         return scalar;
 209 }
 210
 211 /**
 212  * hkl_vector_vectorial_product:
 213  * @self: the first #HklVector (modify)
 214  * @vector: the second #HklVector
 215  *
 216  * compute the vectorial product of two vectors
 217  **/
 218 void hkl_vector_vectorial_product(HklVector *self, const HklVector *vector)
 219 {
 220         HklVector tmp;
 221
 222         tmp = *self;
 223         self->data[0] = tmp.data[1] * vector->data[2] - tmp.data[2] * vector->data[1];
 224         self->data[1] = tmp.data[2] * vector->data[0] - tmp.data[0] * vector->data[2];
 225         self->data[2] = tmp.data[0] * vector->data[1] - tmp.data[1] * vector->data[0];
 226 }
 227
 228
 229 /**
 230  * hkl_vector_angle:
 231  * @self: the fist #HklVector
 232  * @vector: the second #HklVector
 233  *
 234  * compute the angles beetween two #HklVector
 235  *
 236  * Returns: the return value is in beetween [0, pi]
 237  **/
 238 double hkl_vector_angle(const HklVector *self, const HklVector *vector)
 239 {
 240         double angle;
 241         double cos_angle;
 242         double norm;
 243         double norm_self;
 244         double norm_vector;
 245
 246         norm_self = hkl_vector_norm2(self);
 247         norm_vector = hkl_vector_norm2(vector);
 248
 249         if (norm_self < HKL_EPSILON || norm_vector < HKL_EPSILON)
 250                 return 0.0;
 251
 252         norm = norm_self * norm_vector;
 253
 254         cos_angle = hkl_vector_scalar_product(self, vector) / norm;
 255
 256         /* problem with round */
 257         if (cos_angle >= 1 )
 258                 angle = 0;
 259         else
 260                 if (cos_angle <= -1 )
 261                         angle = M_PI;
 262                 else
 263                         angle = acos(cos_angle);
 264         return angle;
 265 }
 266
 267 /**
 268  * hkl_vector_oriented_angle:
 269  * @self: the first #HklVector
 270  * @vector: the second #HklVector
 271  * @ref: the reference #HklVector
 272  *
 273  * compute the angles beetween two #HklVector and use
 274  * a reference #HklVector to orientate the space. That's
 275  * way the return value can be in beetween [-pi, pi].
 276  * the (self, vector, ref) is a right oriented base.
 277  *
 278  * Returns: the angles [-pi, pi]
 279  **/
 280 double hkl_vector_oriented_angle(const HklVector *self,
 281                                  const HklVector *vector,
 282                                  const HklVector *ref)
 283 {
 284         double angle;
 285         HklVector tmp;
 286         HklVector ref_u;
 287
 288         angle = hkl_vector_angle(self, vector);
 289         tmp = *self;
 290         hkl_vector_vectorial_product(&tmp, vector);
 291         hkl_vector_normalize(&tmp);
 292         ref_u = *ref;
 293         hkl_vector_normalize(&ref_u);
 294         if (hkl_vector_is_opposite(&tmp, &ref_u))
 295                 angle = -angle;
 296         return angle;
 297 }
 298 /**
 299  * hkl_vector_oriented_angle_points:
 300  * @self: the first point
 301  * @p2: the second point
 302  * @p2: the third point
 303  * @ref: the reference #HklVector
 304  *
 305  * compute the angles beetween three points (p1, p2, p3) and use
 306  * a reference #HklVector to orientate the space. That's
 307  * way the return value can be in beetween [-pi, pi].
 308  * the (self, vector, ref) is a right oriented base.
 309  *
 310  * Returns: the angles [-pi, pi]
 311  **/
 312 double hkl_vector_oriented_angle_points(const HklVector *self,
 313                                         const HklVector *p2,
 314                                         const HklVector *p3,
 315                                         const HklVector *ref)
 316 {
 317         HklVector v1;
 318         HklVector v2;
 319
 320         v1 = *self;
 321         v2 = *p3;
 322         hkl_vector_minus_vector(&v1, p2);
 323         hkl_vector_minus_vector(&v2, p2);
 324         return hkl_vector_oriented_angle(&v1, &v2, ref);
 325 }
 326
 327 /**
 328  * hkl_vector_normalize:
 329  * @self: the #HklVector to normalize
 330  *
 331  * normalize a hkl_vector
 332  *
 333  * Returns: HKL_SUCCESS if the #HklVector can be normalized, HKL_FAIL otherwise
 334  **/
 335 int hkl_vector_normalize(HklVector *self)
 336 {
 337         int status = HKL_FAIL;
 338
 339         double norm = hkl_vector_norm2(self);
 340         if ( norm > HKL_EPSILON )
 341         {
 342                 hkl_vector_div_double(self, norm);
 343                 status = HKL_SUCCESS;
 344         }
 345
 346         return status;
 347 }
 348
 349 /**
 350  * hkl_vector_is_colinear:
 351  * @self: the first #HklVector
 352  * @vector: the second #HklVector
 353  *
 354  * check if two #HklVector are colinears
 355  *
 356  * Returns: HKL_TRUE if both are colinear.
 357  **/
 358 int hkl_vector_is_colinear(const HklVector *self, const HklVector *vector)
 359 {
 360         int is_colinear = 0;
 361         HklVector tmp = *self;
 362
 363         hkl_vector_vectorial_product(&tmp, vector);
 364         if (hkl_vector_norm2(&tmp) < HKL_EPSILON)
 365                 is_colinear = 1;
 366
 367         return is_colinear;
 368 }
 369
 370
 371 /**
 372  * hkl_vector_randomize:
 373  * @self: the #HklVector to randomize
 374  *
 375  * initialize a vector with random values.
 376  * coordinates range [-1, 1]
 377  **/
 378 void hkl_vector_randomize(HklVector *self)
 379 {
 380         self->data[0] = -1 + 2 *rand()/(RAND_MAX+1.0);
 381         self->data[1] = -1 + 2 *rand()/(RAND_MAX+1.0);
 382         self->data[2] = -1 + 2 *rand()/(RAND_MAX+1.0);
 383 }
 384
 385 /**
 386  * hkl_vector_randomize_vector:
 387  * @self: the #HklVector to randomize
 388  * @vector: the #HklVector result to avoid
 389  *
 390  * randomize an #HklVector an be sure that it is not equal
 391  * to the #HklVector vector.
 392  **/
 393 void hkl_vector_randomize_vector(HklVector *self, const HklVector *vector)
 394 {
 395         do
 396                 hkl_vector_randomize(self);
 397         while (!hkl_vector_cmp(self, vector));
 398 }
 399
 400 /**
 401  * hkl_vector_randomize_vector_vector:
 402  * @self: the #HklVector to randomize
 403  * @vector1: the first #HklVector solution to avoid
 404  * @vector2: the second #HklVector solution to avoid
 405  *
 406  * randomize an #HklVector an be sure that it is not equal
 407  * to the #HklVector vector1 and vector2.
 408  *
 409  **/
 410 void hkl_vector_randomize_vector_vector(HklVector *self,
 411                                         const HklVector *vector1,
 412                                         const HklVector *vector2)
 413 {
 414         do
 415                 hkl_vector_randomize(self);
 416         while (!hkl_vector_cmp(self, vector1) || !hkl_vector_cmp(self, vector2));
 417 }
 418
 419 /**
 420  * hkl_vector_rotated_around_vector:
 421  * @self: the #HklVector to rotate
 422  * @axe: the axe of rotation
 423  * @angle: the angle of the rotation
 424  *
 425  * rotate a vector around another one with a given angle.
 426  **/
 427 void hkl_vector_rotated_around_vector(HklVector *self,
 428                                       const HklVector *axe, double angle)
 429 {
 430         double c = cos(angle);
 431         double s = sin(angle);
 432         HklVector axe_n;
 433         HklVector tmp;
 434
 435         axe_n = *axe;
 436         hkl_vector_normalize(&axe_n);
 437
 438         tmp = *self;
 439
 440         self->data[0]  = (c + (1 - c) * axe_n.data[0] * axe_n.data[0])                     * tmp.data[0];
 441         self->data[0] += ((1 - c)     * axe_n.data[0] * axe_n.data[1] - axe_n.data[2] * s) * tmp.data[1];
 442         self->data[0] += ((1 - c)     * axe_n.data[0] * axe_n.data[2] + axe_n.data[1] * s) * tmp.data[2];
 443
 444         self->data[1]  = ((1 - c)     * axe_n.data[0] * axe_n.data[1] + axe_n.data[2] * s) * tmp.data[0];
 445         self->data[1] += (c + (1 - c) * axe_n.data[1] * axe_n.data[1])                     * tmp.data[1];
 446         self->data[1] += ((1 - c)     * axe_n.data[1] * axe_n.data[2] - axe_n.data[0] * s) * tmp.data[2];
 447
 448         self->data[2]  = ((1 - c)     * axe_n.data[0] * axe_n.data[2] - axe_n.data[1] * s) * tmp.data[0];
 449         self->data[2] += ((1 - c)     * axe_n.data[1] * axe_n.data[2] + axe_n.data[0] * s) * tmp.data[1];
 450         self->data[2] += (c + (1 - c) * axe_n.data[2] * axe_n.data[2])                     * tmp.data[2];
 451 }
 452
 453 /**
 454  * hkl_vector_norm2:
 455  * @self: the #hklvector use to compute the norm2
 456  *
 457  * compute the norm2 of an #HklVector
 458  *
 459  * Returns: the sqrt(|v|)
 460  **/
 461 double hkl_vector_norm2(const HklVector *self)
 462 {
 463         return sqrt(self->data[0] * self->data[0]
 464                     + self->data[1] * self->data[1]
 465                     + self->data[2] * self->data[2]);
 466 }
 467
 468 /**
 469  * hkl_vector_rotated_quaternion:
 470  * @self: the #HklVector to rotate
 471  * @qr: the #HklQuaternion use to rotate the vector
 472  *
 473  * rotate an #HklVector using an #HklQuaternion.
 474  **/
 475 void hkl_vector_rotated_quaternion(HklVector *self, const HklQuaternion *qr)
 476 {
 477         double v1 = self->data[0];
 478         double v2 = self->data[1];
 479         double v3 = self->data[2];
 480         double a = qr->data[0];
 481         double b = qr->data[1];
 482         double c = qr->data[2];
 483         double d = qr->data[3];
 484
 485         double t2 =   a*b;
 486         double t3 =   a*c;
 487         double t4 =   a*d;
 488         double t5 =  -b*b;
 489         double t6 =   b*c;
 490         double t7 =   b*d;
 491         double t8 =  -c*c;
 492         double t9 =   c*d;
 493         double t10 = -d*d;
 494
 495         self->data[0] = 2*( (t8 + t10)*v1 + (t6 -  t4)*v2 + (t3 + t7)*v3 ) + v1;
 496         self->data[1] = 2*( (t4 +  t6)*v1 + (t5 + t10)*v2 + (t9 - t2)*v3 ) + v2;
 497         self->data[2] = 2*( (t7 -  t3)*v1 + (t2 +  t9)*v2 + (t5 + t8)*v3 ) + v3;
 498 }
 499
 500 /**
 501  * hkl_vector_rotated_around_line:
 502  * @self: the point to rotate around a line
 503  * @angle: the angle of the rotation
 504  * @c1: the fist point of the line
 505  * @c2: the second point of the line
 506  *
 507  * This method rotate a point around a line defined by two points
 508  * of a certain amount of angle. The rotation is right handed.
 509  * this mean that c2 - c1 gives the direction of the rotation.
 510  **/
 511 void hkl_vector_rotated_around_line(HklVector *self, double angle,
 512                                     const HklVector *c1, const HklVector *c2)
 513 {
 514         HklVector axis;
 515
 516         if (!self || !c1 || !c2 || fabs(angle) < HKL_EPSILON)
 517                 return;
 518
 519         axis = *c2;
 520         hkl_vector_minus_vector(&axis, c1);
 521         /* the c2 - c1 vector must be non null */
 522
 523         hkl_vector_minus_vector(self, c1);
 524         hkl_vector_rotated_around_vector(self, &axis, angle);
 525         hkl_vector_add_vector(self, c1);
 526 }
 527
 528 /**
 529  * hkl_vector_is_null:
 530  * @self: the #hklvector to check
 531  *
 532  * check if all the coordinates of an #HklVector are null.
 533  *
 534  * Returns: HKl_TRUE if all |elements| are below HKL_EPSILON, HKl_FALSE otherwise
 535  *
 536  * Todo: test
 537  */
 538 int hkl_vector_is_null(const HklVector *self)
 539 {
 540         unsigned int i;
 541         for (i=0; i<3; i++)
 542                 if ( fabs(self->data[i]) > HKL_EPSILON )
 543                         return HKL_FALSE;
 544         return HKL_TRUE;
 545 }
 546
 547 /**
 548  * hkl_vector_project_on_plan:
 549  * @self: the vector to project (modify)
 550  * @normal: the normal of the plane.
 551  *
 552  * project an #HklVector on a plan of normal #normal which contain
 553  * the origin [0, 0, 0]
 554  *
 555  **/
 556 void hkl_vector_project_on_plan(HklVector *self,
 557                                 const HklVector *normal)
 558 {
 559         HklVector tmp;
 560
 561         if(!self || !normal)
 562                 return;
 563
 564         tmp = *normal;
 565         hkl_vector_normalize(&tmp);
 566         hkl_vector_times_double(&tmp, hkl_vector_scalar_product(self, &tmp));
 567         hkl_vector_minus_vector(self, &tmp);
 568 }
 569
 570 /**
 571  * hkl_vector_project_on_plan_with_point:
 572  * @self: the vector to project (modify)
 573  * @normal: the normal of the plane.
 574  * @point: a point of the plan.
 575  *
 576  * project an #HklVector on a plan of normal #normal which contain #point.
 577  **/
 578 void hkl_vector_project_on_plan_with_point(HklVector *self,
 579                                            const HklVector *normal,
 580                                            const HklVector *point)
 581 {
 582         HklVector tmp;
 583         double d1, d2;
 584
 585         if(!self || !normal || !point)
 586                 return;
 587
 588         tmp = *normal;
 589         hkl_vector_normalize(&tmp);
 590         d1 = hkl_vector_scalar_product(self, &tmp);
 591         d2 = hkl_vector_scalar_product(point, &tmp);
 592         hkl_vector_times_double(&tmp, d1 - d2);
 593         hkl_vector_minus_vector(self, &tmp);
 594 }