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