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

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