remove RSA's md4.c, replace by DP's
[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_for_x(double angle)
64 {
65   angle *= 0.5;
66   return Quaternion(static_cast<float>(sin(angle)), 0, 0, static_cast<float>(cos(angle)));
67 }
68
69 inline Quaternion quaternion_for_y(double angle)
70 {
71   angle *= 0.5;
72   return Quaternion(0, static_cast<float>(sin(angle)), 0, static_cast<float>(cos(angle)));
73 }
74
75 inline Quaternion quaternion_for_z(double angle)
76 {
77   angle *= 0.5;
78   return Quaternion(0, 0, static_cast<float>(sin(angle)), static_cast<float>(cos(angle)));
79 }
80
81 inline Quaternion quaternion_inverse(const Quaternion& quaternion)
82 {
83   return Quaternion(vector3_negated(vector4_to_vector3(quaternion)), quaternion[3]);
84 }
85
86 inline void quaternion_conjugate(Quaternion& quaternion)
87 {
88   quaternion = quaternion_inverse(quaternion);
89 }
90
91 inline Quaternion quaternion_normalised(const Quaternion& quaternion)
92 {
93   const double n = (1.0 / (quaternion[0] * quaternion[0] + quaternion[1] * quaternion[1] + quaternion[2] * quaternion[2] + quaternion[3] * quaternion[3]));
94   return Quaternion(
95     static_cast<float>(quaternion[0] * n),
96     static_cast<float>(quaternion[1] * n),
97     static_cast<float>(quaternion[2] * n),
98     static_cast<float>(quaternion[3] * n)
99   );
100 }
101
102 inline void quaternion_normalise(Quaternion& quaternion)
103 {
104   quaternion = quaternion_normalised(quaternion);
105 }
106
107 /// \brief Constructs a pure-rotation matrix from \p quaternion.
108 inline Matrix4 matrix4_rotation_for_quaternion(const Quaternion& quaternion)
109 {
110 #if 0
111   const double xx = quaternion[0] * quaternion[0];
112   const double xy = quaternion[0] * quaternion[1];
113   const double xz = quaternion[0] * quaternion[2];
114   const double xw = quaternion[0] * quaternion[3];
115
116   const double yy = quaternion[1] * quaternion[1];
117   const double yz = quaternion[1] * quaternion[2];
118   const double yw = quaternion[1] * quaternion[3];
119
120   const double zz = quaternion[2] * quaternion[2];
121   const double zw = quaternion[2] * quaternion[3];
122
123   return Matrix4(
124     static_cast<float>( 1 - 2 * ( yy + zz ) ),
125     static_cast<float>(     2 * ( xy + zw ) ),
126     static_cast<float>(     2 * ( xz - yw ) ),
127     0,
128     static_cast<float>(     2 * ( xy - zw ) ),
129     static_cast<float>( 1 - 2 * ( xx + zz ) ),
130     static_cast<float>(     2 * ( yz + xw ) ),
131     0,
132     static_cast<float>(     2 * ( xz + yw ) ),
133     static_cast<float>(     2 * ( yz - xw ) ),
134     static_cast<float>( 1 - 2 * ( xx + yy ) ),
135     0,
136     0,
137     0,
138     0,
139     1
140   );
141
142 #else
143   const double x2 = quaternion[0] + quaternion[0];
144   const double y2 = quaternion[1] + quaternion[1]; 
145   const double z2 = quaternion[2] + quaternion[2];
146   const double xx = quaternion[0] * x2;
147   const double xy = quaternion[0] * y2;
148   const double xz = quaternion[0] * z2;
149   const double yy = quaternion[1] * y2;
150   const double yz = quaternion[1] * z2;
151   const double zz = quaternion[2] * z2;
152   const double wx = quaternion[3] * x2;
153   const double wy = quaternion[3] * y2;
154   const double wz = quaternion[3] * z2;
155
156   return Matrix4(
157     static_cast<float>( 1.0 - (yy + zz) ),
158     static_cast<float>(xy + wz),
159     static_cast<float>(xz - wy),
160     0,
161     static_cast<float>(xy - wz),
162     static_cast<float>( 1.0 - (xx + zz) ),
163     static_cast<float>(yz + wx),
164     0,
165     static_cast<float>(xz + wy),
166     static_cast<float>(yz - wx),
167     static_cast<float>( 1.0 - (xx + yy) ),
168     0,
169     0,
170     0,
171     0,
172     1
173   );
174
175 #endif
176 }
177
178 const double c_half_sqrt2 = 0.70710678118654752440084436210485;
179 const float c_half_sqrt2f = static_cast<float>(c_half_sqrt2);
180
181 inline bool quaternion_component_is_90(float component)
182 {
183   return (fabs(component) - c_half_sqrt2) < 0.001;
184 }
185
186 inline Matrix4 matrix4_rotation_for_quaternion_quantised(const Quaternion& quaternion)
187 {
188   if(quaternion.y() == 0
189     && quaternion.z() == 0
190     && quaternion_component_is_90(quaternion.x())
191     && quaternion_component_is_90(quaternion.w()))
192   {
193     return matrix4_rotation_for_sincos_x((quaternion.x() > 0) ? 1.f : -1.f, 0);
194   }
195
196   if(quaternion.x() == 0
197     && quaternion.z() == 0
198     && quaternion_component_is_90(quaternion.y())
199     && quaternion_component_is_90(quaternion.w()))
200   {
201     return matrix4_rotation_for_sincos_y((quaternion.y() > 0) ? 1.f : -1.f, 0);
202   }
203
204   if(quaternion.x() == 0
205     && quaternion.y() == 0
206     && quaternion_component_is_90(quaternion.z())
207     && quaternion_component_is_90(quaternion.w()))
208   {
209     return matrix4_rotation_for_sincos_z((quaternion.z() > 0) ? 1.f : -1.f, 0);
210   }
211
212   return matrix4_rotation_for_quaternion(quaternion);
213 }
214
215 inline Quaternion quaternion_for_matrix4_rotation(const Matrix4& matrix4)
216 {
217   Matrix4 transposed = matrix4_transposed(matrix4);
218
219   double trace = transposed[0] + transposed[5] + transposed[10] + 1.0;
220
221   if(trace > 0.0001)
222   {
223     double S = 0.5 / sqrt(trace);
224     return Quaternion(
225       static_cast<float>((transposed[9] - transposed[6]) * S),
226       static_cast<float>((transposed[2] - transposed[8]) * S),
227       static_cast<float>((transposed[4] - transposed[1]) * S),
228       static_cast<float>(0.25 / S)
229     );
230   }
231
232   if(transposed[0] >= transposed[5] && transposed[0] >= transposed[10])
233   {
234     double S = 2.0 * sqrt(1.0 + transposed[0] - transposed[5] - transposed[10]);
235     return Quaternion(
236       static_cast<float>(0.25 / S),
237       static_cast<float>((transposed[1] + transposed[4]) / S),
238       static_cast<float>((transposed[2] + transposed[8]) / S),
239       static_cast<float>((transposed[6] + transposed[9]) / S)
240     );
241   }
242   
243   if(transposed[5] >= transposed[0] && transposed[5] >= transposed[10])
244   {
245     double S = 2.0 * sqrt(1.0 + transposed[5] - transposed[0] - transposed[10]);
246     return Quaternion(
247       static_cast<float>((transposed[1] + transposed[4]) / S),
248       static_cast<float>(0.25 / S),
249       static_cast<float>((transposed[6] + transposed[9]) / S),
250       static_cast<float>((transposed[2] + transposed[8]) / S)
251     );
252   }
253
254   double S = 2.0 * sqrt(1.0 + transposed[10] - transposed[0] - transposed[5]);
255   return Quaternion(
256     static_cast<float>((transposed[2] + transposed[8]) / S),
257     static_cast<float>((transposed[6] + transposed[9]) / S),
258     static_cast<float>(0.25 / S),
259     static_cast<float>((transposed[1] + transposed[4]) / S)
260   );
261 }
262
263 /// \brief Returns \p self concatenated with the rotation transform produced by \p rotation.
264 /// The concatenated rotation occurs before \p self.
265 inline Matrix4 matrix4_rotated_by_quaternion(const Matrix4& self, const Quaternion& rotation)
266 {
267   return matrix4_multiplied_by_matrix4(self, matrix4_rotation_for_quaternion(rotation));
268 }
269
270 /// \brief Concatenates \p self with the rotation transform produced by \p rotation.
271 /// The concatenated rotation occurs before \p self.
272 inline void matrix4_rotate_by_quaternion(Matrix4& self, const Quaternion& rotation)
273 {
274   self = matrix4_rotated_by_quaternion(self, rotation);
275 }
276
277 /// \brief Rotates \p self by \p rotation, using \p pivotpoint.
278 inline void matrix4_pivoted_rotate_by_quaternion(Matrix4& self, const Quaternion& rotation, const Vector3& pivotpoint)
279 {
280   matrix4_translate_by_vec3(self, pivotpoint);
281   matrix4_rotate_by_quaternion(self, rotation);
282   matrix4_translate_by_vec3(self, vector3_negated(pivotpoint));
283 }
284
285 inline Vector3 quaternion_transformed_point(const Quaternion& quaternion, const Vector3& point)
286 {
287   double xx = quaternion.x() * quaternion.x();
288   double yy = quaternion.y() * quaternion.y();
289   double zz = quaternion.z() * quaternion.z();
290   double ww = quaternion.w() * quaternion.w();
291
292   double xy2 = quaternion.x() * quaternion.y() * 2;
293   double xz2 = quaternion.x() * quaternion.z() * 2;
294   double xw2 = quaternion.x() * quaternion.w() * 2;
295   double yz2 = quaternion.y() * quaternion.z() * 2;
296   double yw2 = quaternion.y() * quaternion.w() * 2;
297   double zw2 = quaternion.z() * quaternion.w() * 2;
298
299         return Vector3(
300     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()),
301     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()),
302     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())
303   );
304 }
305
306 /// \brief Constructs a pure-rotation transform from \p axis and \p angle (radians).
307 inline Matrix4 matrix4_rotation_for_axisangle(const Vector3& axis, double angle)
308 {
309   return matrix4_rotation_for_quaternion(quaternion_for_axisangle(axis, angle));
310 }
311
312 /// \brief Rotates \p self about \p axis by \p angle.
313 inline void matrix4_rotate_by_axisangle(Matrix4& self, const Vector3& axis, double angle)
314 {
315   matrix4_multiply_by_matrix4(self, matrix4_rotation_for_axisangle(axis, angle));
316 }
317
318 /// \brief Rotates \p self about \p axis by \p angle using \p pivotpoint.
319 inline void matrix4_pivoted_rotate_by_axisangle(Matrix4& self, const Vector3& axis, double angle, const Vector3& pivotpoint)
320 {
321   matrix4_translate_by_vec3(self, pivotpoint);
322   matrix4_rotate_by_axisangle(self, axis, angle);
323   matrix4_translate_by_vec3(self, vector3_negated(pivotpoint));
324 }
325
326
327 #endif