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         return Quaternion(
  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]
  41                            );
  42 }
  43
  44 inline void quaternion_multiply_by_quaternion( Quaternion& quaternion, const Quaternion& other ){
  45         quaternion = quaternion_multiplied_by_quaternion( quaternion, other );
  46 }
  47
  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 ) ) );
  51 }
  52
  53 inline Quaternion quaternion_for_axisangle( const Vector3& axis, double angle ){
  54         angle *= 0.5;
  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 ) ) );
  57 }
  58
  59 inline Quaternion quaternion_for_x( double angle ){
  60         angle *= 0.5;
  61         return Quaternion( static_cast<float>( sin( angle ) ), 0, 0, static_cast<float>( cos( angle ) ) );
  62 }
  63
  64 inline Quaternion quaternion_for_y( double angle ){
  65         angle *= 0.5;
  66         return Quaternion( 0, static_cast<float>( sin( angle ) ), 0, static_cast<float>( cos( angle ) ) );
  67 }
  68
  69 inline Quaternion quaternion_for_z( double angle ){
  70         angle *= 0.5;
  71         return Quaternion( 0, 0, static_cast<float>( sin( angle ) ), static_cast<float>( cos( angle ) ) );
  72 }
  73
  74 inline Quaternion quaternion_inverse( const Quaternion& quaternion ){
  75         return Quaternion( vector3_negated( vector4_to_vector3( quaternion ) ), quaternion[3] );
  76 }
  77
  78 inline void quaternion_conjugate( Quaternion& quaternion ){
  79         quaternion = quaternion_inverse( quaternion );
  80 }
  81
  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] ) );
  84         return Quaternion(
  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 )
  89                            );
  90 }
  91
  92 inline void quaternion_normalise( Quaternion& quaternion ){
  93         quaternion = quaternion_normalised( quaternion );
  94 }
  95
  96 /// \brief Constructs a pure-rotation matrix from \p quaternion.
  97 inline Matrix4 matrix4_rotation_for_quaternion( const Quaternion& quaternion ){
  98 #if 0
  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];
 103
 104         const double yy = quaternion[1] * quaternion[1];
 105         const double yz = quaternion[1] * quaternion[2];
 106         const double yw = quaternion[1] * quaternion[3];
 107
 108         const double zz = quaternion[2] * quaternion[2];
 109         const double zw = quaternion[2] * quaternion[3];
 110
 111         return Matrix4(
 112                            static_cast<float>( 1 - 2 * ( yy + zz ) ),
 113                            static_cast<float>(     2 * ( xy + zw ) ),
 114                            static_cast<float>(     2 * ( xz - yw ) ),
 115                            0,
 116                            static_cast<float>(     2 * ( xy - zw ) ),
 117                            static_cast<float>( 1 - 2 * ( xx + zz ) ),
 118                            static_cast<float>(     2 * ( yz + xw ) ),
 119                            0,
 120                            static_cast<float>(     2 * ( xz + yw ) ),
 121                            static_cast<float>(     2 * ( yz - xw ) ),
 122                            static_cast<float>( 1 - 2 * ( xx + yy ) ),
 123                            0,
 124                            0,
 125                            0,
 126                            0,
 127                            1
 128                            );
 129
 130 #else
 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;
 143
 144         return Matrix4(
 145                            static_cast<float>( 1.0 - ( yy + zz ) ),
 146                            static_cast<float>( xy + wz ),
 147                            static_cast<float>( xz - wy ),
 148                            0,
 149                            static_cast<float>( xy - wz ),
 150                            static_cast<float>( 1.0 - ( xx + zz ) ),
 151                            static_cast<float>( yz + wx ),
 152                            0,
 153                            static_cast<float>( xz + wy ),
 154                            static_cast<float>( yz - wx ),
 155                            static_cast<float>( 1.0 - ( xx + yy ) ),
 156                            0,
 157                            0,
 158                            0,
 159                            0,
 160                            1
 161                            );
 162
 163 #endif
 164 }
 165
 166 const double c_half_sqrt2 = 0.70710678118654752440084436210485;
 167 const float c_half_sqrt2f = static_cast<float>( c_half_sqrt2 );
 168
 169 inline bool quaternion_component_is_90( float component ){
 170         return ( fabs( component ) - c_half_sqrt2 ) < 0.001;
 171 }
 172
 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 );
 179         }
 180
 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 );
 186         }
 187
 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 );
 193         }
 194
 195         return matrix4_rotation_for_quaternion( quaternion );
 196 }
 197
 198 inline Quaternion quaternion_for_matrix4_rotation( const Matrix4& matrix4 ){
 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                 double S = 0.5 / sqrt( trace );
 205                 return Quaternion(
 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 )
 210                                    );
 211         }
 212
 213         if ( transposed[0] >= transposed[5] && transposed[0] >= transposed[10] ) {
 214                 double S = 2.0 * sqrt( 1.0 + transposed[0] - transposed[5] - transposed[10] );
 215                 return Quaternion(
 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 )
 220                                    );
 221         }
 222
 223         if ( transposed[5] >= transposed[0] && transposed[5] >= transposed[10] ) {
 224                 double S = 2.0 * sqrt( 1.0 + transposed[5] - transposed[0] - transposed[10] );
 225                 return Quaternion(
 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 )
 230                                    );
 231         }
 232
 233         double S = 2.0 * sqrt( 1.0 + transposed[10] - transposed[0] - transposed[5] );
 234         return Quaternion(
 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 )
 239                            );
 240 }
 241
 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 ) );
 246 }
 247
 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 );
 252 }
 253
 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 ) );
 259 }
 260
 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();
 266
 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;
 273
 274         return Vector3(
 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() )
 278                            );
 279 }
 280
 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 ) );
 284 }
 285
 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 ) );
 289 }
 290
 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 ) );
 296 }
 297
 298
 299 #endif