DOOM-3-BFG

DOOM 3 BFG Edition
Log | Files | Refs

Angles.cpp (5970B)

      1 /*
      2 ===========================================================================
      3 
      4 Doom 3 BFG Edition GPL Source Code
      5 Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
      6 
      7 This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
      8 
      9 Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
     21 
     22 In addition, the Doom 3 BFG Edition 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 BFG Edition 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 #pragma hdrstop
     30 #include "../precompiled.h"
     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() {
     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() {
     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() 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() 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() 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() 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() const {
    220 	return ToMat3().ToMat4();
    221 }
    222 
    223 /*
    224 =================
    225 idAngles::ToAngularVelocity
    226 =================
    227 */
    228 idVec3 idAngles::ToAngularVelocity() 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 }