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

   1 #ifndef VMATH_H_
   2 #define VMATH_H_
   3
   4 #ifdef __GNUC__
   5 #define INLINE __inline
   6
   7 #elif defined(__WATCOMC__)
   8 #define INLINE __inline
   9
  10 #else
  11 #define INLINE
  12 #endif
  13
  14 typedef struct { float x, y, z; } vec3_t;
  15 typedef struct { float x, y, z, w; } vec4_t;
  16
  17 typedef vec4_t quat_t;
  18
  19 /* vector functions */
  20 static INLINE vec3_t v3_cons(float x, float y, float z)
  21 {
  22         vec3_t res;
  23         res.x = x;
  24         res.y = y;
  25         res.z = z;
  26         return res;
  27 }
  28
  29 static INLINE float v3_dot(vec3_t v1, vec3_t v2)
  30 {
  31         return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
  32 }
  33
  34 /* quaternion functions */
  35 static INLINE quat_t quat_cons(float s, float x, float y, float z)
  36 {
  37         quat_t q;
  38         q.x = x;
  39         q.y = y;
  40         q.z = z;
  41         q.w = s;
  42         return q;
  43 }
  44
  45 static INLINE vec3_t quat_vec(quat_t q)
  46 {
  47         vec3_t v;
  48         v.x = q.x;
  49         v.y = q.y;
  50         v.z = q.z;
  51         return v;
  52 }
  53
  54 static INLINE quat_t quat_mul(quat_t q1, quat_t q2)
  55 {
  56         quat_t res;
  57         vec3_t v1 = quat_vec(q1);
  58         vec3_t v2 = quat_vec(q2);
  59
  60         res.w = q1.w * q2.w - v3_dot(v1, v2);
  61         res.x = v2.x * q1.w + v1.x * q2.w + (v1.y * v2.z - v1.z * v2.y);
  62         res.y = v2.y * q1.w + v1.y * q2.w + (v1.z * v2.x - v1.x * v2.z);
  63         res.z = v2.z * q1.w + v1.z * q2.w + (v1.x * v2.y - v1.y * v2.x);
  64         return res;
  65 }
  66
  67 static INLINE void quat_to_mat(float *res, quat_t q)
  68 {
  69         res[0] = 1.0f - 2.0f * q.y*q.y - 2.0f * q.z*q.z;
  70         res[1] = 2.0f * q.x * q.y - 2.0f * q.w * q.z;
  71         res[2] = 2.0f * q.z * q.x + 2.0f * q.w * q.y;
  72         res[3] = 0.0f;
  73         res[4] = 2.0f * q.x * q.y + 2.0f * q.w * q.z;
  74         res[5] = 1.0f - 2.0f * q.x*q.x - 2.0f * q.z*q.z;
  75         res[6] = 2.0f * q.y * q.z - 2.0f * q.w * q.x;
  76         res[7] = 0.0f;
  77         res[8] = 2.0f * q.z * q.x - 2.0f * q.w * q.y;
  78         res[9] = 2.0f * q.y * q.z + 2.0f * q.w * q.x;
  79         res[10] = 1.0f - 2.0f * q.x*q.x - 2.0f * q.y*q.y;
  80         res[11] = 0.0f;
  81         res[12] = res[13] = res[14] = 0.0f;
  82         res[15] = 1.0f;
  83 }
  84
  85 static INLINE quat_t quat_rotate(quat_t q, float angle, float x, float y, float z)
  86 {
  87         quat_t rq;
  88         float half_angle = angle * 0.5;
  89         float sin_half = sin(half_angle);
  90
  91         rq.w = cos(half_angle);
  92         rq.x = x * sin_half;
  93         rq.y = y * sin_half;
  94         rq.z = z * sin_half;
  95
  96         return quat_mul(q, rq);
  97 }
  98
  99 #endif  /* VMATH_H_ */