2 Copyright (C) 2001-2006, William Joseph.
5 This file is part of GtkRadiant.
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.
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.
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
22 #if !defined( INCLUDED_MATH_QUATERNION_H )
23 #define INCLUDED_MATH_QUATERNION_H
26 /// \brief Quaternion data types and related operations.
28 #include "math/matrix.h"
30 /// \brief A quaternion stored in single-precision floating-point.
31 typedef Vector4 Quaternion;
33 const Quaternion c_quaternion_identity( 0, 0, 0, 1 );
35 inline Quaternion quaternion_multiplied_by_quaternion( const Quaternion& quaternion, const Quaternion& other ){
37 quaternion[3] * other[0] + quaternion[0] * other[3] + quaternion[1] * other[2] - quaternion[2] * other[1],
38 quaternion[3] * other[1] + quaternion[1] * other[3] + quaternion[2] * other[0] - quaternion[0] * other[2],
39 quaternion[3] * other[2] + quaternion[2] * other[3] + quaternion[0] * other[1] - quaternion[1] * other[0],
40 quaternion[3] * other[3] - quaternion[0] * other[0] - quaternion[1] * other[1] - quaternion[2] * other[2]
44 inline void quaternion_multiply_by_quaternion( Quaternion& quaternion, const Quaternion& other ){
45 quaternion = quaternion_multiplied_by_quaternion( quaternion, other );
48 /// \brief Constructs a quaternion which rotates between two points on the unit-sphere, \p from and \p to.
49 inline Quaternion quaternion_for_unit_vectors( const Vector3& from, const Vector3& to ){
50 return Quaternion( vector3_cross( from, to ), static_cast<float>( vector3_dot( from, to ) ) );
53 inline Quaternion quaternion_for_axisangle( const Vector3& axis, double angle ){
55 float sa = static_cast<float>( sin( angle ) );
56 return Quaternion( axis[0] * sa, axis[1] * sa, axis[2] * sa, static_cast<float>( cos( angle ) ) );
59 inline Quaternion quaternion_for_x( double angle ){
61 return Quaternion( static_cast<float>( sin( angle ) ), 0, 0, static_cast<float>( cos( angle ) ) );
64 inline Quaternion quaternion_for_y( double angle ){
66 return Quaternion( 0, static_cast<float>( sin( angle ) ), 0, static_cast<float>( cos( angle ) ) );
69 inline Quaternion quaternion_for_z( double angle ){
71 return Quaternion( 0, 0, static_cast<float>( sin( angle ) ), static_cast<float>( cos( angle ) ) );
74 inline Quaternion quaternion_inverse( const Quaternion& quaternion ){
75 return Quaternion( vector3_negated( vector4_to_vector3( quaternion ) ), quaternion[3] );
78 inline void quaternion_conjugate( Quaternion& quaternion ){
79 quaternion = quaternion_inverse( quaternion );
82 inline Quaternion quaternion_normalised( const Quaternion& quaternion ){
83 const double n = ( 1.0 / ( quaternion[0] * quaternion[0] + quaternion[1] * quaternion[1] + quaternion[2] * quaternion[2] + quaternion[3] * quaternion[3] ) );
85 static_cast<float>( quaternion[0] * n ),
86 static_cast<float>( quaternion[1] * n ),
87 static_cast<float>( quaternion[2] * n ),
88 static_cast<float>( quaternion[3] * n )
92 inline void quaternion_normalise( Quaternion& quaternion ){
93 quaternion = quaternion_normalised( quaternion );
96 /// \brief Constructs a pure-rotation matrix from \p quaternion.
97 inline Matrix4 matrix4_rotation_for_quaternion( const Quaternion& quaternion ){
99 const double xx = quaternion[0] * quaternion[0];
100 const double xy = quaternion[0] * quaternion[1];
101 const double xz = quaternion[0] * quaternion[2];
102 const double xw = quaternion[0] * quaternion[3];
104 const double yy = quaternion[1] * quaternion[1];
105 const double yz = quaternion[1] * quaternion[2];
106 const double yw = quaternion[1] * quaternion[3];
108 const double zz = quaternion[2] * quaternion[2];
109 const double zw = quaternion[2] * quaternion[3];
112 static_cast<float>( 1 - 2 * ( yy + zz ) ),
113 static_cast<float>( 2 * ( xy + zw ) ),
114 static_cast<float>( 2 * ( xz - yw ) ),
116 static_cast<float>( 2 * ( xy - zw ) ),
117 static_cast<float>( 1 - 2 * ( xx + zz ) ),
118 static_cast<float>( 2 * ( yz + xw ) ),
120 static_cast<float>( 2 * ( xz + yw ) ),
121 static_cast<float>( 2 * ( yz - xw ) ),
122 static_cast<float>( 1 - 2 * ( xx + yy ) ),
131 const double x2 = quaternion[0] + quaternion[0];
132 const double y2 = quaternion[1] + quaternion[1];
133 const double z2 = quaternion[2] + quaternion[2];
134 const double xx = quaternion[0] * x2;
135 const double xy = quaternion[0] * y2;
136 const double xz = quaternion[0] * z2;
137 const double yy = quaternion[1] * y2;
138 const double yz = quaternion[1] * z2;
139 const double zz = quaternion[2] * z2;
140 const double wx = quaternion[3] * x2;
141 const double wy = quaternion[3] * y2;
142 const double wz = quaternion[3] * z2;
145 static_cast<float>( 1.0 - ( yy + zz ) ),
146 static_cast<float>( xy + wz ),
147 static_cast<float>( xz - wy ),
149 static_cast<float>( xy - wz ),
150 static_cast<float>( 1.0 - ( xx + zz ) ),
151 static_cast<float>( yz + wx ),
153 static_cast<float>( xz + wy ),
154 static_cast<float>( yz - wx ),
155 static_cast<float>( 1.0 - ( xx + yy ) ),
166 const double c_half_sqrt2 = 0.70710678118654752440084436210485;
167 const float c_half_sqrt2f = static_cast<float>( c_half_sqrt2 );
169 inline bool quaternion_component_is_90( float component ){
170 return ( fabs( component ) - c_half_sqrt2 ) < 0.001;
173 inline Matrix4 matrix4_rotation_for_quaternion_quantised( const Quaternion& quaternion ){
174 if ( quaternion.y() == 0
175 && quaternion.z() == 0
176 && quaternion_component_is_90( quaternion.x() )
177 && quaternion_component_is_90( quaternion.w() ) ) {
178 return matrix4_rotation_for_sincos_x( ( quaternion.x() > 0 ) ? 1.f : -1.f, 0 );
181 if ( quaternion.x() == 0
182 && quaternion.z() == 0
183 && quaternion_component_is_90( quaternion.y() )
184 && quaternion_component_is_90( quaternion.w() ) ) {
185 return matrix4_rotation_for_sincos_y( ( quaternion.y() > 0 ) ? 1.f : -1.f, 0 );
188 if ( quaternion.x() == 0
189 && quaternion.y() == 0
190 && quaternion_component_is_90( quaternion.z() )
191 && quaternion_component_is_90( quaternion.w() ) ) {
192 return matrix4_rotation_for_sincos_z( ( quaternion.z() > 0 ) ? 1.f : -1.f, 0 );
195 return matrix4_rotation_for_quaternion( quaternion );
198 inline Quaternion quaternion_for_matrix4_rotation( const Matrix4& matrix4 ){
199 Matrix4 transposed = matrix4_transposed( matrix4 );
201 double trace = transposed[0] + transposed[5] + transposed[10] + 1.0;
203 if ( trace > 0.0001 ) {
204 double S = 0.5 / sqrt( trace );
206 static_cast<float>( ( transposed[9] - transposed[6] ) * S ),
207 static_cast<float>( ( transposed[2] - transposed[8] ) * S ),
208 static_cast<float>( ( transposed[4] - transposed[1] ) * S ),
209 static_cast<float>( 0.25 / S )
213 if ( transposed[0] >= transposed[5] && transposed[0] >= transposed[10] ) {
214 double S = 2.0 * sqrt( 1.0 + transposed[0] - transposed[5] - transposed[10] );
216 static_cast<float>( 0.25 / S ),
217 static_cast<float>( ( transposed[1] + transposed[4] ) / S ),
218 static_cast<float>( ( transposed[2] + transposed[8] ) / S ),
219 static_cast<float>( ( transposed[6] + transposed[9] ) / S )
223 if ( transposed[5] >= transposed[0] && transposed[5] >= transposed[10] ) {
224 double S = 2.0 * sqrt( 1.0 + transposed[5] - transposed[0] - transposed[10] );
226 static_cast<float>( ( transposed[1] + transposed[4] ) / S ),
227 static_cast<float>( 0.25 / S ),
228 static_cast<float>( ( transposed[6] + transposed[9] ) / S ),
229 static_cast<float>( ( transposed[2] + transposed[8] ) / S )
233 double S = 2.0 * sqrt( 1.0 + transposed[10] - transposed[0] - transposed[5] );
235 static_cast<float>( ( transposed[2] + transposed[8] ) / S ),
236 static_cast<float>( ( transposed[6] + transposed[9] ) / S ),
237 static_cast<float>( 0.25 / S ),
238 static_cast<float>( ( transposed[1] + transposed[4] ) / S )
242 /// \brief Returns \p self concatenated with the rotation transform produced by \p rotation.
243 /// The concatenated rotation occurs before \p self.
244 inline Matrix4 matrix4_rotated_by_quaternion( const Matrix4& self, const Quaternion& rotation ){
245 return matrix4_multiplied_by_matrix4( self, matrix4_rotation_for_quaternion( rotation ) );
248 /// \brief Concatenates \p self with the rotation transform produced by \p rotation.
249 /// The concatenated rotation occurs before \p self.
250 inline void matrix4_rotate_by_quaternion( Matrix4& self, const Quaternion& rotation ){
251 self = matrix4_rotated_by_quaternion( self, rotation );
254 /// \brief Rotates \p self by \p rotation, using \p pivotpoint.
255 inline void matrix4_pivoted_rotate_by_quaternion( Matrix4& self, const Quaternion& rotation, const Vector3& pivotpoint ){
256 matrix4_translate_by_vec3( self, pivotpoint );
257 matrix4_rotate_by_quaternion( self, rotation );
258 matrix4_translate_by_vec3( self, vector3_negated( pivotpoint ) );
261 inline Vector3 quaternion_transformed_point( const Quaternion& quaternion, const Vector3& point ){
262 double xx = quaternion.x() * quaternion.x();
263 double yy = quaternion.y() * quaternion.y();
264 double zz = quaternion.z() * quaternion.z();
265 double ww = quaternion.w() * quaternion.w();
267 double xy2 = quaternion.x() * quaternion.y() * 2;
268 double xz2 = quaternion.x() * quaternion.z() * 2;
269 double xw2 = quaternion.x() * quaternion.w() * 2;
270 double yz2 = quaternion.y() * quaternion.z() * 2;
271 double yw2 = quaternion.y() * quaternion.w() * 2;
272 double zw2 = quaternion.z() * quaternion.w() * 2;
275 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() ),
276 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() ),
277 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() )
281 /// \brief Constructs a pure-rotation transform from \p axis and \p angle (radians).
282 inline Matrix4 matrix4_rotation_for_axisangle( const Vector3& axis, double angle ){
283 return matrix4_rotation_for_quaternion( quaternion_for_axisangle( axis, angle ) );
286 /// \brief Rotates \p self about \p axis by \p angle.
287 inline void matrix4_rotate_by_axisangle( Matrix4& self, const Vector3& axis, double angle ){
288 matrix4_multiply_by_matrix4( self, matrix4_rotation_for_axisangle( axis, angle ) );
291 /// \brief Rotates \p self about \p axis by \p angle using \p pivotpoint.
292 inline void matrix4_pivoted_rotate_by_axisangle( Matrix4& self, const Vector3& axis, double angle, const Vector3& pivotpoint ){
293 matrix4_translate_by_vec3( self, pivotpoint );
294 matrix4_rotate_by_axisangle( self, axis, angle );
295 matrix4_translate_by_vec3( self, vector3_negated( pivotpoint ) );