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