git.mutantstargoat.com Git - nexus3d/blob - src/cgmath2/cgmquat.inl

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