neo/idlib/math/Angles.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 #include <float.h>
  33
  34 idAngles ang_zero( 0.0f, 0.0f, 0.0f );
  35
  36
  37 /*
  38 =================
  39 idAngles::Normalize360
  40
  41 returns angles normalized to the range [0 <= angle < 360]
  42 =================
  43 */
  44 idAngles& idAngles::Normalize360( void ) {
  45         int i;
  46
  47         for ( i = 0; i < 3; i++ ) {
  48                 if ( ( (*this)[i] >= 360.0f ) || ( (*this)[i] < 0.0f ) ) {
  49                         (*this)[i] -= floor( (*this)[i] / 360.0f ) * 360.0f;
  50
  51                         if ( (*this)[i] >= 360.0f ) {
  52                                 (*this)[i] -= 360.0f;
  53                         }
  54                         if ( (*this)[i] < 0.0f ) {
  55                                 (*this)[i] += 360.0f;
  56                         }
  57                 }
  58         }
  59
  60         return *this;
  61 }
  62
  63 /*
  64 =================
  65 idAngles::Normalize180
  66
  67 returns angles normalized to the range [-180 < angle <= 180]
  68 =================
  69 */
  70 idAngles& idAngles::Normalize180( void ) {
  71         Normalize360();
  72
  73         if ( pitch > 180.0f ) {
  74                 pitch -= 360.0f;
  75         }
  76
  77         if ( yaw > 180.0f ) {
  78                 yaw -= 360.0f;
  79         }
  80
  81         if ( roll > 180.0f ) {
  82                 roll -= 360.0f;
  83         }
  84         return *this;
  85 }
  86
  87 /*
  88 =================
  89 idAngles::ToVectors
  90 =================
  91 */
  92 void idAngles::ToVectors( idVec3 *forward, idVec3 *right, idVec3 *up ) const {
  93         float sr, sp, sy, cr, cp, cy;
  94
  95         idMath::SinCos( DEG2RAD( yaw ), sy, cy );
  96         idMath::SinCos( DEG2RAD( pitch ), sp, cp );
  97         idMath::SinCos( DEG2RAD( roll ), sr, cr );
  98
  99         if ( forward ) {
 100                 forward->Set( cp * cy, cp * sy, -sp );
 101         }
 102
 103         if ( right ) {
 104                 right->Set( -sr * sp * cy + cr * sy, -sr * sp * sy + -cr * cy, -sr * cp );
 105         }
 106
 107         if ( up ) {
 108                 up->Set( cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp );
 109         }
 110 }
 111
 112 /*
 113 =================
 114 idAngles::ToForward
 115 =================
 116 */
 117 idVec3 idAngles::ToForward( void ) const {
 118         float sp, sy, cp, cy;
 119
 120         idMath::SinCos( DEG2RAD( yaw ), sy, cy );
 121         idMath::SinCos( DEG2RAD( pitch ), sp, cp );
 122
 123         return idVec3( cp * cy, cp * sy, -sp );
 124 }
 125
 126 /*
 127 =================
 128 idAngles::ToQuat
 129 =================
 130 */
 131 idQuat idAngles::ToQuat( void ) const {
 132         float sx, cx, sy, cy, sz, cz;
 133         float sxcy, cxcy, sxsy, cxsy;
 134
 135         idMath::SinCos( DEG2RAD( yaw ) * 0.5f, sz, cz );
 136         idMath::SinCos( DEG2RAD( pitch ) * 0.5f, sy, cy );
 137         idMath::SinCos( DEG2RAD( roll ) * 0.5f, sx, cx );
 138
 139         sxcy = sx * cy;
 140         cxcy = cx * cy;
 141         sxsy = sx * sy;
 142         cxsy = cx * sy;
 143
 144         return idQuat( cxsy*sz - sxcy*cz, -cxsy*cz - sxcy*sz, sxsy*cz - cxcy*sz, cxcy*cz + sxsy*sz );
 145 }
 146
 147 /*
 148 =================
 149 idAngles::ToRotation
 150 =================
 151 */
 152 idRotation idAngles::ToRotation( void ) const {
 153         idVec3 vec;
 154         float angle, w;
 155         float sx, cx, sy, cy, sz, cz;
 156         float sxcy, cxcy, sxsy, cxsy;
 157
 158         if ( pitch == 0.0f ) {
 159                 if ( yaw == 0.0f ) {
 160                         return idRotation( vec3_origin, idVec3( -1.0f, 0.0f, 0.0f ), roll );
 161                 }
 162                 if ( roll == 0.0f ) {
 163                         return idRotation( vec3_origin, idVec3( 0.0f, 0.0f, -1.0f ), yaw );
 164                 }
 165         } else if ( yaw == 0.0f && roll == 0.0f ) {
 166                 return idRotation( vec3_origin, idVec3( 0.0f, -1.0f, 0.0f ), pitch );
 167         }
 168
 169         idMath::SinCos( DEG2RAD( yaw ) * 0.5f, sz, cz );
 170         idMath::SinCos( DEG2RAD( pitch ) * 0.5f, sy, cy );
 171         idMath::SinCos( DEG2RAD( roll ) * 0.5f, sx, cx );
 172
 173         sxcy = sx * cy;
 174         cxcy = cx * cy;
 175         sxsy = sx * sy;
 176         cxsy = cx * sy;
 177
 178         vec.x =  cxsy * sz - sxcy * cz;
 179         vec.y = -cxsy * cz - sxcy * sz;
 180         vec.z =  sxsy * cz - cxcy * sz;
 181         w =              cxcy * cz + sxsy * sz;
 182         angle = idMath::ACos( w );
 183         if ( angle == 0.0f ) {
 184                 vec.Set( 0.0f, 0.0f, 1.0f );
 185         } else {
 186                 //vec *= (1.0f / sin( angle ));
 187                 vec.Normalize();
 188                 vec.FixDegenerateNormal();
 189                 angle *= 2.0f * idMath::M_RAD2DEG;
 190         }
 191         return idRotation( vec3_origin, vec, angle );
 192 }
 193
 194 /*
 195 =================
 196 idAngles::ToMat3
 197 =================
 198 */
 199 idMat3 idAngles::ToMat3( void ) const {
 200         idMat3 mat;
 201         float sr, sp, sy, cr, cp, cy;
 202
 203         idMath::SinCos( DEG2RAD( yaw ), sy, cy );
 204         idMath::SinCos( DEG2RAD( pitch ), sp, cp );
 205         idMath::SinCos( DEG2RAD( roll ), sr, cr );
 206
 207         mat[ 0 ].Set( cp * cy, cp * sy, -sp );
 208         mat[ 1 ].Set( sr * sp * cy + cr * -sy, sr * sp * sy + cr * cy, sr * cp );
 209         mat[ 2 ].Set( cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp );
 210
 211         return mat;
 212 }
 213
 214 /*
 215 =================
 216 idAngles::ToMat4
 217 =================
 218 */
 219 idMat4 idAngles::ToMat4( void ) const {
 220         return ToMat3().ToMat4();
 221 }
 222
 223 /*
 224 =================
 225 idAngles::ToAngularVelocity
 226 =================
 227 */
 228 idVec3 idAngles::ToAngularVelocity( void ) const {
 229         idRotation rotation = idAngles::ToRotation();
 230         return rotation.GetVec() * DEG2RAD( rotation.GetAngle() );
 231 }
 232
 233 /*
 234 =============
 235 idAngles::ToString
 236 =============
 237 */
 238 const char *idAngles::ToString( int precision ) const {
 239         return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
 240 }