1 inline Quaternion operator -(const Quaternion &q)
3 return Quaternion(-q.x, -q.y, -q.z, -q.w);
6 inline Quaternion operator +(const Quaternion &a, const Quaternion &b)
8 return Quaternion(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
11 inline Quaternion operator -(const Quaternion &a, const Quaternion &b)
13 return Quaternion(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
16 inline Quaternion operator *(const Quaternion &a, const Quaternion &b)
18 Vector3 a_im = Vector3(a.x, a.y, a.z);
19 Vector3 b_im = Vector3(b.x, b.y, b.z);
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);
26 inline Quaternion &operator +=(Quaternion &a, const Quaternion &b)
35 inline Quaternion &operator -=(Quaternion &a, const Quaternion &b)
44 inline Quaternion &operator *=(Quaternion &a, const Quaternion &b)
46 Vector3 a_im = Vector3(a.x, a.y, a.z);
47 Vector3 b_im = Vector3(b.x, b.y, b.z);
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);
55 inline float length(const Quaternion &q)
57 return (float)sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
60 inline float length_sq(const Quaternion &q)
62 return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
65 inline void Quaternion::normalize()
67 float len = length(*this);
76 inline Quaternion normalize(const Quaternion &q)
78 float len = length(q);
80 return Quaternion(q.x / len, q.y / len, q.z / len, q.w / len);
85 inline void Quaternion::conjugate()
92 inline Quaternion conjugate(const Quaternion &q)
94 return Quaternion(-q.x, -q.y, -q.z, q.w);
97 inline void Quaternion::invert()
99 Quaternion conj = gph::conjugate(*this);
100 float len_sq = length_sq(conj);
109 inline Quaternion inverse(const Quaternion &q)
111 Quaternion conj = conjugate(q);
112 float len_sq = length_sq(conj);
114 return Quaternion(conj.x / len_sq, conj.y / len_sq, conj.z / len_sq, conj.w / len_sq);
119 inline void Quaternion::set_rotation(const Vector3 &axis, float angle)
121 float half_angle = angle * 0.5f;
123 float sin_ha = sin(half_angle);
129 inline void Quaternion::rotate(const Vector3 &axis, float angle)
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;
141 inline void Quaternion::rotate(const Quaternion &rq)
143 *this = rq * *this * gph::conjugate(rq);
146 inline Matrix4x4 Quaternion::calc_matrix() const
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;
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,
162 inline Quaternion slerp(const Quaternion &quat1, const Quaternion &q2, float t)
164 Quaternion q1 = quat1;
165 float dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
168 /* make sure we interpolate across the shortest arc */
173 /* clamp dot to [-1, 1] in order to avoid domain errors in acos due to
174 * floating point imprecisions
176 if(dot < -1.0) dot = -1.0;
177 if(dot > 1.0) dot = 1.0;
179 float angle = acos(dot);
182 float sin_angle = sin(angle);
183 if(sin_angle == 0.0f) {
184 // use linear interpolation to avoid div/zero
188 a = sin((1.0f - t) * angle) / sin_angle;
189 b = sin(t * angle) / sin_angle;
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;
197 return Quaternion(x, y, z, w);
200 inline Quaternion lerp(const Quaternion &a, const Quaternion &b, float t)
202 return slerp(a, b, t);