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