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