2 gph-math - math library for graphics programs
3 Copyright (C) 2016 John Tsiombikas <nuclear@member.fsf.org>
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.
10 inline Quaternion operator -(const Quaternion &q)
12 return Quaternion(-q.x, -q.y, -q.z, -q.w);
15 inline Quaternion operator +(const Quaternion &a, const Quaternion &b)
17 return Quaternion(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
20 inline Quaternion operator -(const Quaternion &a, const Quaternion &b)
22 return Quaternion(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
25 inline Quaternion operator *(const Quaternion &a, const Quaternion &b)
27 Vector3 a_im = Vector3(a.x, a.y, a.z);
28 Vector3 b_im = Vector3(b.x, b.y, b.z);
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);
35 inline Quaternion &operator +=(Quaternion &a, const Quaternion &b)
44 inline Quaternion &operator -=(Quaternion &a, const Quaternion &b)
53 inline Quaternion &operator *=(Quaternion &a, const Quaternion &b)
55 Vector3 a_im = Vector3(a.x, a.y, a.z);
56 Vector3 b_im = Vector3(b.x, b.y, b.z);
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);
64 inline float length(const Quaternion &q)
66 return (float)sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
69 inline float length_sq(const Quaternion &q)
71 return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
74 inline void Quaternion::normalize()
76 float len = length(*this);
85 inline Quaternion normalize(const Quaternion &q)
87 float len = length(q);
89 return Quaternion(q.x / len, q.y / len, q.z / len, q.w / len);
94 inline void Quaternion::conjugate()
101 inline Quaternion conjugate(const Quaternion &q)
103 return Quaternion(-q.x, -q.y, -q.z, q.w);
106 inline void Quaternion::invert()
108 Quaternion conj = gph::conjugate(*this);
109 float len_sq = length_sq(conj);
118 inline Quaternion inverse(const Quaternion &q)
120 Quaternion conj = conjugate(q);
121 float len_sq = length_sq(conj);
123 return Quaternion(conj.x / len_sq, conj.y / len_sq, conj.z / len_sq, conj.w / len_sq);
128 inline void Quaternion::set_rotation(const Vector3 &axis, float angle)
130 float half_angle = angle * 0.5f;
132 float sin_ha = sin(half_angle);
138 inline void Quaternion::rotate(const Vector3 &axis, float angle)
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;
150 inline void Quaternion::rotate(const Quaternion &rq)
152 *this = rq * *this * gph::conjugate(rq);
155 inline Matrix4x4 Quaternion::calc_matrix() const
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;
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,
171 inline Quaternion slerp(const Quaternion &quat1, const Quaternion &q2, float t)
173 Quaternion q1 = quat1;
174 float dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
177 /* make sure we interpolate across the shortest arc */
182 /* clamp dot to [-1, 1] in order to avoid domain errors in acos due to
183 * floating point imprecisions
185 if(dot < -1.0) dot = -1.0;
186 if(dot > 1.0) dot = 1.0;
188 float angle = acos(dot);
191 float sin_angle = sin(angle);
192 if(sin_angle == 0.0f) {
193 // use linear interpolation to avoid div/zero
197 a = sin((1.0f - t) * angle) / sin_angle;
198 b = sin(t * angle) / sin_angle;
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;
206 return Quaternion(x, y, z, w);
209 inline Quaternion lerp(const Quaternion &a, const Quaternion &b, float t)
211 return slerp(a, b, t);