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