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