libs/splines/math_angles.cpp

   1 /*
   2    Copyright (C) 1999-2006 Id Software, Inc. and contributors.
   3    For a list of contributors, see the accompanying CONTRIBUTORS file.
   4
   5    This file is part of GtkRadiant.
   6
   7    GtkRadiant is free software; you can redistribute it and/or modify
   8    it under the terms of the GNU General Public License as published by
   9    the Free Software Foundation; either version 2 of the License, or
  10    (at your option) any later version.
  11
  12    GtkRadiant is distributed in the hope that it will be useful,
  13    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 GtkRadiant; 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         }
  38         else if ( sp < -1.0 ) {
  39                 sp = -1.0;
  40         }
  41
  42         theta = -asin( sp );
  43         cp = cos( theta );
  44
  45         if ( cp > 8192 * FLT_EPSILON ) {
  46                 dst.pitch   = theta * 180 / M_PI;
  47                 dst.yaw     = atan2( src[ 0 ][ 1 ], src[ 0 ][ 0 ] ) * 180 / M_PI;
  48                 dst.roll    = atan2( src[ 1 ][ 2 ], src[ 2 ][ 2 ] ) * 180 / M_PI;
  49         }
  50         else {
  51                 dst.pitch   = theta * 180 / M_PI;
  52                 dst.yaw     = -atan2( src[ 1 ][ 0 ], src[ 1 ][ 1 ] ) * 180 / M_PI;
  53                 dst.roll    = 0;
  54         }
  55 }
  56
  57 void toAngles( quat_t &src, angles_t &dst ) {
  58         mat3_t temp;
  59
  60         toMatrix( src, temp );
  61         toAngles( temp, dst );
  62 }
  63
  64 void toAngles( idVec3 &src, angles_t &dst ) {
  65         dst.pitch   = src[ 0 ];
  66         dst.yaw     = src[ 1 ];
  67         dst.roll    = src[ 2 ];
  68 }
  69
  70 void angles_t::toVectors( idVec3 *forward, idVec3 *right, idVec3 *up ) {
  71         float angle;
  72         static float sr, sp, sy, cr, cp, cy;    // static to help MS compiler fp bugs
  73
  74         angle = yaw * ( M_PI * 2 / 360 );
  75         sy = sin( angle );
  76         cy = cos( angle );
  77
  78         angle = pitch * ( M_PI * 2 / 360 );
  79         sp = sin( angle );
  80         cp = cos( angle );
  81
  82         angle = roll * ( M_PI * 2 / 360 );
  83         sr = sin( angle );
  84         cr = cos( angle );
  85
  86         if ( forward ) {
  87                 forward->set( cp * cy, cp * sy, -sp );
  88         }
  89
  90         if ( right ) {
  91                 right->set( -sr * sp * cy + cr * sy, -sr * sp * sy + -cr * cy, -sr * cp );
  92         }
  93
  94         if ( up ) {
  95                 up->set( cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp );
  96         }
  97 }
  98
  99 idVec3 angles_t::toForward( void ) {
 100         float angle;
 101         static float sp, sy, cp, cy;    // static to help MS compiler fp bugs
 102
 103         angle = yaw * ( M_PI * 2 / 360 );
 104         sy = sin( angle );
 105         cy = cos( angle );
 106
 107         angle = pitch * ( M_PI * 2 / 360 );
 108         sp = sin( angle );
 109         cp = cos( angle );
 110
 111         return idVec3( cp * cy, cp * sy, -sp );
 112 }
 113
 114 /*
 115    =================
 116    Normalize360
 117
 118    returns angles normalized to the range [0 <= angle < 360]
 119    =================
 120  */
 121 angles_t& angles_t::Normalize360( void ) {
 122         pitch   = ( 360.0 / 65536 ) * ( ( int )( pitch    * ( 65536 / 360.0 ) ) & 65535 );
 123         yaw     = ( 360.0 / 65536 ) * ( ( int )( yaw      * ( 65536 / 360.0 ) ) & 65535 );
 124         roll    = ( 360.0 / 65536 ) * ( ( int )( roll     * ( 65536 / 360.0 ) ) & 65535 );
 125
 126         return *this;
 127 }
 128
 129
 130 /*
 131    =================
 132    Normalize180
 133
 134    returns angles normalized to the range [-180 < angle <= 180]
 135    =================
 136  */
 137 angles_t& angles_t::Normalize180( void ) {
 138         Normalize360();
 139
 140         if ( pitch > 180.0 ) {
 141                 pitch -= 360.0;
 142         }
 143
 144         if ( yaw > 180.0 ) {
 145                 yaw  -= 360.0;
 146         }
 147
 148         if ( roll > 180.0 ) {
 149                 roll -= 360.0;
 150         }
 151         return *this;
 152 }