[q3map2] Unwind script stack in case of script loading error.
[xonotic/netradiant.git] / 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 }