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 #define MAT_V0 12
  94 #define MAT_V1 13
  95 #define MAT_V2 14
  96 #else
  97 #define MAT_V0 3
  98 #define MAT_V1 7
  99 #define MAT_V2 11
 100 #endif
 101
 102     mat[MAT_V0] = v[0];
 103     mat[MAT_V1] = v[1];
 104     mat[MAT_V2] = v[2];
 105 }
 106
 107 static void mapply_to_point (float *res, float *m, float *v)
 108 {
 109     float x = v[0];
 110     float y = v[1];
 111     float z = v[2];
 112     res[0] = x*m[0] + y*m[4] + z*m[8] + m[MAT_V0];
 113     res[1] = x*m[1] + y*m[5] + z*m[9] + m[MAT_V1];
 114     res[2] = x*m[2] + y*m[6] + z*m[10] + m[MAT_V2];
 115 }
 116
 117 static void mapply_to_vector (float *res, float *m, float *v)
 118 {
 119     float x = v[0];
 120     float y = v[1];
 121     float z = v[2];
 122     res[0] = x*m[0] + y*m[4] + z*m[8];
 123     res[1] = x*m[1] + y*m[5] + z*m[9];
 124     res[2] = x*m[2] + y*m[6] + z*m[10];
 125 }
 126
 127 static void vaddto (float *v1, float *v2)
 128 {
 129     v1[0] += v2[0];
 130     v1[1] += v2[1];
 131     v1[2] += v2[2];
 132 }
 133
 134 static void vcopy (float *res, float *v)
 135 {
 136     *res++ = *v++;
 137     *res++ = *v++;
 138     *res++ = *v++;
 139 }
 140
 141 #else
 142
 143 static void q2matrixt (float *mat, float *q, float *v)
 144 {
 145     float X = q[0];
 146     float Y = q[1];
 147     float Z = q[2];
 148     float W = q[3];
 149
 150     float xx      = X * X;
 151     float xy      = X * Y;
 152     float xz      = X * Z;
 153     float xw      = X * W;
 154
 155     float yy      = Y * Y;
 156     float yz      = Y * Z;
 157     float yw      = Y * W;
 158
 159     float zz      = Z * Z;
 160     float zw      = Z * W;
 161
 162     mat[0]  = 1 - 2 * ( yy + zz );
 163     mat[4]  =     2 * ( xy - zw );
 164     mat[8]  =     2 * ( xz + yw );
 165
 166     mat[1]  =     2 * ( xy + zw );
 167     mat[5]  = 1 - 2 * ( xx + zz );
 168     mat[9]  =     2 * ( yz - xw );
 169
 170     mat[2]  =     2 * ( xz - yw );
 171     mat[6]  =     2 * ( yz + xw );
 172     mat[10] = 1 - 2 * ( xx + yy );
 173
 174     mat[3] = v[0];
 175     mat[7] = v[1];
 176     mat[11] = v[2];
 177 }
 178 #endif