mathlib.c

   1 /*
   2 Copyright (C) 1996-1997 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 // mathlib.c -- math primitives
  21
  22 #include <math.h>
  23 #include "quakedef.h"
  24
  25 void Sys_Error (char *error, ...);
  26
  27 vec3_t vec3_origin = {0,0,0};
  28 int nanmask = 255<<23;
  29
  30 /*-----------------------------------------------------------------*/
  31
  32 #define DEG2RAD( a ) ( a * M_PI ) / 180.0F
  33
  34 void ProjectPointOnPlane( vec3_t dst, const vec3_t p, const vec3_t normal )
  35 {
  36         float d;
  37         vec3_t n;
  38         float inv_denom;
  39
  40         inv_denom = 1.0F / DotProduct( normal, normal );
  41
  42         d = DotProduct( normal, p ) * inv_denom;
  43
  44         n[0] = normal[0] * inv_denom;
  45         n[1] = normal[1] * inv_denom;
  46         n[2] = normal[2] * inv_denom;
  47
  48         dst[0] = p[0] - d * n[0];
  49         dst[1] = p[1] - d * n[1];
  50         dst[2] = p[2] - d * n[2];
  51 }
  52
  53 /*
  54 ** assumes "src" is normalized
  55 */
  56 void PerpendicularVector( vec3_t dst, const vec3_t src )
  57 {
  58         int     pos;
  59         int i;
  60         float minelem = 1.0F;
  61         vec3_t tempvec;
  62
  63         /*
  64         ** find the smallest magnitude axially aligned vector
  65         */
  66         for ( pos = 0, i = 0; i < 3; i++ )
  67         {
  68                 if ( fabs( src[i] ) < minelem )
  69                 {
  70                         pos = i;
  71                         minelem = fabs( src[i] );
  72                 }
  73         }
  74         tempvec[0] = tempvec[1] = tempvec[2] = 0.0F;
  75         tempvec[pos] = 1.0F;
  76
  77         /*
  78         ** project the point onto the plane defined by src
  79         */
  80         ProjectPointOnPlane( dst, tempvec, src );
  81
  82         /*
  83         ** normalize the result
  84         */
  85         VectorNormalize( dst );
  86 }
  87
  88 #ifdef _WIN32
  89 #pragma optimize( "", off )
  90 #endif
  91
  92
  93 void RotatePointAroundVector( vec3_t dst, const vec3_t dir, const vec3_t point, float degrees )
  94 {
  95         float   m[3][3];
  96         float   im[3][3];
  97         float   zrot[3][3];
  98         float   tmpmat[3][3];
  99         float   rot[3][3];
 100         int     i;
 101         vec3_t vr, vup, vf;
 102
 103         vf[0] = dir[0];
 104         vf[1] = dir[1];
 105         vf[2] = dir[2];
 106
 107         PerpendicularVector( vr, dir );
 108         CrossProduct( vr, vf, vup );
 109
 110         m[0][0] = vr[0];
 111         m[1][0] = vr[1];
 112         m[2][0] = vr[2];
 113
 114         m[0][1] = vup[0];
 115         m[1][1] = vup[1];
 116         m[2][1] = vup[2];
 117
 118         m[0][2] = vf[0];
 119         m[1][2] = vf[1];
 120         m[2][2] = vf[2];
 121
 122         memcpy( im, m, sizeof( im ) );
 123
 124         im[0][1] = m[1][0];
 125         im[0][2] = m[2][0];
 126         im[1][0] = m[0][1];
 127         im[1][2] = m[2][1];
 128         im[2][0] = m[0][2];
 129         im[2][1] = m[1][2];
 130
 131         memset( zrot, 0, sizeof( zrot ) );
 132         zrot[0][0] = zrot[1][1] = zrot[2][2] = 1.0F;
 133
 134         zrot[0][0] = cos( DEG2RAD( degrees ) );
 135         zrot[0][1] = sin( DEG2RAD( degrees ) );
 136         zrot[1][0] = -sin( DEG2RAD( degrees ) );
 137         zrot[1][1] = cos( DEG2RAD( degrees ) );
 138
 139         R_ConcatRotations( m, zrot, tmpmat );
 140         R_ConcatRotations( tmpmat, im, rot );
 141
 142         for ( i = 0; i < 3; i++ )
 143         {
 144                 dst[i] = rot[i][0] * point[0] + rot[i][1] * point[1] + rot[i][2] * point[2];
 145         }
 146 }
 147
 148 #ifdef _WIN32
 149 #pragma optimize( "", on )
 150 #endif
 151
 152 /*-----------------------------------------------------------------*/
 153
 154
 155 float   anglemod(float a)
 156 {
 157 #if 0
 158         if (a >= 0)
 159                 a -= 360*(int)(a/360);
 160         else
 161                 a += 360*( 1 + (int)(-a/360) );
 162 #endif
 163         a = (360.0/65536) * ((int)(a*(65536/360.0)) & 65535);
 164         return a;
 165 }
 166
 167 int BoxOnPlaneSide0 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2]) >= p->dist) | (((p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2]) < p->dist) << 1));}
 168 int BoxOnPlaneSide1 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2]) >= p->dist) | (((p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2]) < p->dist) << 1));}
 169 int BoxOnPlaneSide2 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2]) >= p->dist) | (((p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2]) < p->dist) << 1));}
 170 int BoxOnPlaneSide3 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2]) >= p->dist) | (((p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2]) < p->dist) << 1));}
 171 int BoxOnPlaneSide4 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2]) >= p->dist) | (((p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2]) < p->dist) << 1));}
 172 int BoxOnPlaneSide5 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2]) >= p->dist) | (((p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2]) < p->dist) << 1));}
 173 int BoxOnPlaneSide6 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2]) >= p->dist) | (((p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2]) < p->dist) << 1));}
 174 int BoxOnPlaneSide7 (vec3_t emins, vec3_t emaxs, mplane_t *p) {return (((p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2]) >= p->dist) | (((p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2]) < p->dist) << 1));}
 175
 176 void BoxOnPlaneSideClassify(mplane_t *p)
 177 {
 178         if (p->normal[2] < 0) // 4
 179         {
 180                 if (p->normal[1] < 0) // 2
 181                 {
 182                         if (p->normal[0] < 0) // 1
 183                                 p->BoxOnPlaneSideFunc = BoxOnPlaneSide7;
 184                         else
 185                                 p->BoxOnPlaneSideFunc = BoxOnPlaneSide6;
 186                 }
 187                 else
 188                 {
 189                         if (p->normal[0] < 0) // 1
 190                                 p->BoxOnPlaneSideFunc = BoxOnPlaneSide5;
 191                         else
 192                                 p->BoxOnPlaneSideFunc = BoxOnPlaneSide4;
 193                 }
 194         }
 195         else
 196         {
 197                 if (p->normal[1] < 0) // 2
 198                 {
 199                         if (p->normal[0] < 0) // 1
 200                                 p->BoxOnPlaneSideFunc = BoxOnPlaneSide3;
 201                         else
 202                                 p->BoxOnPlaneSideFunc = BoxOnPlaneSide2;
 203                 }
 204                 else
 205                 {
 206                         if (p->normal[0] < 0) // 1
 207                                 p->BoxOnPlaneSideFunc = BoxOnPlaneSide1;
 208                         else
 209                                 p->BoxOnPlaneSideFunc = BoxOnPlaneSide0;
 210                 }
 211         }
 212 }
 213
 214 void AngleVectors (vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
 215 {
 216         float           angle;
 217         float           sr, sp, sy, cr, cp, cy;
 218
 219         angle = angles[YAW] * (M_PI*2 / 360);
 220         sy = sin(angle);
 221         cy = cos(angle);
 222         angle = angles[PITCH] * (M_PI*2 / 360);
 223         sp = sin(angle);
 224         cp = cos(angle);
 225         angle = angles[ROLL] * (M_PI*2 / 360);
 226         sr = sin(angle);
 227         cr = cos(angle);
 228
 229         forward[0] = cp*cy;
 230         forward[1] = cp*sy;
 231         forward[2] = -sp;
 232         right[0] = (-1*sr*sp*cy+-1*cr*-sy);
 233         right[1] = (-1*sr*sp*sy+-1*cr*cy);
 234         right[2] = -1*sr*cp;
 235         up[0] = (cr*sp*cy+-sr*-sy);
 236         up[1] = (cr*sp*sy+-sr*cy);
 237         up[2] = cr*cp;
 238 }
 239
 240 int VectorCompare (vec3_t v1, vec3_t v2)
 241 {
 242         int             i;
 243
 244         for (i=0 ; i<3 ; i++)
 245                 if (v1[i] != v2[i])
 246                         return 0;
 247
 248         return 1;
 249 }
 250
 251 void VectorMA (vec3_t veca, float scale, vec3_t vecb, vec3_t vecc)
 252 {
 253         vecc[0] = veca[0] + scale*vecb[0];
 254         vecc[1] = veca[1] + scale*vecb[1];
 255         vecc[2] = veca[2] + scale*vecb[2];
 256 }
 257
 258
 259 vec_t _DotProduct (vec3_t v1, vec3_t v2)
 260 {
 261         return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
 262 }
 263
 264 void _VectorSubtract (vec3_t veca, vec3_t vecb, vec3_t out)
 265 {
 266         out[0] = veca[0]-vecb[0];
 267         out[1] = veca[1]-vecb[1];
 268         out[2] = veca[2]-vecb[2];
 269 }
 270
 271 void _VectorAdd (vec3_t veca, vec3_t vecb, vec3_t out)
 272 {
 273         out[0] = veca[0]+vecb[0];
 274         out[1] = veca[1]+vecb[1];
 275         out[2] = veca[2]+vecb[2];
 276 }
 277
 278 void _VectorCopy (vec3_t in, vec3_t out)
 279 {
 280         out[0] = in[0];
 281         out[1] = in[1];
 282         out[2] = in[2];
 283 }
 284
 285 // LordHavoc: changed CrossProduct to a #define
 286 /*
 287 void CrossProduct (vec3_t v1, vec3_t v2, vec3_t cross)
 288 {
 289         cross[0] = v1[1]*v2[2] - v1[2]*v2[1];
 290         cross[1] = v1[2]*v2[0] - v1[0]*v2[2];
 291         cross[2] = v1[0]*v2[1] - v1[1]*v2[0];
 292 }
 293 */
 294
 295 double sqrt(double x);
 296
 297 vec_t Length(vec3_t v)
 298 {
 299         int             i;
 300         float   length;
 301
 302         length = 0;
 303         for (i=0 ; i< 3 ; i++)
 304                 length += v[i]*v[i];
 305         length = sqrt (length);         // FIXME
 306
 307         return length;
 308 }
 309
 310 // LordHavoc: renamed these to Length, and made the normal ones #define
 311 float VectorNormalizeLength (vec3_t v)
 312 {
 313         float   length, ilength;
 314
 315         length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
 316         length = sqrt (length);         // FIXME
 317
 318         if (length)
 319         {
 320                 ilength = 1/length;
 321                 v[0] *= ilength;
 322                 v[1] *= ilength;
 323                 v[2] *= ilength;
 324         }
 325
 326         return length;
 327
 328 }
 329
 330 float VectorNormalizeLength2 (vec3_t v, vec3_t dest) // LordHavoc: added to allow copying while doing the calculation...
 331 {
 332         float   length, ilength;
 333
 334         length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
 335         length = sqrt (length);         // FIXME
 336
 337         if (length)
 338         {
 339                 ilength = 1/length;
 340                 dest[0] = v[0] * ilength;
 341                 dest[1] = v[1] * ilength;
 342                 dest[2] = v[2] * ilength;
 343         }
 344         else
 345                 dest[0] = dest[1] = dest[2] = 0;
 346
 347         return length;
 348
 349 }
 350
 351 void VectorInverse (vec3_t v)
 352 {
 353         v[0] = -v[0];
 354         v[1] = -v[1];
 355         v[2] = -v[2];
 356 }
 357
 358 void VectorScale (vec3_t in, vec_t scale, vec3_t out)
 359 {
 360         out[0] = in[0]*scale;
 361         out[1] = in[1]*scale;
 362         out[2] = in[2]*scale;
 363 }
 364
 365
 366 int Q_log2(int val)
 367 {
 368         int answer=0;
 369         while (val>>=1)
 370                 answer++;
 371         return answer;
 372 }
 373
 374
 375 /*
 376 ================
 377 R_ConcatRotations
 378 ================
 379 */
 380 void R_ConcatRotations (float in1[3][3], float in2[3][3], float out[3][3])
 381 {
 382         out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] + in1[0][2] * in2[2][0];
 383         out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] + in1[0][2] * in2[2][1];
 384         out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] + in1[0][2] * in2[2][2];
 385         out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] + in1[1][2] * in2[2][0];
 386         out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] + in1[1][2] * in2[2][1];
 387         out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] + in1[1][2] * in2[2][2];
 388         out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] + in1[2][2] * in2[2][0];
 389         out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] + in1[2][2] * in2[2][1];
 390         out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] + in1[2][2] * in2[2][2];
 391 }
 392
 393
 394 /*
 395 ================
 396 R_ConcatTransforms
 397 ================
 398 */
 399 void R_ConcatTransforms (float in1[3][4], float in2[3][4], float out[3][4])
 400 {
 401         out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] + in1[0][2] * in2[2][0];
 402         out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] + in1[0][2] * in2[2][1];
 403         out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] + in1[0][2] * in2[2][2];
 404         out[0][3] = in1[0][0] * in2[0][3] + in1[0][1] * in2[1][3] + in1[0][2] * in2[2][3] + in1[0][3];
 405         out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] + in1[1][2] * in2[2][0];
 406         out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] + in1[1][2] * in2[2][1];
 407         out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] + in1[1][2] * in2[2][2];
 408         out[1][3] = in1[1][0] * in2[0][3] + in1[1][1] * in2[1][3] + in1[1][2] * in2[2][3] + in1[1][3];
 409         out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] + in1[2][2] * in2[2][0];
 410         out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] + in1[2][2] * in2[2][1];
 411         out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] + in1[2][2] * in2[2][2];
 412         out[2][3] = in1[2][0] * in2[0][3] + in1[2][1] * in2[1][3] + in1[2][2] * in2[2][3] + in1[2][3];
 413 }
 414
 415
 416 /*
 417 ===================
 418 FloorDivMod
 419
 420 Returns mathematically correct (floor-based) quotient and remainder for
 421 numer and denom, both of which should contain no fractional part. The
 422 quotient must fit in 32 bits.
 423 ====================
 424 */
 425
 426 void FloorDivMod (double numer, double denom, int *quotient,
 427                 int *rem)
 428 {
 429         int             q, r;
 430         double  x;
 431
 432 #ifndef PARANOID
 433         if (denom <= 0.0)
 434                 Sys_Error ("FloorDivMod: bad denominator %d\n", denom);
 435
 436 //      if ((floor(numer) != numer) || (floor(denom) != denom))
 437 //              Sys_Error ("FloorDivMod: non-integer numer or denom %f %f\n",
 438 //                              numer, denom);
 439 #endif
 440
 441         if (numer >= 0.0)
 442         {
 443
 444                 x = floor(numer / denom);
 445                 q = (int)x;
 446                 r = (int)floor(numer - (x * denom));
 447         }
 448         else
 449         {
 450         //
 451         // perform operations with positive values, and fix mod to make floor-based
 452         //
 453                 x = floor(-numer / denom);
 454                 q = -(int)x;
 455                 r = (int)floor(-numer - (x * denom));
 456                 if (r != 0)
 457                 {
 458                         q--;
 459                         r = (int)denom - r;
 460                 }
 461         }
 462
 463         *quotient = q;
 464         *rem = r;
 465 }
 466
 467
 468 /*
 469 ===================
 470 GreatestCommonDivisor
 471 ====================
 472 */
 473 int GreatestCommonDivisor (int i1, int i2)
 474 {
 475         if (i1 > i2)
 476         {
 477                 if (i2 == 0)
 478                         return (i1);
 479                 return GreatestCommonDivisor (i2, i1 % i2);
 480         }
 481         else
 482         {
 483                 if (i1 == 0)
 484                         return (i2);
 485                 return GreatestCommonDivisor (i1, i2 % i1);
 486         }
 487 }