src/hkl-vector.c

   1 #include <stdlib.h>
   2 #include <assert.h>
   3 #include <string.h>
   4
   5 #include <hkl/hkl-vector.h>
   6 #include <hkl/hkl-matrix.h>
   7 #include <hkl/hkl-quaternion.h>
   8
   9 void hkl_vector_init(HklVector *v, double x, double y, double z)
  10 {
  11         v->data[0] = x;
  12         v->data[1] = y;
  13         v->data[2] = z;
  14 }
  15
  16 void hkl_vector_fprintf(FILE *file, HklVector const *v)
  17 {
  18         fprintf(file, "|%f, %f, %f|", v->data[0], v->data[1], v->data[2]);
  19 }
  20
  21 int hkl_vector_cmp(HklVector const *v, HklVector const *v1)
  22 {
  23         unsigned int i;
  24
  25         for (i=0; i<3; i++)
  26                 if ( fabs(v->data[i] - v1->data[i]) > HKL_EPSILON )
  27                         return HKL_TRUE;
  28         return HKL_FALSE;
  29 }
  30
  31 /**not yet used*/
  32 extern int hkl_vector_is_opposite(HklVector const *v, HklVector const *v1)
  33 {
  34         unsigned int i;
  35
  36         for (i=0; i<3; i++)
  37                 if ( fabs(v->data[i] + v1->data[i]) > HKL_EPSILON )
  38                         return HKL_FALSE;
  39         return HKL_TRUE;
  40 }
  41
  42 void hkl_vector_minus_vector(HklVector *v, HklVector const *v1)
  43 {
  44         unsigned int i;
  45         for (i=0;i<3;i++)
  46                 v->data[i] -= v1->data[i];
  47 }
  48
  49 void hkl_vector_div_double(HklVector *v, double const d)
  50 {
  51         unsigned int i;
  52         for (i=0;i<3;i++)
  53                 v->data[i] /= d;
  54 }
  55
  56 void hkl_vector_times_double(HklVector *v, double const d)
  57 {
  58         unsigned int i;
  59         for (i=0;i<3;i++)
  60                 v->data[i] *= d;
  61 }
  62
  63 void hkl_vector_times_vector(HklVector *v, HklVector const *v1)
  64 {
  65         unsigned int i;
  66         for (i=0;i<3;i++)
  67                 v->data[i] *= v1->data[i];
  68 }
  69
  70 void hkl_vector_times_smatrix(HklVector *v, HklMatrix const *m)
  71 {
  72         HklVector tmp;
  73         tmp = *v;
  74
  75         v->data[0] = tmp.data[0] *m->data[0][0] + tmp.data[1] *m->data[1][0] + tmp.data[2] *m->data[2][0];
  76         v->data[1] = tmp.data[0] *m->data[0][1] + tmp.data[1] *m->data[1][1] + tmp.data[2] *m->data[2][1];
  77         v->data[2] = tmp.data[0] *m->data[0][2] + tmp.data[1] *m->data[1][2] + tmp.data[2] *m->data[2][2];
  78 }
  79
  80 double hkl_vector_sum(HklVector const *v)
  81 {
  82         return v->data[0] + v->data[1] + v->data[2];
  83 }
  84
  85 double hkl_vector_scalar_product(HklVector const *v, HklVector const *v1)
  86 {
  87         unsigned int i;
  88         double scalar = 0;
  89
  90         for (i=0;i<3;i++)
  91                 scalar += v->data[i] *v1->data[i];
  92         return scalar;
  93 }
  94
  95 void hkl_vector_vectorial_product(HklVector *v, HklVector const *v1)
  96 {
  97         HklVector tmp;
  98
  99         tmp = *v;
 100         v->data[0] = tmp.data[1] *v1->data[2] - tmp.data[2] *v1->data[1];
 101         v->data[1] = tmp.data[2] *v1->data[0] - tmp.data[0] *v1->data[2];
 102         v->data[2] = tmp.data[0] *v1->data[1] - tmp.data[1] *v1->data[0];
 103 }
 104
 105
 106 double hkl_vector_angle(HklVector const *v, HklVector const *v1)
 107 {
 108         double angle;
 109         double cos_angle;
 110         double norm;
 111         double norm_v;
 112         double norm_v1;
 113
 114         norm_v = hkl_vector_norm2(v);
 115         norm_v1 = hkl_vector_norm2(v1);
 116
 117         // check the validity of the parameters
 118         assert(norm_v > HKL_EPSILON);
 119         assert(norm_v1 > HKL_EPSILON);
 120
 121         norm = norm_v *norm_v1;
 122
 123         cos_angle = hkl_vector_scalar_product(v, v1) / norm;
 124
 125         // problem with round
 126         if (cos_angle >= 1 )
 127                 angle = 0;
 128         else
 129                 if (cos_angle <= -1 )
 130                         angle = M_PI;
 131                 else
 132                         angle = acos(cos_angle);
 133         return angle;
 134 }
 135
 136 /**
 137  *@brief normalize a hkl_vector
 138  *@return true if the hkl_vector can be normalized, false otherwise
 139  *@todo check the status
 140  */
 141 int hkl_vector_normalize(HklVector *v)
 142 {
 143         int status = HKL_FAIL;
 144
 145         double norm = hkl_vector_norm2(v);
 146         if ( norm > HKL_EPSILON )
 147         {
 148                 hkl_vector_div_double(v, norm);
 149                 status = HKL_SUCCESS;
 150         }
 151
 152         return status;
 153 }
 154
 155 int hkl_vector_is_colinear(HklVector const *v, HklVector const *v1)
 156 {
 157         int is_colinear = 0;
 158         HklVector tmp = *v;
 159
 160         hkl_vector_vectorial_product(&tmp, v1);
 161         if (hkl_vector_norm2(&tmp) < HKL_EPSILON)
 162                 is_colinear = 1;
 163
 164         return is_colinear;
 165 }
 166
 167
 168 void hkl_vector_randomize(HklVector *v)
 169 {
 170         v->data[0] = -1 + 2 *rand()/(RAND_MAX+1.0);
 171         v->data[1] = -1 + 2 *rand()/(RAND_MAX+1.0);
 172         v->data[2] = -1 + 2 *rand()/(RAND_MAX+1.0);
 173 }
 174
 175 void hkl_vector_randomize_vector(HklVector *v, HklVector const *v1)
 176 {
 177         do
 178                 hkl_vector_randomize(v);
 179         while (!hkl_vector_cmp(v, v1));
 180 }
 181
 182 void hkl_vector_randomize_vector_vector(HklVector *v, HklVector const *v1, HklVector const *v2)
 183 {
 184         do
 185                 hkl_vector_randomize(v);
 186         while (!hkl_vector_cmp(v, v1) || !hkl_vector_cmp(v, v2));
 187 }
 188
 189 /**rotate a vector around another vector with an angle */
 190 void hkl_vector_rotated_around_vector(HklVector *v, HklVector const *axe, double angle)
 191 {
 192         double c = cos(angle);
 193         double s = sin(angle);
 194         HklVector axe_n;
 195         HklVector tmp;
 196
 197         axe_n = *axe;
 198         hkl_vector_normalize(&axe_n);
 199
 200         tmp = *v;
 201
 202         v->data[0] = (c + (1 - c) *axe_n.data[0] *axe_n.data[0]) *tmp.data[0];
 203         v->data[0] += ((1 - c) *axe_n.data[0] *axe_n.data[1] - axe_n.data[2] *s) *tmp.data[1];
 204         v->data[0] += ((1 - c) *axe_n.data[0] *axe_n.data[2] + axe_n.data[1] *s) *tmp.data[2];
 205
 206         v->data[1] = ((1 - c) *axe_n.data[0] *axe_n.data[1] + axe_n.data[2] *s) *tmp.data[0];
 207         v->data[1] += (c + (1 - c) *axe_n.data[1] *axe_n.data[1]) *tmp.data[1];
 208         v->data[1] += ((1 - c) *axe_n.data[1] *axe_n.data[2] - axe_n.data[0] *s) *tmp.data[2];
 209
 210         v->data[2] = ((1 - c) *axe_n.data[0] *axe_n.data[2] - axe_n.data[1] *s) *tmp.data[0];
 211         v->data[2] += ((1 - c) *axe_n.data[1] *axe_n.data[2] + axe_n.data[0] *s) *tmp.data[1];
 212         v->data[2] += (c + (1 - c) *axe_n.data[2] *axe_n.data[2]) *tmp.data[2];
 213 }
 214
 215 double hkl_vector_norm2(HklVector const *v)
 216 {
 217         return sqrt(v->data[0] *v->data[0] + v->data[1] *v->data[1] + v->data[2] *v->data[2]);
 218 }
 219
 220 /**
 221  * apply a quaternion rotation to a vector
 222  * @todo test
 223  */
 224 void hkl_vector_rotated_quaternion(HklVector *v, HklQuaternion const *qr)
 225 {
 226         HklQuaternion q;
 227         HklQuaternion tmp;
 228
 229         // compute qr * qv * *qr
 230         q = *qr;
 231         hkl_quaternion_from_vector(&tmp, v);
 232
 233         hkl_quaternion_times_quaternion(&q, &tmp);
 234         tmp = *qr;
 235         hkl_quaternion_conjugate(&tmp);
 236         hkl_quaternion_times_quaternion(&q, &tmp);
 237
 238         // copy the vector part of the quaternion in the vector
 239         memcpy(v->data, &q.data[1], sizeof(v->data));
 240 }
 241
 242 /**
 243  * @brief check if the hkl_vector is null
 244  * @return true if all |elements| are below HKL_EPSILON, false otherwise
 245  * @todo test
 246  */
 247 int hkl_vector_is_null(HklVector const *v)
 248 {
 249         unsigned int i;
 250         for (i=0; i<3; i++)
 251                 if ( fabs(v->data[i]) > HKL_EPSILON )
 252                         return HKL_FALSE;
 253         return HKL_TRUE;
 254 }