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