7bd42cb13f1091c88e2b271322fb7a060e3a7a68
[xonotic/netradiant.git] / libs / math / quaternion.h
1 /*
2 Copyright (C) 2001-2006, William Joseph.
3 All Rights Reserved.
4
5 This file is part of GtkRadiant.
6
7 GtkRadiant is free software; you can redistribute it and/or modify
8 it under the terms of the GNU General Public License as published by
9 the Free Software Foundation; either version 2 of the License, or
10 (at your option) any later version.
11
12 GtkRadiant is distributed in the hope that it will be useful,
13 but WITHOUT ANY WARRANTY; without even the implied warranty of
14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15 GNU General Public License for more details.
16
17 You should have received a copy of the GNU General Public License
18 along with GtkRadiant; if not, write to the Free Software
19 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
20 */
21
22 #if !defined(INCLUDED_MATH_QUATERNION_H)
23 #define INCLUDED_MATH_QUATERNION_H
24
25 /// \file
26 /// \brief Quaternion data types and related operations.
27
28 #include "math/matrix.h"
29
30 /// \brief A quaternion stored in single-precision floating-point.
31 typedef Vector4 Quaternion;
32
33 const Quaternion c_quaternion_identity(0, 0, 0, 1);
34
35 inline Quaternion quaternion_multiplied_by_quaternion(const Quaternion& quaternion, const Quaternion& other)
36 {
37   return Quaternion(
38     quaternion[3]*other[0] + quaternion[0]*other[3] + quaternion[1]*other[2] - quaternion[2]*other[1],
39     quaternion[3]*other[1] + quaternion[1]*other[3] + quaternion[2]*other[0] - quaternion[0]*other[2],
40     quaternion[3]*other[2] + quaternion[2]*other[3] + quaternion[0]*other[1] - quaternion[1]*other[0],
41     quaternion[3]*other[3] - quaternion[0]*other[0] - quaternion[1]*other[1] - quaternion[2]*other[2]
42   );
43 }
44
45 inline void quaternion_multiply_by_quaternion(Quaternion& quaternion, const Quaternion& other)
46 {
47   quaternion = quaternion_multiplied_by_quaternion(quaternion, other);
48 }
49
50 /// \brief Constructs a quaternion which rotates between two points on the unit-sphere, \p from and \p to.
51 inline Quaternion quaternion_for_unit_vectors(const Vector3& from, const Vector3& to)
52 {
53   return Quaternion(vector3_cross(from, to), static_cast<float>(vector3_dot(from, to)));
54 }
55
56 inline Quaternion quaternion_for_axisangle(const Vector3& axis, double angle)
57 {
58   angle *= 0.5;
59   float sa = static_cast<float>(sin(angle));
60   return Quaternion(axis[0] * sa, axis[1] * sa, axis[2] * sa, static_cast<float>(cos(angle)));
61 }
62
63 inline Quaternion quaternion_inverse(const Quaternion& quaternion)
64 {
65   return Quaternion(vector3_negated(vector4_to_vector3(quaternion)), quaternion[3]);
66 }
67
68 inline void quaternion_conjugate(Quaternion& quaternion)
69 {
70   quaternion = quaternion_inverse(quaternion);
71 }
72
73 inline Quaternion quaternion_normalised(const Quaternion& quaternion)
74 {
75   const double n = (1.0 / (quaternion[0] * quaternion[0] + quaternion[1] * quaternion[1] + quaternion[2] * quaternion[2] + quaternion[3] * quaternion[3]));
76   return Quaternion(
77     static_cast<float>(quaternion[0] * n),
78     static_cast<float>(quaternion[1] * n),
79     static_cast<float>(quaternion[2] * n),
80     static_cast<float>(quaternion[3] * n)
81   );
82 }
83
84 inline void quaternion_normalise(Quaternion& quaternion)
85 {
86   quaternion = quaternion_normalised(quaternion);
87 }
88
89 /// \brief Constructs a pure-rotation matrix from \p quaternion.
90 inline Matrix4 matrix4_rotation_for_quaternion(const Quaternion& quaternion)
91 {
92 #if 0
93   const double xx = quaternion[0] * quaternion[0];
94   const double xy = quaternion[0] * quaternion[1];
95   const double xz = quaternion[0] * quaternion[2];
96   const double xw = quaternion[0] * quaternion[3];
97
98   const double yy = quaternion[1] * quaternion[1];
99   const double yz = quaternion[1] * quaternion[2];
100   const double yw = quaternion[1] * quaternion[3];
101
102   const double zz = quaternion[2] * quaternion[2];
103   const double zw = quaternion[2] * quaternion[3];
104
105   return Matrix4(
106     static_cast<float>( 1 - 2 * ( yy + zz ) ),
107     static_cast<float>(     2 * ( xy + zw ) ),
108     static_cast<float>(     2 * ( xz - yw ) ),
109     0,
110     static_cast<float>(     2 * ( xy - zw ) ),
111     static_cast<float>( 1 - 2 * ( xx + zz ) ),
112     static_cast<float>(     2 * ( yz + xw ) ),
113     0,
114     static_cast<float>(     2 * ( xz + yw ) ),
115     static_cast<float>(     2 * ( yz - xw ) ),
116     static_cast<float>( 1 - 2 * ( xx + yy ) ),
117     0,
118     0,
119     0,
120     0,
121     1
122   );
123
124 #else
125   const double x2 = quaternion[0] + quaternion[0];
126   const double y2 = quaternion[1] + quaternion[1]; 
127   const double z2 = quaternion[2] + quaternion[2];
128   const double xx = quaternion[0] * x2;
129   const double xy = quaternion[0] * y2;
130   const double xz = quaternion[0] * z2;
131   const double yy = quaternion[1] * y2;
132   const double yz = quaternion[1] * z2;
133   const double zz = quaternion[2] * z2;
134   const double wx = quaternion[3] * x2;
135   const double wy = quaternion[3] * y2;
136   const double wz = quaternion[3] * z2;
137
138   return Matrix4(
139     static_cast<float>( 1.0 - (yy + zz) ),
140     static_cast<float>(xy + wz),
141     static_cast<float>(xz - wy),
142     0,
143     static_cast<float>(xy - wz),
144     static_cast<float>( 1.0 - (xx + zz) ),
145     static_cast<float>(yz + wx),
146     0,
147     static_cast<float>(xz + wy),
148     static_cast<float>(yz - wx),
149     static_cast<float>( 1.0 - (xx + yy) ),
150     0,
151     0,
152     0,
153     0,
154     1
155   );
156
157 #endif
158 }
159
160 const double c_half_sqrt2 = 0.70710678118654752440084436210485;
161 const float c_half_sqrt2f = static_cast<float>(c_half_sqrt2);
162
163 inline bool quaternion_component_is_90(float component)
164 {
165   return (fabs(component) - c_half_sqrt2) < 0.001;
166 }
167
168 inline Matrix4 matrix4_rotation_for_quaternion_quantised(const Quaternion& quaternion)
169 {
170   if(quaternion.y() == 0
171     && quaternion.z() == 0
172     && quaternion_component_is_90(quaternion.x())
173     && quaternion_component_is_90(quaternion.w()))
174   {
175     return matrix4_rotation_for_sincos_x((quaternion.x() > 0) ? 1.f : -1.f, 0);
176   }
177
178   if(quaternion.x() == 0
179     && quaternion.z() == 0
180     && quaternion_component_is_90(quaternion.y())
181     && quaternion_component_is_90(quaternion.w()))
182   {
183     return matrix4_rotation_for_sincos_y((quaternion.y() > 0) ? 1.f : -1.f, 0);
184   }
185
186   if(quaternion.x() == 0
187     && quaternion.y() == 0
188     && quaternion_component_is_90(quaternion.z())
189     && quaternion_component_is_90(quaternion.w()))
190   {
191     return matrix4_rotation_for_sincos_z((quaternion.z() > 0) ? 1.f : -1.f, 0);
192   }
193
194   return matrix4_rotation_for_quaternion(quaternion);
195 }
196
197 inline Quaternion quaternion_for_matrix4_rotation(const Matrix4& matrix4)
198 {
199   Matrix4 transposed = matrix4_transposed(matrix4);
200
201   double trace = transposed[0] + transposed[5] + transposed[10] + 1.0;
202
203   if(trace > 0.0001)
204   {
205     double S = 0.5 / sqrt(trace);
206     return Quaternion(
207       static_cast<float>((transposed[9] - transposed[6]) * S),
208       static_cast<float>((transposed[2] - transposed[8]) * S),
209       static_cast<float>((transposed[4] - transposed[1]) * S),
210       static_cast<float>(0.25 / S)
211     );
212   }
213
214   if(transposed[0] >= transposed[5] && transposed[0] >= transposed[10])
215   {
216     double S = 2.0 * sqrt(1.0 + transposed[0] - transposed[5] - transposed[10]);
217     return Quaternion(
218       static_cast<float>(0.25 / S),
219       static_cast<float>((transposed[1] + transposed[4]) / S),
220       static_cast<float>((transposed[2] + transposed[8]) / S),
221       static_cast<float>((transposed[6] + transposed[9]) / S)
222     );
223   }
224   
225   if(transposed[5] >= transposed[0] && transposed[5] >= transposed[10])
226   {
227     double S = 2.0 * sqrt(1.0 + transposed[5] - transposed[0] - transposed[10]);
228     return Quaternion(
229       static_cast<float>((transposed[1] + transposed[4]) / S),
230       static_cast<float>(0.25 / S),
231       static_cast<float>((transposed[6] + transposed[9]) / S),
232       static_cast<float>((transposed[2] + transposed[8]) / S)
233     );
234   }
235
236   double S = 2.0 * sqrt(1.0 + transposed[10] - transposed[0] - transposed[5]);
237   return Quaternion(
238     static_cast<float>((transposed[2] + transposed[8]) / S),
239     static_cast<float>((transposed[6] + transposed[9]) / S),
240     static_cast<float>(0.25 / S),
241     static_cast<float>((transposed[1] + transposed[4]) / S)
242   );
243 }
244
245 /// \brief Returns \p self concatenated with the rotation transform produced by \p rotation.
246 /// The concatenated rotation occurs before \p self.
247 inline Matrix4 matrix4_rotated_by_quaternion(const Matrix4& self, const Quaternion& rotation)
248 {
249   return matrix4_multiplied_by_matrix4(self, matrix4_rotation_for_quaternion(rotation));
250 }
251
252 /// \brief Concatenates \p self with the rotation transform produced by \p rotation.
253 /// The concatenated rotation occurs before \p self.
254 inline void matrix4_rotate_by_quaternion(Matrix4& self, const Quaternion& rotation)
255 {
256   self = matrix4_rotated_by_quaternion(self, rotation);
257 }
258
259 /// \brief Rotates \p self by \p rotation, using \p pivotpoint.
260 inline void matrix4_pivoted_rotate_by_quaternion(Matrix4& self, const Quaternion& rotation, const Vector3& pivotpoint)
261 {
262   matrix4_translate_by_vec3(self, pivotpoint);
263   matrix4_rotate_by_quaternion(self, rotation);
264   matrix4_translate_by_vec3(self, vector3_negated(pivotpoint));
265 }
266
267 inline Vector3 quaternion_transformed_point(const Quaternion& quaternion, const Vector3& point)
268 {
269   double xx = quaternion.x() * quaternion.x();
270   double yy = quaternion.y() * quaternion.y();
271   double zz = quaternion.z() * quaternion.z();
272   double ww = quaternion.w() * quaternion.w();
273
274   double xy2 = quaternion.x() * quaternion.y() * 2;
275   double xz2 = quaternion.x() * quaternion.z() * 2;
276   double xw2 = quaternion.x() * quaternion.w() * 2;
277   double yz2 = quaternion.y() * quaternion.z() * 2;
278   double yw2 = quaternion.y() * quaternion.w() * 2;
279   double zw2 = quaternion.z() * quaternion.w() * 2;
280
281         return Vector3(
282     static_cast<float>(ww * point.x() + yw2 * point.z() - zw2 * point.y() + xx * point.x() + xy2 * point.y() + xz2 * point.z() - zz * point.x() - yy * point.x()),
283     static_cast<float>(xy2 * point.x() + yy * point.y() + yz2 * point.z() + zw2 * point.x() - zz * point.y() + ww * point.y() - xw2 * point.z() - xx * point.y()),
284     static_cast<float>(xz2 * point.x() + yz2 * point.y() + zz * point.z() - yw2 * point.x() - yy * point.z() + xw2 * point.y() - xx * point.z() + ww * point.z())
285   );
286 }
287
288 /// \brief Constructs a pure-rotation transform from \p axis and \p angle (radians).
289 inline Matrix4 matrix4_rotation_for_axisangle(const Vector3& axis, double angle)
290 {
291   return matrix4_rotation_for_quaternion(quaternion_for_axisangle(axis, angle));
292 }
293
294 /// \brief Rotates \p self about \p axis by \p angle.
295 inline void matrix4_rotate_by_axisangle(Matrix4& self, const Vector3& axis, double angle)
296 {
297   matrix4_multiply_by_matrix4(self, matrix4_rotation_for_axisangle(axis, angle));
298 }
299
300 /// \brief Rotates \p self about \p axis by \p angle using \p pivotpoint.
301 inline void matrix4_pivoted_rotate_by_axisangle(Matrix4& self, const Vector3& axis, double angle, const Vector3& pivotpoint)
302 {
303   matrix4_translate_by_vec3(self, pivotpoint);
304   matrix4_rotate_by_axisangle(self, axis, angle);
305   matrix4_translate_by_vec3(self, vector3_negated(pivotpoint));
306 }
307
308
309 #endif