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