git.mutantstargoat.com Git - gph-math/blob - src/quat.inl

   1 inline Quaternion operator -(const Quaternion &q)
   2 {
   3         return Quaternion(-q.x, -q.y, -q.z, -q.w);
   4 }
   5
   6 inline Quaternion operator +(const Quaternion &a, const Quaternion &b)
   7 {
   8         return Quaternion(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
   9 }
  10
  11 inline Quaternion operator -(const Quaternion &a, const Quaternion &b)
  12 {
  13         return Quaternion(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
  14 }
  15
  16 inline Quaternion operator *(const Quaternion &a, const Quaternion &b)
  17 {
  18         Vector3 a_im = Vector3(a.x, a.y, a.z);
  19         Vector3 b_im = Vector3(b.x, b.y, b.z);
  20
  21         float w = a.w * b.w - dot(a_im, b_im);
  22         Vector3 im = a.w * b_im + b.w * a_im + cross(a_im, b_im);
  23         return Quaternion(im.x, im.y, im.z, w);
  24 }
  25
  26 inline Quaternion &operator +=(Quaternion &a, const Quaternion &b)
  27 {
  28         a.x += b.x;
  29         a.y += b.y;
  30         a.z += b.z;
  31         a.w += b.w;
  32         return a;
  33 }
  34
  35 inline Quaternion &operator -=(Quaternion &a, const Quaternion &b)
  36 {
  37         a.x -= b.x;
  38         a.y -= b.y;
  39         a.z -= b.z;
  40         a.w -= b.w;
  41         return a;
  42 }
  43
  44 inline Quaternion &operator *=(Quaternion &a, const Quaternion &b)
  45 {
  46         Vector3 a_im = Vector3(a.x, a.y, a.z);
  47         Vector3 b_im = Vector3(b.x, b.y, b.z);
  48
  49         float w = a.w * b.w - dot(a_im, b_im);
  50         Vector3 im = a.w * b_im + b.w * a_im + cross(a_im, b_im);
  51         a = Quaternion(im.x, im.y, im.z, w);
  52         return a;
  53 }
  54
  55 inline float length(const Quaternion &q)
  56 {
  57         return (float)sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
  58 }
  59
  60 inline float length_sq(const Quaternion &q)
  61 {
  62         return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
  63 }
  64
  65 inline void Quaternion::normalize()
  66 {
  67         float len = length(*this);
  68         if(len != 0.0f) {
  69                 x /= len;
  70                 y /= len;
  71                 z /= len;
  72                 w /= len;
  73         }
  74 }
  75
  76 inline Quaternion normalize(const Quaternion &q)
  77 {
  78         float len = length(q);
  79         if(len != 0.0f) {
  80                 return Quaternion(q.x / len, q.y / len, q.z / len, q.w / len);
  81         }
  82         return q;
  83 }
  84
  85 inline void Quaternion::conjugate()
  86 {
  87         x = -x;
  88         y = -y;
  89         z = -z;
  90 }
  91
  92 inline Quaternion conjugate(const Quaternion &q)
  93 {
  94         return Quaternion(-q.x, -q.y, -q.z, q.w);
  95 }
  96
  97 inline void Quaternion::invert()
  98 {
  99         Quaternion conj = gph::conjugate(*this);
 100         float len_sq = length_sq(conj);
 101         if(len_sq != 0.0) {
 102                 x = conj.x / len_sq;
 103                 y = conj.y / len_sq;
 104                 z = conj.z / len_sq;
 105                 w = conj.w / len_sq;
 106         }
 107 }
 108
 109 inline Quaternion inverse(const Quaternion &q)
 110 {
 111         Quaternion conj = conjugate(q);
 112         float len_sq = length_sq(conj);
 113         if(len_sq != 0.0) {
 114                 return Quaternion(conj.x / len_sq, conj.y / len_sq, conj.z / len_sq, conj.w / len_sq);
 115         }
 116         return q;
 117 }
 118
 119 inline void Quaternion::set_rotation(const Vector3 &axis, float angle)
 120 {
 121         float half_angle = angle * 0.5f;
 122         w = cos(half_angle);
 123         float sin_ha = sin(half_angle);
 124         x = axis.x * sin_ha;
 125         y = axis.y * sin_ha;
 126         z = axis.z * sin_ha;
 127 }
 128
 129 inline void Quaternion::rotate(const Vector3 &axis, float angle)
 130 {
 131         Quaternion q;
 132         float half_angle = angle * 0.5f;
 133         q.w = cos(half_angle);
 134         float sin_ha = sin(half_angle);
 135         q.x = axis.x * sin_ha;
 136         q.y = axis.y * sin_ha;
 137         q.z = axis.z * sin_ha;
 138         *this *= q;
 139 }
 140
 141 inline void Quaternion::rotate(const Quaternion &rq)
 142 {
 143         *this = rq * *this * gph::conjugate(rq);
 144 }
 145
 146 inline Matrix4x4 Quaternion::calc_matrix() const
 147 {
 148         float xsq2 = 2.0 * x * x;
 149         float ysq2 = 2.0 * y * y;
 150         float zsq2 = 2.0 * z * z;
 151         float sx = 1.0 - ysq2 - zsq2;
 152         float sy = 1.0 - xsq2 - zsq2;
 153         float sz = 1.0 - xsq2 - ysq2;
 154
 155         return Matrix4x4(
 156                         sx,     2.0 * x * y - 2.0 * w * z, 2.0 * z * x + 2.0 * w * y, 0,
 157                         2.0 * x * y + 2.0 * w * z, sy, 2.0 * y * z - 2.0 * w * x, 0,
 158                         2.0 * z * x - 2.0 * w * y, 2.0 * y * z + 2.0 * w * x, sz, 0,
 159                         0, 0, 0, 1);
 160 }
 161
 162 inline Quaternion slerp(const Quaternion &quat1, const Quaternion &q2, float t)
 163 {
 164         Quaternion q1 = quat1;
 165         float dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
 166
 167         if(dot < 0.0) {
 168                 /* make sure we interpolate across the shortest arc */
 169                 q1 = -quat1;
 170                 dot = -dot;
 171         }
 172
 173         /* clamp dot to [-1, 1] in order to avoid domain errors in acos due to
 174          * floating point imprecisions
 175          */
 176         if(dot < -1.0) dot = -1.0;
 177         if(dot > 1.0) dot = 1.0;
 178
 179         float angle = acos(dot);
 180         float a, b;
 181
 182         float sin_angle = sin(angle);
 183         if(sin_angle == 0.0f) {
 184                 // use linear interpolation to avoid div/zero
 185                 a = 1.0f - t;
 186                 b = t;
 187         } else {
 188                 a = sin((1.0f - t) * angle) / sin_angle;
 189                 b = sin(t * angle) / sin_angle;
 190         }
 191
 192         float x = q1.x * a + q2.x * b;
 193         float y = q1.y * a + q2.y * b;
 194         float z = q1.z * a + q2.z * b;
 195         float w = q1.w * a + q2.w * b;
 196
 197         return Quaternion(x, y, z, w);
 198 }
 199
 200 inline Quaternion lerp(const Quaternion &a, const Quaternion &b, float t)
 201 {
 202         return slerp(a, b, t);
 203 }