maths/vecmat.c

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