l_math.c (5868B)
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 // mathlib.c -- math primitives 23 24 #include "l_cmd.h" 25 #include "l_math.h" 26 27 vec3_t vec3_origin = {0,0,0}; 28 29 void AngleVectors (const vec3_t angles, vec3_t forward, vec3_t right, vec3_t up) 30 { 31 float angle; 32 static float sr, sp, sy, cr, cp, cy; 33 // static to help MS compiler fp bugs 34 35 angle = angles[YAW] * (M_PI*2 / 360); 36 sy = sin(angle); 37 cy = cos(angle); 38 angle = angles[PITCH] * (M_PI*2 / 360); 39 sp = sin(angle); 40 cp = cos(angle); 41 angle = angles[ROLL] * (M_PI*2 / 360); 42 sr = sin(angle); 43 cr = cos(angle); 44 45 if (forward) 46 { 47 forward[0] = cp*cy; 48 forward[1] = cp*sy; 49 forward[2] = -sp; 50 } 51 if (right) 52 { 53 right[0] = (-1*sr*sp*cy+-1*cr*-sy); 54 right[1] = (-1*sr*sp*sy+-1*cr*cy); 55 right[2] = -1*sr*cp; 56 } 57 if (up) 58 { 59 up[0] = (cr*sp*cy+-sr*-sy); 60 up[1] = (cr*sp*sy+-sr*cy); 61 up[2] = cr*cp; 62 } 63 } 64 65 /* 66 ================= 67 RadiusFromBounds 68 ================= 69 */ 70 float RadiusFromBounds( const vec3_t mins, const vec3_t maxs ) { 71 int i; 72 vec3_t corner; 73 float a, b; 74 75 for (i=0 ; i<3 ; i++) { 76 a = fabs( mins[i] ); 77 b = fabs( maxs[i] ); 78 corner[i] = a > b ? a : b; 79 } 80 81 return VectorLength (corner); 82 } 83 84 /* 85 ================ 86 R_ConcatRotations 87 ================ 88 */ 89 void R_ConcatRotations (float in1[3][3], float in2[3][3], float out[3][3]) 90 { 91 out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] + 92 in1[0][2] * in2[2][0]; 93 out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] + 94 in1[0][2] * in2[2][1]; 95 out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] + 96 in1[0][2] * in2[2][2]; 97 out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] + 98 in1[1][2] * in2[2][0]; 99 out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] + 100 in1[1][2] * in2[2][1]; 101 out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] + 102 in1[1][2] * in2[2][2]; 103 out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] + 104 in1[2][2] * in2[2][0]; 105 out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] + 106 in1[2][2] * in2[2][1]; 107 out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] + 108 in1[2][2] * in2[2][2]; 109 } 110 111 void AxisClear( vec3_t axis[3] ) { 112 axis[0][0] = 1; 113 axis[0][1] = 0; 114 axis[0][2] = 0; 115 axis[1][0] = 0; 116 axis[1][1] = 1; 117 axis[1][2] = 0; 118 axis[2][0] = 0; 119 axis[2][1] = 0; 120 axis[2][2] = 1; 121 } 122 123 float VectorLengthSquared(vec3_t v) { 124 return DotProduct(v, v); 125 } 126 127 double VectorLength(vec3_t v) 128 { 129 int i; 130 double length; 131 132 length = 0; 133 for (i=0 ; i< 3 ; i++) 134 length += v[i]*v[i]; 135 length = sqrt (length); // FIXME 136 137 return length; 138 } 139 140 qboolean VectorCompare (vec3_t v1, vec3_t v2) 141 { 142 int i; 143 144 for (i=0 ; i<3 ; i++) 145 if (fabs(v1[i]-v2[i]) > EQUAL_EPSILON) 146 return false; 147 148 return true; 149 } 150 151 vec_t Q_rint (vec_t in) 152 { 153 return floor(in + 0.5); 154 } 155 156 void CrossProduct (const vec3_t v1, const vec3_t v2, vec3_t cross) 157 { 158 cross[0] = v1[1]*v2[2] - v1[2]*v2[1]; 159 cross[1] = v1[2]*v2[0] - v1[0]*v2[2]; 160 cross[2] = v1[0]*v2[1] - v1[1]*v2[0]; 161 } 162 163 void _VectorMA (vec3_t va, double scale, vec3_t vb, vec3_t vc) 164 { 165 vc[0] = va[0] + scale*vb[0]; 166 vc[1] = va[1] + scale*vb[1]; 167 vc[2] = va[2] + scale*vb[2]; 168 } 169 170 vec_t _DotProduct (vec3_t v1, vec3_t v2) 171 { 172 return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]; 173 } 174 175 void _VectorSubtract (vec3_t va, vec3_t vb, vec3_t out) 176 { 177 out[0] = va[0]-vb[0]; 178 out[1] = va[1]-vb[1]; 179 out[2] = va[2]-vb[2]; 180 } 181 182 void _VectorAdd (vec3_t va, vec3_t vb, vec3_t out) 183 { 184 out[0] = va[0]+vb[0]; 185 out[1] = va[1]+vb[1]; 186 out[2] = va[2]+vb[2]; 187 } 188 189 void _VectorCopy (vec3_t in, vec3_t out) 190 { 191 out[0] = in[0]; 192 out[1] = in[1]; 193 out[2] = in[2]; 194 } 195 196 void _VectorScale (vec3_t v, vec_t scale, vec3_t out) 197 { 198 out[0] = v[0] * scale; 199 out[1] = v[1] * scale; 200 out[2] = v[2] * scale; 201 } 202 203 vec_t VectorNormalize(vec3_t inout) 204 { 205 vec_t length, ilength; 206 207 length = sqrt (inout[0]*inout[0] + inout[1]*inout[1] + inout[2]*inout[2]); 208 if (length == 0) 209 { 210 VectorClear (inout); 211 return 0; 212 } 213 214 ilength = 1.0/length; 215 inout[0] = inout[0]*ilength; 216 inout[1] = inout[1]*ilength; 217 inout[2] = inout[2]*ilength; 218 219 return length; 220 } 221 222 vec_t VectorNormalize2(const vec3_t in, vec3_t out) 223 { 224 vec_t length, ilength; 225 226 length = sqrt (in[0]*in[0] + in[1]*in[1] + in[2]*in[2]); 227 if (length == 0) 228 { 229 VectorClear (out); 230 return 0; 231 } 232 233 ilength = 1.0/length; 234 out[0] = in[0]*ilength; 235 out[1] = in[1]*ilength; 236 out[2] = in[2]*ilength; 237 238 return length; 239 } 240 241 vec_t ColorNormalize (vec3_t in, vec3_t out) 242 { 243 float max, scale; 244 245 max = in[0]; 246 if (in[1] > max) 247 max = in[1]; 248 if (in[2] > max) 249 max = in[2]; 250 251 if (max == 0) 252 return 0; 253 254 scale = 1.0 / max; 255 256 VectorScale (in, scale, out); 257 258 return max; 259 } 260 261 262 263 void VectorInverse (vec3_t v) 264 { 265 v[0] = -v[0]; 266 v[1] = -v[1]; 267 v[2] = -v[2]; 268 } 269 270 void ClearBounds(vec3_t mins, vec3_t maxs) 271 { 272 mins[0] = mins[1] = mins[2] = 99999; 273 maxs[0] = maxs[1] = maxs[2] = -99999; 274 } 275 276 void AddPointToBounds(const vec3_t v, vec3_t mins, vec3_t maxs) 277 { 278 int i; 279 vec_t val; 280 281 for (i=0 ; i<3 ; i++) 282 { 283 val = v[i]; 284 if (val < mins[i]) 285 mins[i] = val; 286 if (val > maxs[i]) 287 maxs[i] = val; 288 } 289 }