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