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