1 /*
2 Copyright (C) 1997-2001 Id Software, Inc.
3
4 This program is free software; you can redistribute it and/or
5 modify it under the terms of the GNU General Public License
6 as published by the Free Software Foundation; either version 2
7 of the License, or (at your option) any later version.
8
9 This program is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
12
13 See the GNU General Public License for more details.
14
15 You should have received a copy of the GNU General Public License
16 along with this program; if not, write to the Free Software
17 Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
18 */
19
20 //
21 // m_angles.c
22 //
23
24 #include "shared.h"
25
26 /*
27 ===============
28 AngleModf
29 ===============
30 */
AngleModf(float a)31 float AngleModf (float a)
32 {
33 return (360.0f/65536.0f) * ((int)(a*(65536.0f/360.0f)) & 65535);
34 }
35
36
37 /*
38 ===============
39 Angles_Matrix3
40 ===============
41 */
Angles_Matrix3(vec3_t angles,mat3x3_t axis)42 void Angles_Matrix3 (vec3_t angles, mat3x3_t axis)
43 {
44 double angle;
45 static double sr, sp, sy, cr, cp, cy;
46 // static to help MS compiler fp bugs
47
48 angle = DEG2RAD (angles[YAW]);
49 sy = sin (angle);
50 cy = cos (angle);
51 angle = DEG2RAD (angles[PITCH]);
52 sp = sin (angle);
53 cp = cos (angle);
54 angle = DEG2RAD (angles[ROLL]);
55 sr = sin (angle);
56 cr = cos (angle);
57
58 // Forward
59 axis[0][0] = cp * cy;
60 axis[0][1] = cp * sy;
61 axis[0][2] = -sp;
62
63 // Left
64 axis[1][0] = (-1 * sr * sp * cy + -1 * cr * -sy) * -1;
65 axis[1][1] = (-1 * sr * sp * sy + -1 * cr * cy) * -1;
66 axis[1][2] = (-1 * sr * cp) * -1;
67
68 // Up
69 axis[2][0] = (cr * sp * cy + -sr * -sy);
70 axis[2][1] = (cr * sp * sy + -sr * cy);
71 axis[2][2] = cr * cp;
72 }
73
74
75 /*
76 ===============
77 Angles_Vectors
78 ===============
79 */
Angles_Vectors(vec3_t angles,vec3_t forward,vec3_t right,vec3_t up)80 void Angles_Vectors (vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
81 {
82 float angle;
83 static float sr, sp, sy, cr, cp, cy;
84 // static to help MS compiler fp bugs
85
86 angle = DEG2RAD (angles[YAW]);
87 sy = (float)sin (angle);
88 cy = (float)cos (angle);
89 angle = DEG2RAD (angles[PITCH]);
90 sp = (float)sin (angle);
91 cp = (float)cos (angle);
92 angle = DEG2RAD (angles[ROLL]);
93 sr = (float)sin (angle);
94 cr = (float)cos (angle);
95
96 if (forward) {
97 forward[0] = cp * cy;
98 forward[1] = cp * sy;
99 forward[2] = -sp;
100 }
101 if (right) {
102 right[0] = (-1 * sr * sp * cy + -1 * cr * -sy);
103 right[1] = (-1 * sr * sp * sy + -1 * cr * cy);
104 right[2] = -1 * sr * cp;
105 }
106 if (up) {
107 up[0] = (cr * sp * cy + -sr * -sy);
108 up[1] = (cr * sp * sy + -sr * cy);
109 up[2] = cr * cp;
110 }
111 }
112
113
114 /*
115 ===============
116 LerpAngle
117 ===============
118 */
LerpAngle(float a2,float a1,float frac)119 float LerpAngle (float a2, float a1, float frac)
120 {
121 if (a1 - a2 > 180)
122 a1 -= 360;
123 if (a1 - a2 < -180)
124 a1 += 360;
125
126 return a2 + frac * (a1 - a2);
127 }
128
129
130 /*
131 ===============
132 VecToAngles
133 ===============
134 */
VecToAngles(vec3_t vec,vec3_t angles)135 void VecToAngles (vec3_t vec, vec3_t angles)
136 {
137 float forward;
138 float yaw, pitch;
139
140 if (vec[1] == 0 && vec[0] == 0) {
141 yaw = 0;
142 if (vec[2] > 0)
143 pitch = 90;
144 else
145 pitch = 270;
146 }
147 else {
148 if (vec[0])
149 yaw = (float)(atan2 (vec[1], vec[0]) * (180.0f / M_PI));
150 else if (vec[1] > 0)
151 yaw = 90;
152 else
153 yaw = 270;
154
155 if (yaw < 0)
156 yaw += 360;
157
158 forward = (float)sqrt (vec[0] * vec[0] + vec[1] * vec[1]);
159 pitch = (float)(atan2 (vec[2], forward) * (180.0f / M_PI));
160 if (pitch < 0)
161 pitch += 360;
162 }
163
164 angles[PITCH] = -pitch;
165 angles[YAW] = yaw;
166 angles[ROLL] = 0;
167 }
168
169
170 /*
171 ===============
172 VecToAngleRolled
173 ===============
174 */
VecToAngleRolled(vec3_t value,float angleYaw,vec3_t angles)175 void VecToAngleRolled (vec3_t value, float angleYaw, vec3_t angles)
176 {
177 float forward, yaw, pitch;
178
179 yaw = (float)(atan2(value[1], value[0]) * 180.0f / M_PI);
180 forward = (float)sqrt (value[0]*value[0] + value[1]*value[1]);
181 pitch = (float)(atan2(value[2], forward) * 180.0f / M_PI);
182
183 if (pitch < 0)
184 pitch += 360;
185
186 angles[PITCH] = -pitch;
187 angles[YAW] = yaw;
188 angles[ROLL] = -angleYaw;
189 }
190
191
192 /*
193 ===============
194 VecToYaw
195 ===============
196 */
VecToYaw(vec3_t vec)197 float VecToYaw (vec3_t vec)
198 {
199 float yaw;
200
201 if (vec[PITCH] == 0) {
202 yaw = 0;
203 if (vec[YAW] > 0)
204 yaw = 90;
205 else if (vec[YAW] < 0)
206 yaw = -90;
207 }
208 else {
209 yaw = (float)(atan2(vec[YAW], vec[PITCH]) * 180 / M_PI);
210 if (yaw < 0)
211 yaw += 360;
212 }
213
214 return yaw;
215 }
216