maths/vecmat.c

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