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