sparc/sunos support: rename quad to quadint. courtesy of Martin Schaffner <maschaffne...
[btb/d2x.git] / 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