src/svector.c

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