maths/vecmat.c

   1 /*
   2 THE COMPUTER CODE CONTAINED HEREIN IS THE SOLE PROPERTY OF PARALLAX
   3 SOFTWARE CORPORATION ("PARALLAX").  PARALLAX, IN DISTRIBUTING THE CODE TO
   4 END-USERS, AND SUBJECT TO ALL OF THE TERMS AND CONDITIONS HEREIN, GRANTS A
   5 ROYALTY-FREE, PERPETUAL LICENSE TO SUCH END-USERS FOR USE BY SUCH END-USERS
   6 IN USING, DISPLAYING,  AND CREATING DERIVATIVE WORKS THEREOF, SO LONG AS
   7 SUCH USE, DISPLAY OR CREATION IS FOR NON-COMMERCIAL, ROYALTY OR REVENUE
   8 FREE PURPOSES.  IN NO EVENT SHALL THE END-USER USE THE COMPUTER CODE
   9 CONTAINED HEREIN FOR REVENUE-BEARING PURPOSES.  THE END-USER UNDERSTANDS
  10 AND AGREES TO THE TERMS HEREIN AND ACCEPTS THE SAME BY USE OF THIS FILE.
  11 COPYRIGHT 1993-1998 PARALLAX SOFTWARE CORPORATION.  ALL RIGHTS RESERVED.
  12 */
  13
  14 /*
  15  *
  16  * C version of vecmat library
  17  *
  18  */
  19
  20 #ifdef HAVE_CONFIG_H
  21 #include <conf.h>
  22 #endif
  23
  24 #include <stdlib.h>
  25 #include <math.h>           // for sqrt
  26
  27 #include "maths.h"
  28 #include "error.h"
  29
  30 //#define USE_ISQRT 1
  31
  32 #ifdef NO_ASM
  33 vms_vector vmd_zero_vector = {0, 0, 0};
  34 vms_matrix vmd_identity_matrix = { { f1_0, 0, 0 },
  35                                    { 0, f1_0, 0 },
  36                                    { 0, 0, f1_0 } };
  37
  38 //adds two vectors, fills in dest, returns ptr to dest
  39 //ok for dest to equal either source, but should use vm_vec_add2() if so
  40 vms_vector *vm_vec_add(vms_vector *dest,const vms_vector *src0,const vms_vector *src1)
  41 {
  42         dest->x = src0->x + src1->x;
  43         dest->y = src0->y + src1->y;
  44         dest->z = src0->z + src1->z;
  45
  46         return dest;
  47 }
  48
  49
  50 //subs two vectors, fills in dest, returns ptr to dest
  51 //ok for dest to equal either source, but should use vm_vec_sub2() if so
  52 vms_vector *vm_vec_sub(vms_vector *dest,const vms_vector *src0,const vms_vector *src1)
  53 {
  54         dest->x = src0->x - src1->x;
  55         dest->y = src0->y - src1->y;
  56         dest->z = src0->z - src1->z;
  57
  58         return dest;
  59 }
  60
  61 //adds one vector to another. returns ptr to dest
  62 //dest can equal source
  63 vms_vector *vm_vec_add2(vms_vector *dest,const vms_vector *src)
  64 {
  65         dest->x += src->x;
  66         dest->y += src->y;
  67         dest->z += src->z;
  68
  69         return dest;
  70 }
  71
  72 //subs one vector from another, returns ptr to dest
  73 //dest can equal source
  74 vms_vector *vm_vec_sub2(vms_vector *dest,const vms_vector *src)
  75 {
  76         dest->x -= src->x;
  77         dest->y -= src->y;
  78         dest->z -= src->z;
  79
  80         return dest;
  81 }
  82
  83 //averages two vectors. returns ptr to dest
  84 //dest can equal either source
  85 vms_vector *vm_vec_avg(vms_vector *dest,const vms_vector *src0,const vms_vector *src1)
  86 {
  87         dest->x = (src0->x + src1->x)/2;
  88         dest->y = (src0->y + src1->y)/2;
  89         dest->z = (src0->z + src1->z)/2;
  90
  91         return dest;
  92 }
  93
  94
  95 //averages four vectors. returns ptr to dest
  96 //dest can equal any source
  97 vms_vector *vm_vec_avg4(vms_vector *dest,const vms_vector *src0,const vms_vector *src1,const vms_vector *src2,const vms_vector *src3)
  98 {
  99         dest->x = (src0->x + src1->x + src2->x + src3->x)/4;
 100         dest->y = (src0->y + src1->y + src2->y + src3->y)/4;
 101         dest->z = (src0->z + src1->z + src2->z + src3->z)/4;
 102
 103         return dest;
 104 }
 105
 106
 107 //scales a vector in place.  returns ptr to vector
 108 vms_vector *vm_vec_scale(vms_vector *dest,fix s)
 109 {
 110         dest->x = fixmul(dest->x,s);
 111         dest->y = fixmul(dest->y,s);
 112         dest->z = fixmul(dest->z,s);
 113
 114         return dest;
 115 }
 116
 117 //scales and copies a vector.  returns ptr to dest
 118 vms_vector *vm_vec_copy_scale(vms_vector *dest,const vms_vector *src,fix s)
 119 {
 120         dest->x = fixmul(src->x,s);
 121         dest->y = fixmul(src->y,s);
 122         dest->z = fixmul(src->z,s);
 123
 124         return dest;
 125 }
 126
 127 //scales a vector, adds it to another, and stores in a 3rd vector
 128 //dest = src1 + k * src2
 129 vms_vector *vm_vec_scale_add(vms_vector *dest,const vms_vector *src1,const vms_vector *src2,fix k)
 130 {
 131         dest->x = src1->x + fixmul(src2->x,k);
 132         dest->y = src1->y + fixmul(src2->y,k);
 133         dest->z = src1->z + fixmul(src2->z,k);
 134
 135         return dest;
 136 }
 137
 138 //scales a vector and adds it to another
 139 //dest += k * src
 140 vms_vector *vm_vec_scale_add2(vms_vector *dest,const vms_vector *src,fix k)
 141 {
 142         dest->x += fixmul(src->x,k);
 143         dest->y += fixmul(src->y,k);
 144         dest->z += fixmul(src->z,k);
 145
 146         return dest;
 147 }
 148
 149 //scales a vector in place, taking n/d for scale.  returns ptr to vector
 150 //dest *= n/d
 151 vms_vector *vm_vec_scale2(vms_vector *dest,fix n,fix d)
 152 {
 153 #if 1 // DPH: Kludge: this was overflowing a lot, so I made it use the FPU.
 154         float nd;
 155 //      printf("scale n=%d d=%d\n",n,d);
 156         nd = f2fl(n) / f2fl(d);
 157         dest->x = fl2f( f2fl(dest->x) * nd);
 158         dest->y = fl2f( f2fl(dest->y) * nd);
 159         dest->z = fl2f( f2fl(dest->z) * nd);
 160 #else
 161         dest->x = fixmuldiv(dest->x,n,d);
 162         dest->y = fixmuldiv(dest->y,n,d);
 163         dest->z = fixmuldiv(dest->z,n,d);
 164 #endif
 165
 166         return dest;
 167 }
 168
 169 fix vm_vec_dotprod(const vms_vector *v0,const vms_vector *v1)
 170 {
 171         quadint q;
 172
 173         q.low = q.high = 0;
 174
 175         fixmulaccum(&q,v0->x,v1->x);
 176         fixmulaccum(&q,v0->y,v1->y);
 177         fixmulaccum(&q,v0->z,v1->z);
 178
 179         return fixquadadjust(&q);
 180 }
 181
 182 fix vm_vec_dot3(fix x,fix y,fix z,vms_vector *v)
 183 {
 184         quadint q;
 185
 186         q.low = q.high = 0;
 187
 188         fixmulaccum(&q,x,v->x);
 189         fixmulaccum(&q,y,v->y);
 190         fixmulaccum(&q,z,v->z);
 191
 192         return fixquadadjust(&q);
 193 }
 194
 195 //returns magnitude of a vector
 196 fix vm_vec_mag(vms_vector *v)
 197 {
 198         quadint q;
 199
 200         q.low = q.high = 0;
 201
 202         fixmulaccum(&q,v->x,v->x);
 203         fixmulaccum(&q,v->y,v->y);
 204         fixmulaccum(&q,v->z,v->z);
 205
 206         return quad_sqrt(q.low,q.high);
 207 }
 208
 209 //computes the distance between two points. (does sub and mag)
 210 fix vm_vec_dist(vms_vector *v0,vms_vector *v1)
 211 {
 212         vms_vector t;
 213
 214         vm_vec_sub(&t,v0,v1);
 215
 216         return vm_vec_mag(&t);
 217 }
 218
 219
 220 //computes an approximation of the magnitude of the vector
 221 //uses dist = largest + next_largest*3/8 + smallest*3/16
 222 fix vm_vec_mag_quick(vms_vector *v)
 223 {
 224         fix a,b,c,bc;
 225
 226         a = (fix)labs(v->x);
 227         b = (fix)labs(v->y);
 228         c = (fix)labs(v->z);
 229
 230         if (a < b) {
 231                 fix t=a; a=b; b=t;
 232         }
 233
 234         if (b < c) {
 235                 fix t=b; b=c; c=t;
 236
 237                 if (a < b) {
 238                         fix t=a; a=b; b=t;
 239                 }
 240         }
 241
 242         bc = (b>>2) + (c>>3);
 243
 244         return a + bc + (bc>>1);
 245 }
 246
 247
 248 //computes an approximation of the distance between two points.
 249 //uses dist = largest + next_largest*3/8 + smallest*3/16
 250 fix vm_vec_dist_quick(vms_vector *v0,vms_vector *v1)
 251 {
 252         vms_vector t;
 253
 254         vm_vec_sub(&t,v0,v1);
 255
 256         return vm_vec_mag_quick(&t);
 257 }
 258
 259 //normalize a vector. returns mag of source vec
 260 fix vm_vec_copy_normalize(vms_vector *dest,vms_vector *src)
 261 {
 262         fix m;
 263
 264         m = vm_vec_mag(src);
 265
 266         if (m > 0) {
 267                 dest->x = fixdiv(src->x,m);
 268                 dest->y = fixdiv(src->y,m);
 269                 dest->z = fixdiv(src->z,m);
 270         }
 271
 272         return m;
 273 }
 274
 275 //normalize a vector. returns mag of source vec
 276 fix vm_vec_normalize(vms_vector *v)
 277 {
 278         return vm_vec_copy_normalize(v,v);
 279 }
 280
 281 #ifndef USE_ISQRT
 282 //normalize a vector. returns mag of source vec. uses approx mag
 283 fix vm_vec_copy_normalize_quick(vms_vector *dest,vms_vector *src)
 284 {
 285         fix m;
 286
 287         m = vm_vec_mag_quick(src);
 288
 289         if (m > 0) {
 290                 dest->x = fixdiv(src->x,m);
 291                 dest->y = fixdiv(src->y,m);
 292                 dest->z = fixdiv(src->z,m);
 293         }
 294
 295         return m;
 296 }
 297
 298 #else
 299 //these routines use an approximation for 1/sqrt
 300
 301 //returns approximation of 1/magnitude of a vector
 302 fix vm_vec_imag(vms_vector *v)
 303 {
 304         quadint q;
 305
 306         q.low = q.high = 0;
 307
 308         fixmulaccum(&q,v->x,v->x);
 309         fixmulaccum(&q,v->y,v->y);
 310         fixmulaccum(&q,v->z,v->z);
 311
 312         if (q.high==0)
 313                 return fix_isqrt(fixquadadjust(&q));
 314         else if (q.high >= 0x800000) {
 315                 return (fix_isqrt(q.high) >> 8);
 316         }
 317         else
 318                 return (fix_isqrt((q.high<<8) + (q.low>>24)) >> 4);
 319 }
 320
 321 //normalize a vector. returns 1/mag of source vec. uses approx 1/mag
 322 fix vm_vec_copy_normalize_quick(vms_vector *dest,vms_vector *src)
 323 {
 324         fix im;
 325
 326         im = vm_vec_imag(src);
 327
 328         dest->x = fixmul(src->x,im);
 329         dest->y = fixmul(src->y,im);
 330         dest->z = fixmul(src->z,im);
 331
 332         return im;
 333 }
 334
 335 #endif
 336
 337 //normalize a vector. returns 1/mag of source vec. uses approx 1/mag
 338 fix vm_vec_normalize_quick(vms_vector *v)
 339 {
 340         return vm_vec_copy_normalize_quick(v,v);
 341 }
 342
 343 //return the normalized direction vector between two points
 344 //dest = normalized(end - start).  Returns 1/mag of direction vector
 345 //NOTE: the order of the parameters matches the vector subtraction
 346 fix vm_vec_normalized_dir_quick(vms_vector *dest,vms_vector *end,vms_vector *start)
 347 {
 348         vm_vec_sub(dest,end,start);
 349
 350         return vm_vec_normalize_quick(dest);
 351 }
 352
 353 //return the normalized direction vector between two points
 354 //dest = normalized(end - start).  Returns mag of direction vector
 355 //NOTE: the order of the parameters matches the vector subtraction
 356 fix vm_vec_normalized_dir(vms_vector *dest,vms_vector *end,vms_vector *start)
 357 {
 358         vm_vec_sub(dest,end,start);
 359
 360         return vm_vec_normalize(dest);
 361 }
 362
 363 //computes surface normal from three points. result is normalized
 364 //returns ptr to dest
 365 //dest CANNOT equal either source
 366 vms_vector *vm_vec_normal(vms_vector *dest,vms_vector *p0,vms_vector *p1,vms_vector *p2)
 367 {
 368         vm_vec_perp(dest,p0,p1,p2);
 369
 370         vm_vec_normalize(dest);
 371
 372         return dest;
 373 }
 374
 375 //make sure a vector is reasonably sized to go into a cross product
 376 void check_vec(vms_vector *v)
 377 {
 378         fix check;
 379         int cnt = 0;
 380
 381         check = (fix)labs(v->x) | (fix)labs(v->y) | (fix)labs(v->z);
 382
 383         if (check == 0)
 384                 return;
 385
 386         if (check & 0xfffc0000) {               //too big
 387
 388                 while (check & 0xfff00000) {
 389                         cnt += 4;
 390                         check >>= 4;
 391                 }
 392
 393                 while (check & 0xfffc0000) {
 394                         cnt += 2;
 395                         check >>= 2;
 396                 }
 397
 398                 v->x >>= cnt;
 399                 v->y >>= cnt;
 400                 v->z >>= cnt;
 401         }
 402         else                                                                                            //maybe too small
 403                 if ((check & 0xffff8000) == 0) {                //yep, too small
 404
 405                         while ((check & 0xfffff000) == 0) {
 406                                 cnt += 4;
 407                                 check <<= 4;
 408                         }
 409
 410                         while ((check & 0xffff8000) == 0) {
 411                                 cnt += 2;
 412                                 check <<= 2;
 413                         }
 414
 415                         v->x >>= cnt;
 416                         v->y >>= cnt;
 417                         v->z >>= cnt;
 418                 }
 419 }
 420
 421 //computes cross product of two vectors.
 422 //Note: this magnitude of the resultant vector is the
 423 //product of the magnitudes of the two source vectors.  This means it is
 424 //quite easy for this routine to overflow and underflow.  Be careful that
 425 //your inputs are ok.
 426 //#ifndef __powerc
 427 #if 0
 428 vms_vector *vm_vec_crossprod(vms_vector *dest,vms_vector *src0,vms_vector *src1)
 429 {
 430         double d;
 431         Assert(dest!=src0 && dest!=src1);
 432
 433         d = (double)(src0->y) * (double)(src1->z);
 434         d += (double)-(src0->z) * (double)(src1->y);
 435         d /= 65536.0;
 436         if (d < 0.0)
 437                 d = d - 1.0;
 438         dest->x = (fix)d;
 439
 440         d = (double)(src0->z) * (double)(src1->x);
 441         d += (double)-(src0->x) * (double)(src1->z);
 442         d /= 65536.0;
 443         if (d < 0.0)
 444                 d = d - 1.0;
 445         dest->y = (fix)d;
 446
 447         d = (double)(src0->x) * (double)(src1->y);
 448         d += (double)-(src0->y) * (double)(src1->x);
 449         d /= 65536.0;
 450         if (d < 0.0)
 451                 d = d - 1.0;
 452         dest->z = (fix)d;
 453
 454         return dest;
 455 }
 456 #else
 457
 458 vms_vector *vm_vec_crossprod(vms_vector *dest,vms_vector *src0,vms_vector *src1)
 459 {
 460         quadint q;
 461
 462         Assert(dest!=src0 && dest!=src1);
 463
 464         q.low = q.high = 0;
 465         fixmulaccum(&q,src0->y,src1->z);
 466         fixmulaccum(&q,-src0->z,src1->y);
 467         dest->x = fixquadadjust(&q);
 468
 469         q.low = q.high = 0;
 470         fixmulaccum(&q,src0->z,src1->x);
 471         fixmulaccum(&q,-src0->x,src1->z);
 472         dest->y = fixquadadjust(&q);
 473
 474         q.low = q.high = 0;
 475         fixmulaccum(&q,src0->x,src1->y);
 476         fixmulaccum(&q,-src0->y,src1->x);
 477         dest->z = fixquadadjust(&q);
 478
 479         return dest;
 480 }
 481
 482 #endif
 483
 484
 485 //computes non-normalized surface normal from three points.
 486 //returns ptr to dest
 487 //dest CANNOT equal either source
 488 vms_vector *vm_vec_perp(vms_vector *dest,vms_vector *p0,vms_vector *p1,vms_vector *p2)
 489 {
 490         vms_vector t0,t1;
 491
 492         vm_vec_sub(&t0,p1,p0);
 493         vm_vec_sub(&t1,p2,p1);
 494
 495         check_vec(&t0);
 496         check_vec(&t1);
 497
 498         return vm_vec_crossprod(dest,&t0,&t1);
 499 }
 500
 501
 502 //computes the delta angle between two vectors.
 503 //vectors need not be normalized. if they are, call vm_vec_delta_ang_norm()
 504 //the forward vector (third parameter) can be NULL, in which case the absolute
 505 //value of the angle in returned.  Otherwise the angle around that vector is
 506 //returned.
 507 fixang vm_vec_delta_ang(vms_vector *v0,vms_vector *v1,vms_vector *fvec)
 508 {
 509         vms_vector t0,t1;
 510
 511         vm_vec_copy_normalize(&t0,v0);
 512         vm_vec_copy_normalize(&t1,v1);
 513
 514         return vm_vec_delta_ang_norm(&t0,&t1,fvec);
 515 }
 516
 517 //computes the delta angle between two normalized vectors.
 518 fixang vm_vec_delta_ang_norm(vms_vector *v0,vms_vector *v1,vms_vector *fvec)
 519 {
 520         fixang a;
 521
 522         a = fix_acos(vm_vec_dot(v0,v1));
 523
 524         if (fvec) {
 525                 vms_vector t;
 526
 527                 vm_vec_cross(&t,v0,v1);
 528
 529                 if (vm_vec_dot(&t,fvec) < 0)
 530                         a = -a;
 531         }
 532
 533         return a;
 534 }
 535
 536 vms_matrix *sincos_2_matrix(vms_matrix *m,fix sinp,fix cosp,fix sinb,fix cosb,fix sinh,fix cosh)
 537 {
 538         fix sbsh,cbch,cbsh,sbch;
 539
 540         sbsh = fixmul(sinb,sinh);
 541         cbch = fixmul(cosb,cosh);
 542         cbsh = fixmul(cosb,sinh);
 543         sbch = fixmul(sinb,cosh);
 544
 545         m->rvec.x = cbch + fixmul(sinp,sbsh);           //m1
 546         m->uvec.z = sbsh + fixmul(sinp,cbch);           //m8
 547
 548         m->uvec.x = fixmul(sinp,cbsh) - sbch;           //m2
 549         m->rvec.z = fixmul(sinp,sbch) - cbsh;           //m7
 550
 551         m->fvec.x = fixmul(sinh,cosp);                          //m3
 552         m->rvec.y = fixmul(sinb,cosp);                          //m4
 553         m->uvec.y = fixmul(cosb,cosp);                          //m5
 554         m->fvec.z = fixmul(cosh,cosp);                          //m9
 555
 556         m->fvec.y = -sinp;                                                              //m6
 557
 558         return m;
 559
 560 }
 561
 562 //computes a matrix from a set of three angles.  returns ptr to matrix
 563 vms_matrix *vm_angles_2_matrix(vms_matrix *m,vms_angvec *a)
 564 {
 565         fix sinp,cosp,sinb,cosb,sinh,cosh;
 566
 567         fix_sincos(a->p,&sinp,&cosp);
 568         fix_sincos(a->b,&sinb,&cosb);
 569         fix_sincos(a->h,&sinh,&cosh);
 570
 571         return sincos_2_matrix(m,sinp,cosp,sinb,cosb,sinh,cosh);
 572
 573 }
 574
 575 //computes a matrix from a forward vector and an angle
 576 vms_matrix *vm_vec_ang_2_matrix(vms_matrix *m,vms_vector *v,fixang a)
 577 {
 578         fix sinb,cosb,sinp,cosp,sinh,cosh;
 579
 580         fix_sincos(a,&sinb,&cosb);
 581
 582         sinp = -v->y;
 583         cosp = fix_sqrt(f1_0 - fixmul(sinp,sinp));
 584
 585         sinh = fixdiv(v->x,cosp);
 586         cosh = fixdiv(v->z,cosp);
 587
 588         return sincos_2_matrix(m,sinp,cosp,sinb,cosb,sinh,cosh);
 589 }
 590
 591
 592 //computes a matrix from one or more vectors. The forward vector is required,
 593 //with the other two being optional.  If both up & right vectors are passed,
 594 //the up vector is used.  If only the forward vector is passed, a bank of
 595 //zero is assumed
 596 //returns ptr to matrix
 597 vms_matrix *vm_vector_2_matrix(vms_matrix *m,vms_vector *fvec,vms_vector *uvec,vms_vector *rvec)
 598 {
 599         vms_vector *xvec=&m->rvec,*yvec=&m->uvec,*zvec=&m->fvec;
 600
 601         Assert(fvec != NULL);
 602
 603         if (vm_vec_copy_normalize(zvec,fvec) == 0) {
 604                 Int3();         //forward vec should not be zero-length
 605                 return m;
 606         }
 607
 608         if (uvec == NULL) {
 609
 610                 if (rvec == NULL) {             //just forward vec
 611
 612 bad_vector2:
 613         ;
 614
 615                         if (zvec->x==0 && zvec->z==0) {         //forward vec is straight up or down
 616
 617                                 m->rvec.x = f1_0;
 618                                 m->uvec.z = (zvec->y<0)?f1_0:-f1_0;
 619
 620                                 m->rvec.y = m->rvec.z = m->uvec.x = m->uvec.y = 0;
 621                         }
 622                         else {          //not straight up or down
 623
 624                                 xvec->x = zvec->z;
 625                                 xvec->y = 0;
 626                                 xvec->z = -zvec->x;
 627
 628                                 vm_vec_normalize(xvec);
 629
 630                                 vm_vec_crossprod(yvec,zvec,xvec);
 631
 632                         }
 633
 634                 }
 635                 else {                                          //use right vec
 636
 637                         if (vm_vec_copy_normalize(xvec,rvec) == 0)
 638                                 goto bad_vector2;
 639
 640                         vm_vec_crossprod(yvec,zvec,xvec);
 641
 642                         //normalize new perpendicular vector
 643                         if (vm_vec_normalize(yvec) == 0)
 644                                 goto bad_vector2;
 645
 646                         //now recompute right vector, in case it wasn't entirely perpendiclar
 647                         vm_vec_crossprod(xvec,yvec,zvec);
 648
 649                 }
 650         }
 651         else {          //use up vec
 652
 653                 if (vm_vec_copy_normalize(yvec,uvec) == 0)
 654                         goto bad_vector2;
 655
 656                 vm_vec_crossprod(xvec,yvec,zvec);
 657
 658                 //normalize new perpendicular vector
 659                 if (vm_vec_normalize(xvec) == 0)
 660                         goto bad_vector2;
 661
 662                 //now recompute up vector, in case it wasn't entirely perpendiclar
 663                 vm_vec_crossprod(yvec,zvec,xvec);
 664
 665         }
 666
 667         return m;
 668 }
 669
 670
 671 //quicker version of vm_vector_2_matrix() that takes normalized vectors
 672 vms_matrix *vm_vector_2_matrix_norm(vms_matrix *m,vms_vector *fvec,vms_vector *uvec,vms_vector *rvec)
 673 {
 674         vms_vector *xvec=&m->rvec,*yvec=&m->uvec,*zvec=&m->fvec;
 675
 676         Assert(fvec != NULL);
 677
 678         if (uvec == NULL) {
 679
 680                 if (rvec == NULL) {             //just forward vec
 681
 682 bad_vector2:
 683         ;
 684
 685                         if (zvec->x==0 && zvec->z==0) {         //forward vec is straight up or down
 686
 687                                 m->rvec.x = f1_0;
 688                                 m->uvec.z = (zvec->y<0)?f1_0:-f1_0;
 689
 690                                 m->rvec.y = m->rvec.z = m->uvec.x = m->uvec.y = 0;
 691                         }
 692                         else {          //not straight up or down
 693
 694                                 xvec->x = zvec->z;
 695                                 xvec->y = 0;
 696                                 xvec->z = -zvec->x;
 697
 698                                 vm_vec_normalize(xvec);
 699
 700                                 vm_vec_crossprod(yvec,zvec,xvec);
 701
 702                         }
 703
 704                 }
 705                 else {                                          //use right vec
 706
 707                         vm_vec_crossprod(yvec,zvec,xvec);
 708
 709                         //normalize new perpendicular vector
 710                         if (vm_vec_normalize(yvec) == 0)
 711                                 goto bad_vector2;
 712
 713                         //now recompute right vector, in case it wasn't entirely perpendiclar
 714                         vm_vec_crossprod(xvec,yvec,zvec);
 715
 716                 }
 717         }
 718         else {          //use up vec
 719
 720                 vm_vec_crossprod(xvec,yvec,zvec);
 721
 722                 //normalize new perpendicular vector
 723                 if (vm_vec_normalize(xvec) == 0)
 724                         goto bad_vector2;
 725
 726                 //now recompute up vector, in case it wasn't entirely perpendiclar
 727                 vm_vec_crossprod(yvec,zvec,xvec);
 728
 729         }
 730
 731         return m;
 732 }
 733
 734
 735 //rotates a vector through a matrix. returns ptr to dest vector
 736 //dest CANNOT equal source
 737 vms_vector *vm_vec_rotate(vms_vector *dest,const vms_vector *src,const vms_matrix *m)
 738 {
 739         Assert(dest != src);
 740
 741         dest->x = vm_vec_dot(src,&m->rvec);
 742         dest->y = vm_vec_dot(src,&m->uvec);
 743         dest->z = vm_vec_dot(src,&m->fvec);
 744
 745         return dest;
 746 }
 747
 748
 749 //transpose a matrix in place. returns ptr to matrix
 750 vms_matrix *vm_transpose_matrix(vms_matrix *m)
 751 {
 752         fix t;
 753
 754         t = m->uvec.x;  m->uvec.x = m->rvec.y;  m->rvec.y = t;
 755         t = m->fvec.x;  m->fvec.x = m->rvec.z;  m->rvec.z = t;
 756         t = m->fvec.y;  m->fvec.y = m->uvec.z;  m->uvec.z = t;
 757
 758         return m;
 759 }
 760
 761 //copy and transpose a matrix. returns ptr to matrix
 762 //dest CANNOT equal source. use vm_transpose_matrix() if this is the case
 763 vms_matrix *vm_copy_transpose_matrix(vms_matrix *dest,vms_matrix *src)
 764 {
 765         Assert(dest != src);
 766
 767         dest->rvec.x = src->rvec.x;
 768         dest->rvec.y = src->uvec.x;
 769         dest->rvec.z = src->fvec.x;
 770
 771         dest->uvec.x = src->rvec.y;
 772         dest->uvec.y = src->uvec.y;
 773         dest->uvec.z = src->fvec.y;
 774
 775         dest->fvec.x = src->rvec.z;
 776         dest->fvec.y = src->uvec.z;
 777         dest->fvec.z = src->fvec.z;
 778
 779         return dest;
 780 }
 781
 782 //mulitply 2 matrices, fill in dest.  returns ptr to dest
 783 //dest CANNOT equal either source
 784 vms_matrix *vm_matrix_x_matrix(vms_matrix *dest,vms_matrix *src0,vms_matrix *src1)
 785 {
 786         Assert(dest!=src0 && dest!=src1);
 787
 788         dest->rvec.x = vm_vec_dot3(src0->rvec.x,src0->uvec.x,src0->fvec.x, &src1->rvec);
 789         dest->uvec.x = vm_vec_dot3(src0->rvec.x,src0->uvec.x,src0->fvec.x, &src1->uvec);
 790         dest->fvec.x = vm_vec_dot3(src0->rvec.x,src0->uvec.x,src0->fvec.x, &src1->fvec);
 791
 792         dest->rvec.y = vm_vec_dot3(src0->rvec.y,src0->uvec.y,src0->fvec.y, &src1->rvec);
 793         dest->uvec.y = vm_vec_dot3(src0->rvec.y,src0->uvec.y,src0->fvec.y, &src1->uvec);
 794         dest->fvec.y = vm_vec_dot3(src0->rvec.y,src0->uvec.y,src0->fvec.y, &src1->fvec);
 795
 796         dest->rvec.z = vm_vec_dot3(src0->rvec.z,src0->uvec.z,src0->fvec.z, &src1->rvec);
 797         dest->uvec.z = vm_vec_dot3(src0->rvec.z,src0->uvec.z,src0->fvec.z, &src1->uvec);
 798         dest->fvec.z = vm_vec_dot3(src0->rvec.z,src0->uvec.z,src0->fvec.z, &src1->fvec);
 799
 800         return dest;
 801 }
 802 #endif
 803
 804
 805 //extract angles from a matrix
 806 vms_angvec *vm_extract_angles_matrix(vms_angvec *a,vms_matrix *m)
 807 {
 808         fix sinh,cosh,cosp;
 809
 810         if (m->fvec.x==0 && m->fvec.z==0)               //zero head
 811                 a->h = 0;
 812         else
 813                 a->h = fix_atan2(m->fvec.z,m->fvec.x);
 814
 815         fix_sincos(a->h,&sinh,&cosh);
 816
 817         if (abs(sinh) > abs(cosh))                              //sine is larger, so use it
 818                 cosp = fixdiv(m->fvec.x,sinh);
 819         else                                                                                    //cosine is larger, so use it
 820                 cosp = fixdiv(m->fvec.z,cosh);
 821
 822         if (cosp==0 && m->fvec.y==0)
 823                 a->p = 0;
 824         else
 825                 a->p = fix_atan2(cosp,-m->fvec.y);
 826
 827
 828         if (cosp == 0)  //the cosine of pitch is zero.  we're pitched straight up. say no bank
 829
 830                 a->b = 0;
 831
 832         else {
 833                 fix sinb,cosb;
 834
 835                 sinb = fixdiv(m->rvec.y,cosp);
 836                 cosb = fixdiv(m->uvec.y,cosp);
 837
 838                 if (sinb==0 && cosb==0)
 839                         a->b = 0;
 840                 else
 841                         a->b = fix_atan2(cosb,sinb);
 842
 843         }
 844
 845         return a;
 846 }
 847
 848
 849 //extract heading and pitch from a vector, assuming bank==0
 850 vms_angvec *vm_extract_angles_vector_normalized(vms_angvec *a,vms_vector *v)
 851 {
 852         a->b = 0;               //always zero bank
 853
 854         a->p = fix_asin(-v->y);
 855
 856         if (v->x==0 && v->z==0)
 857                 a->h = 0;
 858         else
 859                 a->h = fix_atan2(v->z,v->x);
 860
 861         return a;
 862 }
 863
 864 //extract heading and pitch from a vector, assuming bank==0
 865 vms_angvec *vm_extract_angles_vector(vms_angvec *a,vms_vector *v)
 866 {
 867         vms_vector t;
 868
 869         if (vm_vec_copy_normalize(&t,v) != 0)
 870                 vm_extract_angles_vector_normalized(a,&t);
 871
 872         return a;
 873
 874 }
 875
 876 //compute the distance from a point to a plane.  takes the normalized normal
 877 //of the plane (ebx), a point on the plane (edi), and the point to check (esi).
 878 //returns distance in eax
 879 //distance is signed, so negative dist is on the back of the plane
 880 fix vm_dist_to_plane(vms_vector *checkp,vms_vector *norm,vms_vector *planep)
 881 {
 882         vms_vector t;
 883
 884         vm_vec_sub(&t,checkp,planep);
 885
 886         return vm_vec_dot(&t,norm);
 887
 888 }
 889
 890 vms_vector *vm_vec_make(vms_vector *v,fix x,fix y,fix z)
 891 {
 892         v->x=x; v->y=y; v->z=z;
 893         return v;
 894 }