git.mutantstargoat.com Git - gph-cmath/blob - src/cgmquat.inl

   1 static inline void cgm_qcons(cgm_quat *q, float x, float y, float z, float w)
   2 {
   3         q->x = x;
   4         q->y = y;
   5         q->z = z;
   6         q->w = w;
   7 }
   8
   9
  10 static inline void cgm_qneg(cgm_quat *q)
  11 {
  12         q->x = -q->x;
  13         q->y = -q->y;
  14         q->z = -q->z;
  15         q->w = -q->w;
  16 }
  17
  18 static inline void cgm_qadd(cgm_quat *a, const cgm_quat *b)
  19 {
  20         a->x += b->x;
  21         a->y += b->y;
  22         a->z += b->z;
  23         a->w += b->w;
  24 }
  25
  26 static inline void cgm_qsub(cgm_quat *a, const cgm_quat *b)
  27 {
  28         a->x -= b->x;
  29         a->y -= b->y;
  30         a->z -= b->z;
  31         a->w -= b->w;
  32 }
  33
  34 static inline void cgm_qmul(cgm_quat *a, const cgm_quat *b)
  35 {
  36         float x, y, z, dot;
  37         cgm_vec3 cross;
  38
  39         dot = a->x * b->x + a->y * b->y + a->z * b->z;
  40         cgm_vcross(&cross, (cgm_vec3*)a, (cgm_vec3*)b);
  41
  42         x = a->w * b->x + b->w * a->x + cross.x;
  43         y = a->w * b->y + b->w * a->y + cross.y;
  44         z = a->w * b->z + b->w * a->z + cross.z;
  45         a->w = a->w * b->w - dot;
  46         a->x = x;
  47         a->y = y;
  48         a->z = z;
  49 }
  50
  51 static inline float cgm_qlength(const cgm_quat *q)
  52 {
  53         return sqrt(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w);
  54 }
  55
  56 static inline float cgm_qlength_sq(const cgm_quat *q)
  57 {
  58         return q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w;
  59 }
  60
  61 static inline void cgm_qnormalize(cgm_quat *q)
  62 {
  63         float len = cgm_qlength(q);
  64         if(len != 0.0f) {
  65                 float s = 1.0f / len;
  66                 q->x *= s;
  67                 q->y *= s;
  68                 q->z *= s;
  69                 q->w *= s;
  70         }
  71 }
  72
  73 static inline void cgm_qconjugate(cgm_quat *q)
  74 {
  75         q->x = -q->x;
  76         q->y = -q->y;
  77         q->z = -q->z;
  78 }
  79
  80 static inline void cgm_qinvert(cgm_quat *q)
  81 {
  82         float len_sq = cgm_qlength_sq(q);
  83         cgm_qconjugate(q);
  84         if(len_sq != 0.0f) {
  85                 float s = 1.0f / len_sq;
  86                 q->x *= s;
  87                 q->y *= s;
  88                 q->z *= s;
  89                 q->w *= s;
  90         }
  91 }
  92
  93 static inline void cgm_qrotation(cgm_quat *q, const cgm_vec3 *axis, float angle)
  94 {
  95         float hangle = angle * 0.5f;
  96         float sin_ha = sin(hangle);
  97         q->w = cos(hangle);
  98         q->x = axis->x * sin_ha;
  99         q->y = axis->y * sin_ha;
 100         q->z = axis->z * sin_ha;
 101 }
 102
 103 static inline void cgm_qrotate(cgm_quat *q, const cgm_vec3 *axis, float angle)
 104 {
 105         cgm_quat qrot;
 106         cgm_qrotation(&qrot, axis, angle);
 107         cgm_qmul(q, &qrot);
 108 }
 109
 110 static inline void cgm_qslerp(cgm_quat *res, const cgm_quat *quat1, const cgm_quat *q2, float t)
 111 {
 112         float angle, dot, a, b, sin_angle;
 113         cgm_quat q1 = *quat1;
 114
 115         dot = quat1->x * q2->x + quat1->y * q2->y + quat1->z * q2->z + quat1->w * q2->w;
 116         if(dot < 0.0f) {
 117                 /* make sure we inteprolate across the shortest arc */
 118                 cgm_qneg(&q1);
 119                 dot = -dot;
 120         }
 121
 122         /* clamp dot to [-1, 1] in order to avoid domain errors in acos due to
 123          * floating point imprecisions
 124          */
 125         if(dot < -1.0f) dot = -1.0f;
 126         if(dot > 1.0f) dot = 1.0f;
 127         angle = acos(dot);
 128
 129         sin_angle = sin(angle);
 130         if(sin_angle == 0.0f) {
 131                 /* use linear interpolation to avoid div/zero */
 132                 a = 1.0f;
 133                 b = t;
 134         } else {
 135                 a = sin((1.0f - t) * angle) / sin_angle;
 136                 b = sin(t * angle) / sin_angle;
 137         }
 138
 139         res->x = q1.x * a + q2->x * b;
 140         res->y = q1.y * a + q2->y * b;
 141         res->z = q1.z * a + q2->z * b;
 142         res->w = q1.w * a + q2->w * b;
 143 }
 144
 145 static inline void cgm_qlerp(cgm_quat *res, const cgm_quat *a, const cgm_quat *b, float t)
 146 {
 147         res->x = a->x + (b->x - a->x) * t;
 148         res->y = a->y + (b->y - a->y) * t;
 149         res->z = a->z + (b->z - a->z) * t;
 150         res->w = a->w + (b->w - a->w) * t;
 151 }