vec.c

   1 static void vneg (float *res, float *v)
   2 {
   3     res[0] = -v[0];
   4     res[1] = -v[1];
   5     res[2] = -v[2];
   6 }
   7
   8 static void vadd (float *res, float *v1, float *v2)
   9 {
  10     res[0] = v1[0] + v2[0];
  11     res[1] = v1[1] + v2[1];
  12     res[2] = v1[2] + v2[2];
  13 }
  14
  15 static void vsub (float *res, float *v1, float *v2)
  16 {
  17     res[0] = v1[0] - v2[0];
  18     res[1] = v1[1] - v2[1];
  19     res[2] = v1[2] - v2[2];
  20 }
  21
  22 static void qconjugate (float *res, float *q)
  23 {
  24     vneg (res, q);
  25     res[3] = q[3];
  26 }
  27
  28 static void qapply (float *res, float *q, float *v)
  29 {
  30     float a = -q[3];
  31     float b = q[0];
  32     float c = q[1];
  33     float d = q[2];
  34     float v1 = v[0];
  35     float v2 = v[1];
  36     float v3 = v[2];
  37     float t2, t3, t4, t5, t6, t7, t8, t9, t10;
  38     t2 =   a*b;
  39     t3 =   a*c;
  40     t4 =   a*d;
  41     t5 =  -b*b;
  42     t6 =   b*c;
  43     t7 =   b*d;
  44     t8 =  -c*c;
  45     t9 =   c*d;
  46     t10 = -d*d;
  47     res[0] = 2*( (t8 + t10)*v1 + (t6 -  t4)*v2 + (t3 + t7)*v3 ) + v1;
  48     res[1] = 2*( (t4 +  t6)*v1 + (t5 + t10)*v2 + (t9 - t2)*v3 ) + v2;
  49     res[2] = 2*( (t7 -  t3)*v1 + (t2 +  t9)*v2 + (t5 + t8)*v3 ) + v3;
  50 }
  51
  52 static void qcompose (float *res, float *q1, float *q2)
  53 {
  54     res[0] = q1[3]*q2[0] + q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1];
  55     res[1] = q1[3]*q2[1] + q1[1]*q2[3] + q1[2]*q2[0] - q1[0]*q2[2];
  56     res[2] = q1[3]*q2[2] + q1[2]*q2[3] + q1[0]*q2[1] - q1[1]*q2[0];
  57     res[3] = q1[3]*q2[3] - q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2];
  58 }
  59
  60 #ifndef USE_VP
  61 static void q2matrixt (float *mat, float *q, float *v)
  62 {
  63     float X = q[0];
  64     float Y = q[1];
  65     float Z = q[2];
  66     float W = q[3];
  67
  68     float xx      = X * X;
  69     float xy      = X * Y;
  70     float xz      = X * Z;
  71     float xw      = X * W;
  72
  73     float yy      = Y * Y;
  74     float yz      = Y * Z;
  75     float yw      = Y * W;
  76
  77     float zz      = Z * Z;
  78     float zw      = Z * W;
  79
  80     mat[0]  = 1 - 2 * ( yy + zz );
  81     mat[1]  =     2 * ( xy - zw );
  82     mat[2]  =     2 * ( xz + yw );
  83
  84     mat[4]  =     2 * ( xy + zw );
  85     mat[5]  = 1 - 2 * ( xx + zz );
  86     mat[6]  =     2 * ( yz - xw );
  87
  88     mat[8]  =     2 * ( xz - yw );
  89     mat[9]  =     2 * ( yz + xw );
  90     mat[10] = 1 - 2 * ( xx + yy );
  91
  92 #ifdef USE_ALTIVEC
  93     mat[12] = v[0];
  94     mat[13] = v[1];
  95     mat[14] = v[2];
  96 #else
  97     mat[3] = v[0];
  98     mat[7] = v[1];
  99     mat[11] = v[2];
 100 #endif
 101 }
 102
 103 #ifndef USE_ALTIVEC
 104 static void mapply_to_point (float *res, float *m, float *v)
 105 {
 106     float x = v[0];
 107     float y = v[1];
 108     float z = v[2];
 109     res[0] = x*m[0] + y*m[4] + z*m[8] + m[3];
 110     res[1] = x*m[1] + y*m[5] + z*m[9] + m[7];
 111     res[2] = x*m[2] + y*m[6] + z*m[10] + m[11];
 112 }
 113
 114 static void mapply_to_vector (float *res, float *m, float *v)
 115 {
 116     float x = v[0];
 117     float y = v[1];
 118     float z = v[2];
 119     res[0] = x*m[0] + y*m[4] + z*m[8];
 120     res[1] = x*m[1] + y*m[5] + z*m[9];
 121     res[2] = x*m[2] + y*m[6] + z*m[10];
 122 }
 123
 124 static void vaddto (float *v1, float *v2)
 125 {
 126     v1[0] += v2[0];
 127     v1[1] += v2[1];
 128     v1[2] += v2[2];
 129 }
 130
 131 static void vcopy (float *res, float *v)
 132 {
 133     *res++ = *v++;
 134     *res++ = *v++;
 135     *res++ = *v++;
 136 }
 137 #endif
 138
 139 #else
 140
 141 static void q2matrixt (float *mat, float *q, float *v)
 142 {
 143     float X = q[0];
 144     float Y = q[1];
 145     float Z = q[2];
 146     float W = q[3];
 147
 148     float xx      = X * X;
 149     float xy      = X * Y;
 150     float xz      = X * Z;
 151     float xw      = X * W;
 152
 153     float yy      = Y * Y;
 154     float yz      = Y * Z;
 155     float yw      = Y * W;
 156
 157     float zz      = Z * Z;
 158     float zw      = Z * W;
 159
 160     mat[0]  = 1 - 2 * ( yy + zz );
 161     mat[4]  =     2 * ( xy - zw );
 162     mat[8]  =     2 * ( xz + yw );
 163
 164     mat[1]  =     2 * ( xy + zw );
 165     mat[5]  = 1 - 2 * ( xx + zz );
 166     mat[9]  =     2 * ( yz - xw );
 167
 168     mat[2]  =     2 * ( xz - yw );
 169     mat[6]  =     2 * ( yz + xw );
 170     mat[10] = 1 - 2 * ( xx + yy );
 171
 172     mat[3] = v[0];
 173     mat[7] = v[1];
 174     mat[11] = v[2];
 175 }
 176 #endif