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