quaternions.c

   1 #include <string.h>
   2 #include <math.h>
   3 #include "xgalaxy.h"
   4
   5 void
   6 vector_sub(Vector *res, Vector *a, Vector *b) {
   7         res->x =  a->x - b->x;
   8         res->y =  a->y - b->y;
   9         res->z =  a->z - b->z;
  10 }
  11
  12 void
  13 vector_add(Vector *res, Vector *a, Vector *b) {
  14         res->x =  a->x + b->x;
  15         res->y =  a->y + b->y;
  16         res->z =  a->z + b->z;
  17 }
  18
  19 void
  20 vector_vmul(Vector *res, Vector *a, Vector *b) {
  21         res->x =  a->y * b->z - b->y * a->z;
  22         res->y =  a->z * b->x - b->z * a->x;
  23         res->z =  a->x * b->y - b->x * a->y;
  24 }
  25
  26 double
  27 vector_smul(Vector *a, Vector *b) {
  28         double res;
  29         res =  a->x * b->x;
  30         res+=  a->y * b->y;
  31         res+=  a->z * b->z;
  32         return res;
  33 }
  34
  35 double
  36 vector_length(Vector *v) {
  37         return sqrt( v->x*v->x + v->y*v->y + v->z*v->z );
  38 }
  39
  40 void
  41 vector_mul(Vector *res, Vector *a, double b) {
  42         res->x =  a->x * b;
  43         res->y =  a->y * b;
  44         res->z =  a->z * b;
  45 }
  46
  47 void
  48 quater_vmul(Quaternion *res, Quaternion *a, Quaternion *b) {
  49         Vector  aXb, wb, Wa;
  50
  51         vector_vmul(&aXb, &(a->d), &(b->d));
  52         vector_mul(&wb, &(b->d), a->w);
  53         vector_mul(&Wa, &(a->d), b->w);
  54
  55         res->d.x = aXb.x + wb.x + Wa.x;
  56         res->d.y = aXb.y + wb.y + Wa.y;
  57         res->d.z = aXb.z + wb.z + Wa.z;
  58
  59                  /*XXX*/
  60         res->w = a->w * b->w - vector_smul( &(a->d),  &(b->d) );
  61 }
  62
  63 double
  64 quater_length(Quaternion *a) {
  65         return sqrt(a->d.x*a->d.x + a->d.y*a->d.y + a->d.z*a->d.z + a->w * a->w);
  66 }
  67
  68 void
  69 quater_inverse(Quaternion *res, Quaternion *a) {
  70         double norm = a->d.x*a->d.x + a->d.y*a->d.y + a->d.z*a->d.z + a->w * a->w;
  71         res->d.x = -a->d.x/norm;
  72         res->d.y = -a->d.y/norm;
  73         res->d.z = -a->d.z/norm;
  74         res->w   =  a->w  /norm;
  75 }
  76
  77 void
  78 rotate(Vector *res, Vector *v, Quaternion *q) {
  79         Quaternion V, qV, Q, R;
  80
  81         memcpy( &(V.d), v, sizeof(Vector) );
  82         V.w = 0;
  83
  84         quater_vmul(&qV, q, &V);
  85         quater_inverse(&Q, q);
  86         quater_vmul(&R, &qV, &Q);
  87
  88         memcpy( res, &(R.d), sizeof(Vector) );
  89 }
  90
  91 void
  92 quater_normalize(Quaternion *q) {
  93         double mag;
  94
  95         mag = sqrt(q->d.x*q->d.x + q->d.y*q->d.y + q->d.z*q->d.z + q->w*q->w);
  96         q->d.x /= mag;
  97         q->d.y /= mag;
  98         q->d.z /= mag;
  99         q->w   /= mag;
 100 }
 101
 102 void
 103 vector_normal(Vector *v) {
 104         vector_mul( v, v, 1.0/ vector_length(v) );
 105 }
 106
 107 void
 108 quater_to_matrix( Quaternion *q, double m[3][3]  ) {
 109         double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
 110         double s  = 2.0/(q->d.x*q->d.x + q->d.y*q->d.y + q->d.z*q->d.z + q->w*q->w);  // 4 mul 3 add 1 div
 111         x2 = q->d.x * s;    y2 = q->d.y * s;    z2 = q->d.z * s;
 112         xx = q->d.x * x2;   xy = q->d.x * y2;   xz = q->d.x * z2;
 113         yy = q->d.y * y2;   yz = q->d.y * z2;   zz = q->d.z * z2;
 114         wx =   q->w * x2;   wy =   q->w * y2;   wz =   q->w * z2;
 115
 116         m[0][0] = 1.0 - (yy + zz);
 117         m[1][0] = xy - wz;
 118         m[2][0] = xz + wy;
 119
 120         m[0][1] = xy + wz;
 121         m[1][1] = 1.0 - (xx + zz);
 122         m[2][1] = yz - wx;
 123
 124         m[0][2] = xz - wy;
 125         m[1][2] = yz + wx;
 126         m[2][2] = 1.0 - (xx + yy);
 127 }
 128
 129 void
 130 matrix_to_quater(  double m[3][3], Quaternion *q ) {
 131         double tr = m[0][0] + m[1][1] + m[2][2]; // trace of martix
 132         if (tr > 0.0){     // if trace positive than "w" is biggest component
 133                 q->d.x = m[1][2] - m[2][1];
 134                 q->d.y = m[2][0] - m[0][2];
 135                 q->d.z = m[0][1] - m[1][0];
 136                 q->w   =  tr+1.0;
 137         }else  if( (m[0][0] > m[1][1] ) && ( m[0][0] > m[2][2]) ) {
 138                 q->d.x = 1.0 + m[0][0] - m[1][1] - m[2][2];
 139                 q->d.y = m[1][0] + m[0][1];
 140                 q->d.z = m[2][0] + m[0][2];
 141                 q->w   = m[1][2] - m[2][1];
 142         } else if ( m[1][1] > m[2][2] ){
 143                 q->d.x = m[1][0] + m[0][1];
 144                 q->d.y = 1.0 + m[1][1] - m[0][0] - m[2][2];
 145                 q->d.z = m[2][1] + m[1][2];
 146                 q->w   = m[2][0] - m[0][2];
 147         } else {
 148                 q->d.x = m[2][0] + m[0][2];
 149                 q->d.y = m[2][1] + m[1][2];
 150                 q->d.z = 1.0 + m[2][2] - m[0][0] - m[1][1];
 151                 q->w   = m[0][1] - m[1][0];
 152         }
 153         quater_normalize(q);
 154 }
 155
 156 void
 157 quater_to_angles( Quaternion *q, Vector *a) {
 158         double  m[3][3];
 159         double D,C,trx,try;
 160
 161         quater_to_matrix(q, m);
 162
 163         D = a->y = asin(m[2][0]);
 164         C =  cos( a->y );
 165
 166         if ( fabs(C) > 1e-8 ) {
 167                 trx =  m[2][2] / C;
 168                 try = -m[2][1]  / C;
 169                 a->x = atan2( try, trx );
 170
 171                 trx =  m[0][0] / C;
 172                 try = -m[1][0] / C;
 173                 a->z =  atan2( try, trx );
 174         } else {
 175                 a->x = 0.0;
 176                 trx      = m[1][1];
 177                 try      = m[0][1];
 178
 179                 a->z  = atan2( try, trx );
 180         }
 181 }
 182