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