neo/idlib/math/Quat.cpp

   1 /*
   2 ===========================================================================
   3
   4 Doom 3 GPL Source Code
   5 Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company.
   6
   7 This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).
   8
   9 Doom 3 Source Code is free software: you can redistribute it and/or modify
  10 it under the terms of the GNU General Public License as published by
  11 the Free Software Foundation, either version 3 of the License, or
  12 (at your option) any later version.
  13
  14 Doom 3 Source Code is distributed in the hope that it will be useful,
  15 but WITHOUT ANY WARRANTY; without even the implied warranty of
  16 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  17 GNU General Public License for more details.
  18
  19 You should have received a copy of the GNU General Public License
  20 along with Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
  21
  22 In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
  23
  24 If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
  25
  26 ===========================================================================
  27 */
  28
  29 #include "../precompiled.h"
  30 #pragma hdrstop
  31
  32 /*
  33 =====================
  34 idQuat::ToAngles
  35 =====================
  36 */
  37 idAngles idQuat::ToAngles( void ) const {
  38         return ToMat3().ToAngles();
  39 }
  40
  41 /*
  42 =====================
  43 idQuat::ToRotation
  44 =====================
  45 */
  46 idRotation idQuat::ToRotation( void ) const {
  47         idVec3 vec;
  48         float angle;
  49
  50         vec.x = x;
  51         vec.y = y;
  52         vec.z = z;
  53         angle = idMath::ACos( w );
  54         if ( angle == 0.0f ) {
  55                 vec.Set( 0.0f, 0.0f, 1.0f );
  56         } else {
  57                 //vec *= (1.0f / sin( angle ));
  58                 vec.Normalize();
  59                 vec.FixDegenerateNormal();
  60                 angle *= 2.0f * idMath::M_RAD2DEG;
  61         }
  62         return idRotation( vec3_origin, vec, angle );
  63 }
  64
  65 /*
  66 =====================
  67 idQuat::ToMat3
  68 =====================
  69 */
  70 idMat3 idQuat::ToMat3( void ) const {
  71         idMat3  mat;
  72         float   wx, wy, wz;
  73         float   xx, yy, yz;
  74         float   xy, xz, zz;
  75         float   x2, y2, z2;
  76
  77         x2 = x + x;
  78         y2 = y + y;
  79         z2 = z + z;
  80
  81         xx = x * x2;
  82         xy = x * y2;
  83         xz = x * z2;
  84
  85         yy = y * y2;
  86         yz = y * z2;
  87         zz = z * z2;
  88
  89         wx = w * x2;
  90         wy = w * y2;
  91         wz = w * z2;
  92
  93         mat[ 0 ][ 0 ] = 1.0f - ( yy + zz );
  94         mat[ 0 ][ 1 ] = xy - wz;
  95         mat[ 0 ][ 2 ] = xz + wy;
  96
  97         mat[ 1 ][ 0 ] = xy + wz;
  98         mat[ 1 ][ 1 ] = 1.0f - ( xx + zz );
  99         mat[ 1 ][ 2 ] = yz - wx;
 100
 101         mat[ 2 ][ 0 ] = xz - wy;
 102         mat[ 2 ][ 1 ] = yz + wx;
 103         mat[ 2 ][ 2 ] = 1.0f - ( xx + yy );
 104
 105         return mat;
 106 }
 107
 108 /*
 109 =====================
 110 idQuat::ToMat4
 111 =====================
 112 */
 113 idMat4 idQuat::ToMat4( void ) const {
 114         return ToMat3().ToMat4();
 115 }
 116
 117 /*
 118 =====================
 119 idQuat::ToCQuat
 120 =====================
 121 */
 122 idCQuat idQuat::ToCQuat( void ) const {
 123         if ( w < 0.0f ) {
 124                 return idCQuat( -x, -y, -z );
 125         }
 126         return idCQuat( x, y, z );
 127 }
 128
 129 /*
 130 ============
 131 idQuat::ToAngularVelocity
 132 ============
 133 */
 134 idVec3 idQuat::ToAngularVelocity( void ) const {
 135         idVec3 vec;
 136
 137         vec.x = x;
 138         vec.y = y;
 139         vec.z = z;
 140         vec.Normalize();
 141         return vec * idMath::ACos( w );
 142 }
 143
 144 /*
 145 =============
 146 idQuat::ToString
 147 =============
 148 */
 149 const char *idQuat::ToString( int precision ) const {
 150         return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
 151 }
 152
 153 /*
 154 =====================
 155 idQuat::Slerp
 156
 157 Spherical linear interpolation between two quaternions.
 158 =====================
 159 */
 160 idQuat &idQuat::Slerp( const idQuat &from, const idQuat &to, float t ) {
 161         idQuat  temp;
 162         float   omega, cosom, sinom, scale0, scale1;
 163
 164         if ( t <= 0.0f ) {
 165                 *this = from;
 166                 return *this;
 167         }
 168
 169         if ( t >= 1.0f ) {
 170                 *this = to;
 171                 return *this;
 172         }
 173
 174         if ( from == to ) {
 175                 *this = to;
 176                 return *this;
 177         }
 178
 179         cosom = from.x * to.x + from.y * to.y + from.z * to.z + from.w * to.w;
 180         if ( cosom < 0.0f ) {
 181                 temp = -to;
 182                 cosom = -cosom;
 183         } else {
 184                 temp = to;
 185         }
 186
 187         if ( ( 1.0f - cosom ) > 1e-6f ) {
 188 #if 0
 189                 omega = acos( cosom );
 190                 sinom = 1.0f / sin( omega );
 191                 scale0 = sin( ( 1.0f - t ) * omega ) * sinom;
 192                 scale1 = sin( t * omega ) * sinom;
 193 #else
 194                 scale0 = 1.0f - cosom * cosom;
 195                 sinom = idMath::InvSqrt( scale0 );
 196                 omega = idMath::ATan16( scale0 * sinom, cosom );
 197                 scale0 = idMath::Sin16( ( 1.0f - t ) * omega ) * sinom;
 198                 scale1 = idMath::Sin16( t * omega ) * sinom;
 199 #endif
 200         } else {
 201                 scale0 = 1.0f - t;
 202                 scale1 = t;
 203         }
 204
 205         *this = ( scale0 * from ) + ( scale1 * temp );
 206         return *this;
 207 }
 208
 209 /*
 210 =============
 211 idCQuat::ToAngles
 212 =============
 213 */
 214 idAngles idCQuat::ToAngles( void ) const {
 215         return ToQuat().ToAngles();
 216 }
 217
 218 /*
 219 =============
 220 idCQuat::ToRotation
 221 =============
 222 */
 223 idRotation idCQuat::ToRotation( void ) const {
 224         return ToQuat().ToRotation();
 225 }
 226
 227 /*
 228 =============
 229 idCQuat::ToMat3
 230 =============
 231 */
 232 idMat3 idCQuat::ToMat3( void ) const {
 233         return ToQuat().ToMat3();
 234 }
 235
 236 /*
 237 =============
 238 idCQuat::ToMat4
 239 =============
 240 */
 241 idMat4 idCQuat::ToMat4( void ) const {
 242         return ToQuat().ToMat4();
 243 }
 244
 245 /*
 246 =============
 247 idCQuat::ToString
 248 =============
 249 */
 250 const char *idCQuat::ToString( int precision ) const {
 251         return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
 252 }