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