Imported from d1x
[btb/d2x.git] / maths / fixc.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/fixc.c,v $
15  * $Revision: 1.2 $
16  * $Author: bradleyb $
17  * $Date: 2001-01-31 15:18:04 $
18  * 
19  * C version of fixed point library
20  * 
21  * $Log: not supported by cvs2svn $
22  * Revision 1.1.1.1  2001/01/19 03:29:58  bradleyb
23  * Import of d2x-0.0.8
24  *
25  * Revision 1.3  1999/10/18 00:31:01  donut
26  * more alpha fixes from Falk Hueffner
27  *
28  * Revision 1.2  1999/08/05 22:53:41  sekmu
29  *
30  * D3D patch(es) from ADB
31  *
32  * Revision 1.1.1.1  1999/06/14 22:13:35  donut
33  * Import of d1x 1.37 source.
34  *
35  * Revision 1.7  1995/09/22  14:08:16  allender
36  * fixed fix_atan2 to work correctly with doubles
37  *
38  * Revision 1.6  1995/08/31  15:43:49  allender
39  * *** empty log message ***
40  *
41  * Revision 1.5  1995/07/05  16:15:15  allender
42  * make fixmuldiv use doubles for PPC implementation
43  *
44  * Revision 1.4  1995/05/15  13:57:36  allender
45  * make fixmuldiv compile when compiling under 68k
46  *
47  * Revision 1.3  1995/05/11  13:02:59  allender
48  * some routines are now in assembly
49  *
50  * Revision 1.2  1995/05/04  20:04:45  allender
51  * use MPW fixdiv if compiling with MPW (why did I do this?)
52  *
53  * Revision 1.1  1995/04/17  11:37:54  allender
54  * Initial revision
55  *
56  *
57  * --- PC RCS Info ---
58  * Revision 1.1  1995/03/08  18:55:09  matt
59  * Initial revision
60  * 
61  * 
62  */
63
64 #ifdef HAVE_CONFIG_H
65 #include <conf.h>
66 #endif
67
68 #ifdef RCS
69 static char rcsid[] = "$Id: fixc.c,v 1.2 2001-01-31 15:18:04 bradleyb Exp $";
70 #endif
71
72 #include <stdlib.h>
73 #include <math.h>
74
75 #include "error.h"
76 #include "maths.h"
77
78 #ifndef NO_ASM
79 #ifdef NO_FIX_INLINE
80 #ifdef _MSC_VER
81 #pragma message ("warning: FIX NOT INLINED")
82 #else
83 #warning "FIX NOT INLINED"
84 #endif
85 #endif
86 #endif
87
88 extern ubyte guess_table[];
89 extern short sincos_table[];
90 extern ushort asin_table[];
91 extern ushort acos_table[];
92 extern fix isqrt_guess_table[];
93
94 //negate a quad
95 void fixquadnegate(quad *q)
96 {
97         q->low  = 0 - q->low;
98         q->high = 0 - q->high - (q->low != 0);
99 }
100
101 //multiply two ints & add 64-bit result to 64-bit sum
102 void fixmulaccum(quad *q,fix a,fix b)
103 {
104         u_int32_t aa,bb;
105         u_int32_t ah,al,bh,bl;
106         u_int32_t t,c=0,old;
107         int neg;
108
109         neg = ((a^b) < 0);
110
111         aa = labs(a); bb = labs(b);
112
113         ah = aa>>16;  al = aa&0xffff;
114         bh = bb>>16;  bl = bb&0xffff;
115
116         t = ah*bl + bh*al;
117
118         if (neg)
119                 fixquadnegate(q);
120
121         old = q->low;
122         q->low += al*bl;
123         if (q->low < old) q->high++;
124         
125         old = q->low;
126         q->low += (t<<16);
127         if (q->low < old) q->high++;
128         
129         q->high += ah*bh + (t>>16) + c;
130         
131         if (neg)
132                 fixquadnegate(q);
133
134 }
135
136 //extract a fix from a quad product
137 fix fixquadadjust(quad *q)
138 {
139         return (q->high<<16) + (q->low>>16);
140 }
141
142
143 #define EPSILON (F1_0/100)
144
145 #ifdef _MSC_VER
146 #define QLONG __int64
147 #else
148 #define QLONG long long
149 #endif
150
151 #ifdef NO_FIX_INLINE
152 fix fixmul(fix a, fix b) {
153 /*        return (fix)(((double)a*(double)b)/65536.0);*/
154 /*        register fix ret;
155         asm("imul %%edx; shrd $16,%%edx,%%eax" : "=a" (ret) : "a" (a), "d" (b) : "%edx");
156         return ret;                                 */
157         return (fix)((((QLONG)a)*b) >> 16);
158 }
159
160 fix fixdiv(fix a, fix b)
161 {
162 /*        return (fix)(((double)a * 65536.0) / (double)b);*/
163         return (fix)((((QLONG)a) << 16)/b);
164 /*        register fix ret;
165         asm("mov %%eax,%%edx; sar $16,%%edx; shl $16,%%eax; idiv %%ebx" : "=a" (ret) : "a" (a), "b" (b) : "%edx");
166     return ret; */
167 }
168
169 fix fixmuldiv(fix a, fix b, fix c)
170 {
171 /*        register fix ret;
172         asm("imul %%edx; idiv %%ebx" : "=a" (ret) : "a" (a), "d" (b), "b" (c) : "%edx");
173     return ret;*/
174
175 /*        double d;
176
177         d = (double)a * (double) b;
178         return (fix)(d / (double) c);
179 */
180         return (fix)((((QLONG)a)*b)/c);
181 }
182 #endif
183
184 //given cos & sin of an angle, return that angle.
185 //parms need not be normalized, that is, the ratio of the parms cos/sin must
186 //equal the ratio of the actual cos & sin for the result angle, but the parms 
187 //need not be the actual cos & sin.  
188 //NOTE: this is different from the standard C atan2, since it is left-handed.
189
190 fixang fix_atan2(fix cos,fix sin)
191 {
192         double d, dsin, dcos;
193         fixang t;
194
195         //Assert(!(cos==0 && sin==0));
196
197         //find smaller of two
198
199         dsin = (double)sin;
200         dcos = (double)cos;
201         d = sqrt((dsin * dsin) + (dcos * dcos));
202
203         if (d==0.0)
204                 return 0;
205
206         if (labs(sin) < labs(cos)) {                            //sin is smaller, use arcsin
207                 t = fix_asin((fix)((dsin / d) * 65536.0));
208                 if (cos<0)
209                         t = 0x8000 - t;
210                 return t;
211         }
212         else {
213                 t = fix_acos((fix)((dcos / d) * 65536.0));
214                 if (sin<0)
215                         t = -t;
216                 return t;
217         }
218 }
219
220 #ifdef NO_FIX_INLINE
221 //divide a quad by a fix, returning a fix
222 int32_t fixdivquadlong(u_int32_t nl,u_int32_t nh,u_int32_t d)
223 {
224         int i;
225         u_int32_t tmp0;
226         ubyte tmp1;
227         u_int32_t r;
228         ubyte T,Q,M;
229
230         r = 0;
231
232         Q = ((nh&0x80000000)!=0);
233         M = ((d&0x80000000)!=0);
234         T = (M!=Q);
235
236         if (M == 0)
237         {
238                 for (i=0; i<32; i++ )   {
239         
240                         r <<= 1;
241                         r |= T;
242                         T = ((nl&0x80000000L)!=0);
243                         nl <<= 1;
244         
245                         switch( Q ) {
246                 
247                         case 0:
248                                 Q = (unsigned char)((0x80000000L & nh) != 0 );
249                                 nh = (nh << 1) | (u_int32_t)T;
250
251                                 tmp0 = nh;
252                                 nh -= d;
253                                 tmp1 = (nh>tmp0);
254                                 if (Q == 0)
255                                         Q = tmp1;
256                                 else
257                                         Q = (unsigned char)(tmp1 == 0);
258                                 break;
259                         case 1:
260                                 Q = (unsigned char)((0x80000000L & nh) != 0 );
261                                 nh = (nh << 1) | (u_int32_t)T;
262
263                                 tmp0 = nh;
264                                 nh += d;
265                                 tmp1 = (nh<tmp0);
266                                 if (Q == 0)
267                                         Q = tmp1;
268                                 else
269                                         Q = (unsigned char)(tmp1 == 0);
270                                 break;
271                         }
272                         T = (Q==M);
273                 }
274         }
275         else
276         {
277                 for (i=0; i<32; i++ )   {
278         
279                         r <<= 1;
280                         r |= T;
281                         T = ((nl&0x80000000L)!=0);
282                         nl <<= 1;
283         
284                         switch( Q ) {
285                 
286                         case 0:
287                                 Q = (unsigned char)((0x80000000L & nh) != 0 );
288                                 nh = (nh << 1) | (u_int32_t)T;
289
290                                 tmp0 = nh;
291                                 nh += d;
292                                 tmp1 = (nh<tmp0);
293                                 if (Q == 1)
294                                         Q = tmp1;
295                                 else
296                                         Q = (unsigned char)(tmp1 == 0);
297                                 break;
298                         case 1: 
299                                 Q = (unsigned char)((0x80000000L & nh) != 0 );
300                                 nh = (nh << 1) | (u_int32_t)T;
301
302                                 tmp0 = nh;
303                                 nh = nh - d;
304                                 tmp1 = (nh>tmp0);
305                                 if (Q == 1)
306                                         Q = tmp1;
307                                 else
308                                         Q = (unsigned char)(tmp1 == 0);
309                                 break;
310                         }
311                         T = (Q==M);
312                 }
313         }
314
315         r = (r << 1) | T;
316
317         return r;
318 }
319
320 unsigned int fixdivquadlongu(uint nl, uint nh, uint d)
321 {
322   return fixdivquadlong((u_int32_t) nl,(u_int32_t) nh,(u_int32_t) d);
323 }
324 #else
325 int32_t fixdivquadlong(u_int32_t nl,u_int32_t nh,u_int32_t d) {
326 int32_t a;
327 __asm__("idivl %3"
328          :"=a" (a)
329          :"a" (nl), "d" (nh), "r" (d)
330          :"ax", "dx"
331          );
332 return (a);
333 }
334 static inline u_int32_t fixdivquadlongu(u_int32_t nl,u_int32_t nh,u_int32_t d) {
335 u_int32_t a;
336 __asm__("divl %3"
337          :"=a" (a)
338          :"a" (nl), "d" (nh), "r" (d)
339          :"ax", "dx"
340          );
341 return (a);
342 }
343 #endif
344
345 u_int32_t quad_sqrt(u_int32_t low,int32_t high)
346 {
347         int i, cnt;
348         u_int32_t r,old_r,t;
349         quad tq;
350
351         if (high<0)
352                 return 0;
353
354         if (high==0 && (int32_t)low>=0)
355                 return long_sqrt((int32_t)low);
356
357         if (high & 0xff000000) {
358                 cnt=12+16; i = high >> 24;
359         } else if (high & 0xff0000) {
360                 cnt=8+16; i = high >> 16;
361         } else if (high & 0xff00) {
362                 cnt=4+16; i = high >> 8;
363         } else {
364                 cnt=0+16; i = high;
365         }
366         
367         r = guess_table[i]<<cnt;
368
369         //quad loop usually executed 4 times
370
371         r = (fixdivquadlongu(low,high,r)+r)/2;
372         r = (fixdivquadlongu(low,high,r)+r)/2;
373         r = (fixdivquadlongu(low,high,r)+r)/2;
374
375         do {
376
377                 old_r = r;
378                 t = fixdivquadlongu(low,high,r);
379
380                 if (t==r)       //got it!
381                         return r;
382  
383                 r = (t+r)/2;
384
385         } while (!(r==t || r==old_r));
386
387         t = fixdivquadlongu(low,high,r);
388         //edited 05/17/99 Matt Mueller - tq.high is undefined here.. so set them to = 0
389         tq.low=tq.high=0;
390         //end edit -MM
391         fixmulaccum(&tq,r,t);
392         if (tq.low!=low || tq.high!=high)
393                 r++;
394
395         return r;
396 }
397
398 //computes the square root of a long, returning a short
399 ushort long_sqrt(int32_t a)
400 {
401         int cnt,r,old_r,t;
402
403         if (a<=0)
404                 return 0;
405
406         if (a & 0xff000000)
407                 cnt=12;
408         else if (a & 0xff0000)
409                 cnt=8;
410         else if (a & 0xff00)
411                 cnt=4;
412         else
413                 cnt=0;
414         
415         r = guess_table[(a>>cnt)&0xff]<<cnt;
416
417         //the loop nearly always executes 3 times, so we'll unroll it 2 times and
418         //not do any checking until after the third time.  By my calcutations, the
419         //loop is executed 2 times in 99.97% of cases, 3 times in 93.65% of cases, 
420         //four times in 16.18% of cases, and five times in 0.44% of cases.  It never
421         //executes more than five times.  By timing, I determined that is is faster
422         //to always execute three times and not check for termination the first two
423         //times through.  This means that in 93.65% of cases, we save 6 cmp/jcc pairs,
424         //and in 6.35% of cases we do an extra divide.  In real life, these numbers
425         //might not be the same.
426
427         r = ((a/r)+r)/2;
428         r = ((a/r)+r)/2;
429
430         do {
431
432                 old_r = r;
433                 t = a/r;
434
435                 if (t==r)       //got it!
436                         return r;
437  
438                 r = (t+r)/2;
439
440         } while (!(r==t || r==old_r));
441
442         if (a % r)
443                 r++;
444
445         return r;
446 }
447
448 //computes the square root of a fix, returning a fix
449 fix fix_sqrt(fix a)
450 {
451         return ((fix) long_sqrt(a)) << 8;
452 }
453
454
455 //compute sine and cosine of an angle, filling in the variables
456 //either of the pointers can be NULL
457 //with interpolation
458 void fix_sincos(fix a,fix *s,fix *c)
459 {
460         int i,f;
461         fix ss,cc;
462
463         i = (a>>8)&0xff;
464         f = a&0xff;
465
466         ss = sincos_table[i];
467         if (s) *s = (ss + (((sincos_table[i+1] - ss) * f)>>8))<<2;
468
469         cc = sincos_table[i+64];
470         if (c) *c = (cc + (((sincos_table[i+64+1] - cc) * f)>>8))<<2;
471 }
472
473 //compute sine and cosine of an angle, filling in the variables
474 //either of the pointers can be NULL
475 //no interpolation
476 void fix_fastsincos(fix a,fix *s,fix *c)
477 {
478         int i;
479
480         i = (a>>8)&0xff;
481
482         if (s) *s = sincos_table[i] << 2;
483         if (c) *c = sincos_table[i+64] << 2;
484 }
485
486 //compute inverse sine
487 fixang fix_asin(fix v)
488 {
489         fix vv;
490         int i,f,aa;
491
492         vv = labs(v);
493
494         if (vv >= f1_0)         //check for out of range
495                 return 0x4000;
496
497         i = (vv>>8)&0xff;
498         f = vv&0xff;
499
500         aa = asin_table[i];
501         aa = aa + (((asin_table[i+1] - aa) * f)>>8);
502
503         if (v < 0)
504                 aa = -aa;
505
506         return aa;
507 }
508
509 //compute inverse cosine
510 fixang fix_acos(fix v)
511 {
512         fix vv;
513         int i,f,aa;
514
515         vv = labs(v);
516
517         if (vv >= f1_0)         //check for out of range
518                 return 0;
519
520         i = (vv>>8)&0xff;
521         f = vv&0xff;
522
523         aa = acos_table[i];
524         aa = aa + (((acos_table[i+1] - aa) * f)>>8);
525
526         if (v < 0)
527                 aa = 0x8000 - aa;
528
529         return aa;
530 }
531
532 #define TABLE_SIZE 1024
533
534 //for passed value a, returns 1/sqrt(a) 
535 fix fix_isqrt( fix a )
536 {
537         int i, b = a;
538         int cnt = 0;
539         int r;
540
541         if ( a == 0 ) return 0;
542
543         while( b >= TABLE_SIZE )        {
544                 b >>= 1;
545                 cnt++;
546         }
547
548         //printf( "Count = %d (%d>>%d)\n", cnt, b, (cnt+1)/2 );
549         r = isqrt_guess_table[b] >> ((cnt+1)/2);
550
551         //printf( "Initial r = %d\n", r );
552
553         for (i=0; i<3; i++ )    {
554                 int old_r = r;
555                 r = fixmul( ( (3*65536) - fixmul(fixmul(r,r),a) ), r) / 2;
556                 //printf( "r %d  = %d\n", i, r );
557                 if ( old_r >= r ) return (r+old_r)/2;
558         }
559
560         return r;       
561 }