home *** CD-ROM | disk | FTP | other *** search
/ GameStar 2006 March / Gamestar_82_2006-03_dvd.iso / DVDStar / Editace / quake4_sdkv10.exe / source / idlib / math / Angles.cpp next >
C/C++ Source or Header  |  2005-11-14  |  5KB  |  231 lines
  1.  
  2. #include "../precompiled.h"
  3. #pragma hdrstop
  4.  
  5. #include <float.h>
  6.  
  7. idAngles ang_zero( 0.0f, 0.0f, 0.0f );
  8.  
  9.  
  10. /*
  11. =================
  12. idAngles::Normalize360
  13.  
  14. returns angles normalized to the range [0 <= angle < 360]
  15. =================
  16. */
  17. idAngles& idAngles::Normalize360( void ) {
  18.     int i;
  19.  
  20.     for ( i = 0; i < 3; i++ ) {
  21.         if ( ( (*this)[i] >= 360.0f ) || ( (*this)[i] < 0.0f ) ) {
  22.             (*this)[i] -= floor( (*this)[i] / 360.0f ) * 360.0f;
  23.  
  24.             if ( (*this)[i] >= 360.0f ) {
  25.                 (*this)[i] -= 360.0f;
  26.             }
  27.             if ( (*this)[i] < 0.0f ) {
  28.                 (*this)[i] += 360.0f;
  29.             }
  30.         }
  31.     }
  32.  
  33.     return *this;
  34. }
  35.  
  36. /*
  37. =================
  38. idAngles::Normalize180
  39.  
  40. returns angles normalized to the range [-180 < angle <= 180]
  41. =================
  42. */
  43. idAngles& idAngles::Normalize180( void ) {
  44.     Normalize360();
  45.  
  46.     if ( pitch > 180.0f ) {
  47.         pitch -= 360.0f;
  48.     }
  49.     
  50.     if ( yaw > 180.0f ) {
  51.         yaw -= 360.0f;
  52.     }
  53.  
  54.     if ( roll > 180.0f ) {
  55.         roll -= 360.0f;
  56.     }
  57.     return *this;
  58. }
  59.  
  60. /*
  61. =================
  62. idAngles::ToVectors
  63. =================
  64. */
  65. void idAngles::ToVectors( idVec3 *forward, idVec3 *right, idVec3 *up ) const {
  66.     float sr, sp, sy, cr, cp, cy;
  67.     
  68.     idMath::SinCos( DEG2RAD( yaw ), sy, cy );
  69.     idMath::SinCos( DEG2RAD( pitch ), sp, cp );
  70.     idMath::SinCos( DEG2RAD( roll ), sr, cr );
  71.     if ( forward ) {
  72.         forward->Set( cp * cy, cp * sy, -sp );
  73.     }
  74.  
  75.     if ( right ) {
  76.         right->Set( -sr * sp * cy + cr * sy, -sr * sp * sy + -cr * cy, -sr * cp );
  77.     }
  78.  
  79.     if ( up ) {
  80.         up->Set( cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp );
  81.     }
  82. }
  83.  
  84. /*
  85. =================
  86. idAngles::ToForward
  87. =================
  88. */
  89. idVec3 idAngles::ToForward( void ) const {
  90.     float sp, sy, cp, cy;
  91.     
  92.     idMath::SinCos( DEG2RAD( yaw ), sy, cy );
  93.     idMath::SinCos( DEG2RAD( pitch ), sp, cp );
  94.  
  95.     return idVec3( cp * cy, cp * sy, -sp );
  96. }
  97.  
  98. /*
  99. =================
  100. idAngles::ToQuat
  101. =================
  102. */
  103. idQuat idAngles::ToQuat( void ) const {
  104.     float sx, cx, sy, cy, sz, cz;
  105.     float sxcy, cxcy, sxsy, cxsy;
  106.  
  107.     idMath::SinCos( DEG2RAD( yaw ) * 0.5f, sz, cz );
  108.     idMath::SinCos( DEG2RAD( pitch ) * 0.5f, sy, cy );
  109.     idMath::SinCos( DEG2RAD( roll ) * 0.5f, sx, cx );
  110.  
  111.     sxcy = sx * cy;
  112.     cxcy = cx * cy;
  113.     sxsy = sx * sy;
  114.     cxsy = cx * sy;
  115.  
  116.     return idQuat( cxsy*sz - sxcy*cz, -cxsy*cz - sxcy*sz, sxsy*cz - cxcy*sz, cxcy*cz + sxsy*sz );
  117. }
  118.  
  119. /*
  120. =================
  121. idAngles::ToRotation
  122. =================
  123. */
  124. idRotation idAngles::ToRotation( void ) const {
  125.     idVec3 vec;
  126.     float angle, w;
  127.     float sx, cx, sy, cy, sz, cz;
  128.     float sxcy, cxcy, sxsy, cxsy;
  129.  
  130.     if ( pitch == 0.0f ) {
  131.         if ( yaw == 0.0f ) {
  132.             return idRotation( vec3_origin, idVec3( -1.0f, 0.0f, 0.0f ), roll );
  133.         }
  134.         if ( roll == 0.0f ) {
  135.             return idRotation( vec3_origin, idVec3( 0.0f, 0.0f, -1.0f ), yaw );
  136.         }
  137.     } else if ( yaw == 0.0f && roll == 0.0f ) {
  138.         return idRotation( vec3_origin, idVec3( 0.0f, -1.0f, 0.0f ), pitch );
  139.     }
  140.  
  141.     idMath::SinCos( DEG2RAD( yaw ) * 0.5f, sz, cz );
  142.     idMath::SinCos( DEG2RAD( pitch ) * 0.5f, sy, cy );
  143.     idMath::SinCos( DEG2RAD( roll ) * 0.5f, sx, cx );
  144.  
  145.     sxcy = sx * cy;
  146.     cxcy = cx * cy;
  147.     sxsy = sx * sy;
  148.     cxsy = cx * sy;
  149.  
  150.     vec.x =  cxsy * sz - sxcy * cz;
  151.     vec.y = -cxsy * cz - sxcy * sz;
  152.     vec.z =  sxsy * cz - cxcy * sz;
  153.     w =         cxcy * cz + sxsy * sz;
  154.     angle = idMath::ACos( w );
  155.     if ( angle == 0.0f ) {
  156.         vec.Set( 0.0f, 0.0f, 1.0f );
  157.     } else {
  158.         //vec *= (1.0f / sin( angle ));
  159.         vec.Normalize();
  160.         vec.FixDegenerateNormal();
  161.         angle *= 2.0f * idMath::M_RAD2DEG;
  162.     }
  163.     return idRotation( vec3_origin, vec, angle );
  164. }
  165.  
  166. /*
  167. =================
  168. idAngles::ToMat3
  169. =================
  170. */
  171. idMat3 idAngles::ToMat3( void ) const {
  172.     idMat3 mat;
  173.     float sr, sp, sy, cr, cp, cy;
  174.  
  175.     idMath::SinCos( DEG2RAD( yaw ), sy, cy );
  176.     idMath::SinCos( DEG2RAD( pitch ), sp, cp );
  177.     idMath::SinCos( DEG2RAD( roll ), sr, cr );
  178.  
  179.     mat[ 0 ].Set( cp * cy, cp * sy, -sp );
  180.     mat[ 1 ].Set( sr * sp * cy + cr * -sy, sr * sp * sy + cr * cy, sr * cp );
  181.     mat[ 2 ].Set( cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp );
  182.  
  183.     return mat;
  184. }
  185.  
  186. // RAVEN BEGIN
  187. // jscott: slightly quicker version without the copy
  188. idMat3 &idAngles::ToMat3( idMat3 &mat ) const 
  189. {
  190.     float    sr, sp, sy, cr, cp, cy;
  191.         
  192.     idMath::SinCos( DEG2RAD( yaw ), sy, cy );
  193.     idMath::SinCos( DEG2RAD( pitch ), sp, cp );
  194.     idMath::SinCos( DEG2RAD( roll ), sr, cr );
  195.  
  196.     mat[0].Set( cp * cy, cp * sy, -sp );
  197.     mat[1].Set( sr * sp * cy + cr * -sy, sr * sp * sy + cr * cy, sr * cp );
  198.     mat[2].Set( cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp );
  199.  
  200.     return( mat );
  201. }
  202. // RAVEN END
  203.  
  204. /*
  205. =================
  206. idAngles::ToMat4
  207. =================
  208. */
  209. idMat4 idAngles::ToMat4( void ) const {
  210.     return ToMat3().ToMat4();
  211. }
  212.  
  213. /*
  214. =================
  215. idAngles::ToAngularVelocity
  216. =================
  217. */
  218. idVec3 idAngles::ToAngularVelocity( void ) const {
  219.     idRotation rotation = idAngles::ToRotation();
  220.     return rotation.GetVec() * DEG2RAD( rotation.GetAngle() );
  221. }
  222.  
  223. /*
  224. =============
  225. idAngles::ToString
  226. =============
  227. */
  228. const char *idAngles::ToString( int precision ) const {
  229.     return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
  230. }
  231.