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