git.mutantstargoat.com Git - dosdemo/blob - src/vmath.h

   1 #ifndef VMATH_H_
   2 #define VMATH_H_
   3
   4 #include <math.h>
   5
   6 #ifdef __GNUC__
   7 #define INLINE __inline
   8
   9 #elif defined(__WATCOMC__)
  10 #define INLINE __inline
  11
  12 #else
  13 #define INLINE
  14 #endif
  15
  16 typedef struct { float x, y; } vec2_t;
  17 typedef struct { float x, y, z; } vec3_t;
  18 typedef struct { float x, y, z, w; } vec4_t;
  19
  20 typedef vec4_t quat_t;
  21
  22 /* vector functions */
  23 static INLINE vec3_t v3_cons(float x, float y, float z)
  24 {
  25         vec3_t res;
  26         res.x = x;
  27         res.y = y;
  28         res.z = z;
  29         return res;
  30 }
  31
  32 static INLINE void v3_negate(vec3_t *v)
  33 {
  34         v->x = -v->x;
  35         v->y = -v->y;
  36         v->z = -v->z;
  37 }
  38
  39 static INLINE float v3_dot(vec3_t v1, vec3_t v2)
  40 {
  41         return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
  42 }
  43
  44 static INLINE vec3_t v3_cross(vec3_t v1, vec3_t v2)
  45 {
  46         vec3_t res;
  47         res.x = v1.y * v2.z - v1.z * v2.y;
  48         res.y = v1.z * v2.x - v1.x * v2.z;
  49         res.z = v1.x * v2.y - v1.y * v2.x;
  50         return res;
  51 }
  52
  53 static INLINE void v3_normalize(vec3_t *v)
  54 {
  55         float mag = sqrt(v->x * v->x + v->y * v->y + v->z * v->z);
  56         if(mag != 0.0f) {
  57                 float s = 1.0f / mag;
  58                 v->x *= s;
  59                 v->y *= s;
  60                 v->z *= s;
  61         }
  62 }
  63
  64 /* quaternion functions */
  65 static INLINE quat_t quat_cons(float s, float x, float y, float z)
  66 {
  67         quat_t q;
  68         q.x = x;
  69         q.y = y;
  70         q.z = z;
  71         q.w = s;
  72         return q;
  73 }
  74
  75 static INLINE vec3_t quat_vec(quat_t q)
  76 {
  77         vec3_t v;
  78         v.x = q.x;
  79         v.y = q.y;
  80         v.z = q.z;
  81         return v;
  82 }
  83
  84 static INLINE quat_t quat_mul(quat_t q1, quat_t q2)
  85 {
  86         quat_t res;
  87         vec3_t v1 = quat_vec(q1);
  88         vec3_t v2 = quat_vec(q2);
  89
  90         res.w = q1.w * q2.w - v3_dot(v1, v2);
  91         res.x = v2.x * q1.w + v1.x * q2.w + (v1.y * v2.z - v1.z * v2.y);
  92         res.y = v2.y * q1.w + v1.y * q2.w + (v1.z * v2.x - v1.x * v2.z);
  93         res.z = v2.z * q1.w + v1.z * q2.w + (v1.x * v2.y - v1.y * v2.x);
  94         return res;
  95 }
  96
  97 static INLINE void quat_to_mat(float *res, quat_t q)
  98 {
  99         res[0] = 1.0f - 2.0f * q.y*q.y - 2.0f * q.z*q.z;
 100         res[1] = 2.0f * q.x * q.y - 2.0f * q.w * q.z;
 101         res[2] = 2.0f * q.z * q.x + 2.0f * q.w * q.y;
 102         res[3] = 0.0f;
 103         res[4] = 2.0f * q.x * q.y + 2.0f * q.w * q.z;
 104         res[5] = 1.0f - 2.0f * q.x*q.x - 2.0f * q.z*q.z;
 105         res[6] = 2.0f * q.y * q.z - 2.0f * q.w * q.x;
 106         res[7] = 0.0f;
 107         res[8] = 2.0f * q.z * q.x - 2.0f * q.w * q.y;
 108         res[9] = 2.0f * q.y * q.z + 2.0f * q.w * q.x;
 109         res[10] = 1.0f - 2.0f * q.x*q.x - 2.0f * q.y*q.y;
 110         res[11] = 0.0f;
 111         res[12] = res[13] = res[14] = 0.0f;
 112         res[15] = 1.0f;
 113 }
 114
 115 static INLINE quat_t quat_rotate(quat_t q, float angle, float x, float y, float z)
 116 {
 117         quat_t rq;
 118         float half_angle = angle * 0.5;
 119         float sin_half = sin(half_angle);
 120
 121         rq.w = cos(half_angle);
 122         rq.x = x * sin_half;
 123         rq.y = y * sin_half;
 124         rq.z = z * sin_half;
 125
 126         return quat_mul(q, rq);
 127 }
 128
 129 static INLINE void mat4_transpose(float *mat)
 130 {
 131         int i, j;
 132
 133         for(i=0; i<4; i++) {
 134                 for(j=0; j<i; j++) {
 135                         int rowidx = i * 4 + j;
 136                         int colidx = j * 4 + i;
 137                         float tmp = mat[rowidx];
 138                         mat[rowidx] = mat[colidx];
 139                         mat[colidx] = tmp;
 140                 }
 141         }
 142 }
 143
 144 #endif  /* VMATH_H_ */