Games/Quake/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 /*
 168 ==================
 169 BOPS_Error
 170
 171 Split out like this for ASM to call.
 172 ==================
 173 */
 174 void BOPS_Error (void)
 175 {
 176         Sys_Error ("BoxOnPlaneSide:  Bad signbits");
 177 }
 178
 179
 180 #if     !id386
 181
 182 /*
 183 ==================
 184 BoxOnPlaneSide
 185
 186 Returns 1, 2, or 1 + 2
 187 ==================
 188 */
 189 int BoxOnPlaneSide (vec3_t emins, vec3_t emaxs, mplane_t *p)
 190 {
 191         float   dist1, dist2;
 192         int             sides;
 193
 194 #if 0   // this is done by the BOX_ON_PLANE_SIDE macro before calling this
 195                 // function
 196 // fast axial cases
 197         if (p->type < 3)
 198         {
 199                 if (p->dist <= emins[p->type])
 200                         return 1;
 201                 if (p->dist >= emaxs[p->type])
 202                         return 2;
 203                 return 3;
 204         }
 205 #endif
 206
 207 // general case
 208         switch (p->signbits)
 209         {
 210         case 0:
 211 dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
 212 dist2 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
 213                 break;
 214         case 1:
 215 dist1 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
 216 dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
 217                 break;
 218         case 2:
 219 dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
 220 dist2 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
 221                 break;
 222         case 3:
 223 dist1 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
 224 dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
 225                 break;
 226         case 4:
 227 dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
 228 dist2 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
 229                 break;
 230         case 5:
 231 dist1 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
 232 dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
 233                 break;
 234         case 6:
 235 dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
 236 dist2 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
 237                 break;
 238         case 7:
 239 dist1 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
 240 dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
 241                 break;
 242         default:
 243                 dist1 = dist2 = 0;              // shut up compiler
 244                 BOPS_Error ();
 245                 break;
 246         }
 247
 248 #if 0
 249         int             i;
 250         vec3_t  corners[2];
 251
 252         for (i=0 ; i<3 ; i++)
 253         {
 254                 if (plane->normal[i] < 0)
 255                 {
 256                         corners[0][i] = emins[i];
 257                         corners[1][i] = emaxs[i];
 258                 }
 259                 else
 260                 {
 261                         corners[1][i] = emins[i];
 262                         corners[0][i] = emaxs[i];
 263                 }
 264         }
 265         dist = DotProduct (plane->normal, corners[0]) - plane->dist;
 266         dist2 = DotProduct (plane->normal, corners[1]) - plane->dist;
 267         sides = 0;
 268         if (dist1 >= 0)
 269                 sides = 1;
 270         if (dist2 < 0)
 271                 sides |= 2;
 272
 273 #endif
 274
 275         sides = 0;
 276         if (dist1 >= p->dist)
 277                 sides = 1;
 278         if (dist2 < p->dist)
 279                 sides |= 2;
 280
 281 #ifdef PARANOID
 282 if (sides == 0)
 283         Sys_Error ("BoxOnPlaneSide: sides==0");
 284 #endif
 285
 286         return sides;
 287 }
 288
 289 #endif
 290
 291
 292 void AngleVectors (vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
 293 {
 294         float           angle;
 295         float           sr, sp, sy, cr, cp, cy;
 296
 297         angle = angles[YAW] * (M_PI*2 / 360);
 298         sy = sin(angle);
 299         cy = cos(angle);
 300         angle = angles[PITCH] * (M_PI*2 / 360);
 301         sp = sin(angle);
 302         cp = cos(angle);
 303         angle = angles[ROLL] * (M_PI*2 / 360);
 304         sr = sin(angle);
 305         cr = cos(angle);
 306
 307         forward[0] = cp*cy;
 308         forward[1] = cp*sy;
 309         forward[2] = -sp;
 310         right[0] = (-1*sr*sp*cy+-1*cr*-sy);
 311         right[1] = (-1*sr*sp*sy+-1*cr*cy);
 312         right[2] = -1*sr*cp;
 313         up[0] = (cr*sp*cy+-sr*-sy);
 314         up[1] = (cr*sp*sy+-sr*cy);
 315         up[2] = cr*cp;
 316 }
 317
 318 int VectorCompare (vec3_t v1, vec3_t v2)
 319 {
 320         int             i;
 321
 322         for (i=0 ; i<3 ; i++)
 323                 if (v1[i] != v2[i])
 324                         return 0;
 325
 326         return 1;
 327 }
 328
 329 void VectorMA (vec3_t veca, float scale, vec3_t vecb, vec3_t vecc)
 330 {
 331         vecc[0] = veca[0] + scale*vecb[0];
 332         vecc[1] = veca[1] + scale*vecb[1];
 333         vecc[2] = veca[2] + scale*vecb[2];
 334 }
 335
 336
 337 vec_t _DotProduct (vec3_t v1, vec3_t v2)
 338 {
 339         return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
 340 }
 341
 342 void _VectorSubtract (vec3_t veca, vec3_t vecb, vec3_t out)
 343 {
 344         out[0] = veca[0]-vecb[0];
 345         out[1] = veca[1]-vecb[1];
 346         out[2] = veca[2]-vecb[2];
 347 }
 348
 349 void _VectorAdd (vec3_t veca, vec3_t vecb, vec3_t out)
 350 {
 351         out[0] = veca[0]+vecb[0];
 352         out[1] = veca[1]+vecb[1];
 353         out[2] = veca[2]+vecb[2];
 354 }
 355
 356 void _VectorCopy (vec3_t in, vec3_t out)
 357 {
 358         out[0] = in[0];
 359         out[1] = in[1];
 360         out[2] = in[2];
 361 }
 362
 363 void CrossProduct (vec3_t v1, vec3_t v2, vec3_t cross)
 364 {
 365         cross[0] = v1[1]*v2[2] - v1[2]*v2[1];
 366         cross[1] = v1[2]*v2[0] - v1[0]*v2[2];
 367         cross[2] = v1[0]*v2[1] - v1[1]*v2[0];
 368 }
 369
 370 double sqrt(double x);
 371
 372 vec_t Length(vec3_t v)
 373 {
 374         int             i;
 375         float   length;
 376
 377         length = 0;
 378         for (i=0 ; i< 3 ; i++)
 379                 length += v[i]*v[i];
 380         length = sqrt (length);         // FIXME
 381
 382         return length;
 383 }
 384
 385 float VectorNormalize (vec3_t v)
 386 {
 387         float   length, ilength;
 388
 389         length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
 390         length = sqrt (length);         // FIXME
 391
 392         if (length)
 393         {
 394                 ilength = 1/length;
 395                 v[0] *= ilength;
 396                 v[1] *= ilength;
 397                 v[2] *= ilength;
 398         }
 399
 400         return length;
 401
 402 }
 403
 404 void VectorInverse (vec3_t v)
 405 {
 406         v[0] = -v[0];
 407         v[1] = -v[1];
 408         v[2] = -v[2];
 409 }
 410
 411 void VectorScale (vec3_t in, vec_t scale, vec3_t out)
 412 {
 413         out[0] = in[0]*scale;
 414         out[1] = in[1]*scale;
 415         out[2] = in[2]*scale;
 416 }
 417
 418
 419 int Q_log2(int val)
 420 {
 421         int answer=0;
 422         while (val>>=1)
 423                 answer++;
 424         return answer;
 425 }
 426
 427
 428 /*
 429 ================
 430 R_ConcatRotations
 431 ================
 432 */
 433 void R_ConcatRotations (float in1[3][3], float in2[3][3], float out[3][3])
 434 {
 435         out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] +
 436                                 in1[0][2] * in2[2][0];
 437         out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] +
 438                                 in1[0][2] * in2[2][1];
 439         out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] +
 440                                 in1[0][2] * in2[2][2];
 441         out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] +
 442                                 in1[1][2] * in2[2][0];
 443         out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] +
 444                                 in1[1][2] * in2[2][1];
 445         out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] +
 446                                 in1[1][2] * in2[2][2];
 447         out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] +
 448                                 in1[2][2] * in2[2][0];
 449         out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] +
 450                                 in1[2][2] * in2[2][1];
 451         out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] +
 452                                 in1[2][2] * in2[2][2];
 453 }
 454
 455
 456 /*
 457 ================
 458 R_ConcatTransforms
 459 ================
 460 */
 461 void R_ConcatTransforms (float in1[3][4], float in2[3][4], float out[3][4])
 462 {
 463         out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] +
 464                                 in1[0][2] * in2[2][0];
 465         out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] +
 466                                 in1[0][2] * in2[2][1];
 467         out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] +
 468                                 in1[0][2] * in2[2][2];
 469         out[0][3] = in1[0][0] * in2[0][3] + in1[0][1] * in2[1][3] +
 470                                 in1[0][2] * in2[2][3] + in1[0][3];
 471         out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] +
 472                                 in1[1][2] * in2[2][0];
 473         out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] +
 474                                 in1[1][2] * in2[2][1];
 475         out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] +
 476                                 in1[1][2] * in2[2][2];
 477         out[1][3] = in1[1][0] * in2[0][3] + in1[1][1] * in2[1][3] +
 478                                 in1[1][2] * in2[2][3] + in1[1][3];
 479         out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] +
 480                                 in1[2][2] * in2[2][0];
 481         out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] +
 482                                 in1[2][2] * in2[2][1];
 483         out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] +
 484                                 in1[2][2] * in2[2][2];
 485         out[2][3] = in1[2][0] * in2[0][3] + in1[2][1] * in2[1][3] +
 486                                 in1[2][2] * in2[2][3] + in1[2][3];
 487 }
 488
 489
 490 /*
 491 ===================
 492 FloorDivMod
 493
 494 Returns mathematically correct (floor-based) quotient and remainder for
 495 numer and denom, both of which should contain no fractional part. The
 496 quotient must fit in 32 bits.
 497 ====================
 498 */
 499
 500 void FloorDivMod (double numer, double denom, int *quotient,
 501                 int *rem)
 502 {
 503         int             q, r;
 504         double  x;
 505
 506 #ifndef PARANOID
 507         if (denom <= 0.0)
 508                 Sys_Error ("FloorDivMod: bad denominator %d\n", denom);
 509
 510 //      if ((floor(numer) != numer) || (floor(denom) != denom))
 511 //              Sys_Error ("FloorDivMod: non-integer numer or denom %f %f\n",
 512 //                              numer, denom);
 513 #endif
 514
 515         if (numer >= 0.0)
 516         {
 517
 518                 x = floor(numer / denom);
 519                 q = (int)x;
 520                 r = (int)floor(numer - (x * denom));
 521         }
 522         else
 523         {
 524         //
 525         // perform operations with positive values, and fix mod to make floor-based
 526         //
 527                 x = floor(-numer / denom);
 528                 q = -(int)x;
 529                 r = (int)floor(-numer - (x * denom));
 530                 if (r != 0)
 531                 {
 532                         q--;
 533                         r = (int)denom - r;
 534                 }
 535         }
 536
 537         *quotient = q;
 538         *rem = r;
 539 }
 540
 541
 542 /*
 543 ===================
 544 GreatestCommonDivisor
 545 ====================
 546 */
 547 int GreatestCommonDivisor (int i1, int i2)
 548 {
 549         if (i1 > i2)
 550         {
 551                 if (i2 == 0)
 552                         return (i1);
 553                 return GreatestCommonDivisor (i2, i1 % i2);
 554         }
 555         else
 556         {
 557                 if (i1 == 0)
 558                         return (i2);
 559                 return GreatestCommonDivisor (i1, i2 % i1);
 560         }
 561 }
 562
 563
 564 #if     !id386
 565
 566 // TODO: move to nonintel.c
 567
 568 /*
 569 ===================
 570 Invert24To16
 571
 572 Inverts an 8.24 value to a 16.16 value
 573 ====================
 574 */
 575
 576 fixed16_t Invert24To16(fixed16_t val)
 577 {
 578         if (val < 256)
 579                 return (0xFFFFFFFF);
 580
 581         return (fixed16_t)
 582                         (((double)0x10000 * (double)0x1000000 / (double)val) + 0.5);
 583 }
 584
 585 #endif