Makefile and conf.h fixes
[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.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