Guest User

Vector rotation (no SIMD)

a guest
Mar 25th, 2016
102
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include "test2.h"
  2.  
  3. static void cross_product (const float v1[], const float v2[], float res[])
  4. {
  5.     res[0] =  v1[1]*v2[2] - v1[2]*v2[1];
  6.     res[1] = -v1[0]*v2[2] + v1[2]*v2[0];
  7.     res[2] =  v1[0]*v2[1] - v1[1]*v2[0];
  8. }
  9.  
  10. void quat_mul (float q1[], float q2[], float res[])
  11. {
  12.     res[0] = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3];
  13.     res[1] = q1[2]*q2[3] - q1[3]*q2[2] + q1[1]*q2[0] + q1[0]*q2[1];
  14.     res[2] = q1[3]*q2[1] - q1[1]*q2[3] + q1[2]*q2[0] + q1[0]*q2[2];
  15.     res[3] = q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0] + q1[0]*q2[3];
  16. }
  17.  
  18. void rotate_vector (float base[], float vector[], float res[])
  19. {
  20.     float tmp[3], tmp2[3];
  21.     int i;
  22.     cross_product (base+1, vector, tmp);
  23.     for (i=0; i<3; i++) tmp[i] *= 2.0;
  24.     cross_product (base+1, tmp, tmp2);
  25.     for (i=0; i<3; i++) res[i] = vector[i] + base[0]*tmp[i] + tmp2[i];
  26. }
RAW Paste Data