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