math_angles.cpp (3594B)
1 /* 2 =========================================================================== 3 Copyright (C) 1999-2005 Id Software, Inc. 4 5 This file is part of Quake III Arena source code. 6 7 Quake III Arena source code is free software; you can redistribute it 8 and/or modify it under the terms of the GNU General Public License as 9 published by the Free Software Foundation; either version 2 of the License, 10 or (at your option) any later version. 11 12 Quake III Arena source code is distributed in the hope that it will be 13 useful, 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 Foobar; if not, write to the Free Software 19 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 20 =========================================================================== 21 */ 22 #include "q_shared.h" 23 #include <float.h> 24 25 angles_t ang_zero( 0.0f, 0.0f, 0.0f ); 26 27 void toAngles( mat3_t &src, angles_t &dst ) { 28 double theta; 29 double cp; 30 double sp; 31 32 sp = src[ 0 ][ 2 ]; 33 34 // cap off our sin value so that we don't get any NANs 35 if ( sp > 1.0 ) { 36 sp = 1.0; 37 } else if ( sp < -1.0 ) { 38 sp = -1.0; 39 } 40 41 theta = -asin( sp ); 42 cp = cos( theta ); 43 44 if ( cp > 8192 * FLT_EPSILON ) { 45 dst.pitch = theta * 180 / M_PI; 46 dst.yaw = atan2( src[ 0 ][ 1 ], src[ 0 ][ 0 ] ) * 180 / M_PI; 47 dst.roll = atan2( src[ 1 ][ 2 ], src[ 2 ][ 2 ] ) * 180 / M_PI; 48 } else { 49 dst.pitch = theta * 180 / M_PI; 50 dst.yaw = -atan2( src[ 1 ][ 0 ], src[ 1 ][ 1 ] ) * 180 / M_PI; 51 dst.roll = 0; 52 } 53 } 54 55 void toAngles( quat_t &src, angles_t &dst ) { 56 mat3_t temp; 57 58 toMatrix( src, temp ); 59 toAngles( temp, dst ); 60 } 61 62 void toAngles( idVec3_t &src, angles_t &dst ) { 63 dst.pitch = src[ 0 ]; 64 dst.yaw = src[ 1 ]; 65 dst.roll = src[ 2 ]; 66 } 67 68 void angles_t::toVectors( idVec3_t *forward, idVec3_t *right, idVec3_t *up ) { 69 float angle; 70 static float sr, sp, sy, cr, cp, cy; // static to help MS compiler fp bugs 71 72 angle = yaw * ( M_PI * 2 / 360 ); 73 sy = sin( angle ); 74 cy = cos( angle ); 75 76 angle = pitch * ( M_PI * 2 / 360 ); 77 sp = sin( angle ); 78 cp = cos( angle ); 79 80 angle = roll * ( M_PI * 2 / 360 ); 81 sr = sin( angle ); 82 cr = cos( angle ); 83 84 if ( forward ) { 85 forward->set( cp * cy, cp * sy, -sp ); 86 } 87 88 if ( right ) { 89 right->set( -sr * sp * cy + cr * sy, -sr * sp * sy + -cr * cy, -sr * cp ); 90 } 91 92 if ( up ) { 93 up->set( cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp ); 94 } 95 } 96 97 idVec3_t angles_t::toForward( void ) { 98 float angle; 99 static float sp, sy, cp, cy; // static to help MS compiler fp bugs 100 101 angle = yaw * ( M_PI * 2 / 360 ); 102 sy = sin( angle ); 103 cy = cos( angle ); 104 105 angle = pitch * ( M_PI * 2 / 360 ); 106 sp = sin( angle ); 107 cp = cos( angle ); 108 109 return idVec3_t( cp * cy, cp * sy, -sp ); 110 } 111 112 /* 113 ================= 114 Normalize360 115 116 returns angles normalized to the range [0 <= angle < 360] 117 ================= 118 */ 119 angles_t& angles_t::Normalize360( void ) { 120 pitch = (360.0 / 65536) * ( ( int )( pitch * ( 65536 / 360.0 ) ) & 65535 ); 121 yaw = (360.0 / 65536) * ( ( int )( yaw * ( 65536 / 360.0 ) ) & 65535 ); 122 roll = (360.0 / 65536) * ( ( int )( roll * ( 65536 / 360.0 ) ) & 65535 ); 123 124 return *this; 125 } 126 127 128 /* 129 ================= 130 Normalize180 131 132 returns angles normalized to the range [-180 < angle <= 180] 133 ================= 134 */ 135 angles_t& angles_t::Normalize180( void ) { 136 Normalize360(); 137 138 if ( pitch > 180.0 ) { 139 pitch -= 360.0; 140 } 141 142 if ( yaw > 180.0 ) { 143 yaw -= 360.0; 144 } 145 146 if ( roll > 180.0 ) { 147 roll -= 360.0; 148 } 149 return *this; 150 }