/*
* This file is part of the DXX-Rebirth project .
* It is copyright by its individual contributors, as recorded in the
* project's Git history. See COPYING.txt at the top level for license
* terms and a link to the Git history.
*/
/*
*
* C version of fixed point library
*
*/
#include
#include
#include "dxxerror.h"
#include "maths.h"
//negate a quad
void fixquadnegate(quadint *q)
{
q->low = 0 - q->low;
q->high = 0 - q->high - (q->low != 0);
}
//multiply two ints & add 64-bit result to 64-bit sum
void fixmulaccum(quadint *q,fix a,fix b)
{
u_int32_t aa,bb;
u_int32_t ah,al,bh,bl;
u_int32_t t,c=0,old;
int neg;
neg = ((a^b) < 0);
aa = labs(a); bb = labs(b);
ah = aa>>16; al = aa&0xffff;
bh = bb>>16; bl = bb&0xffff;
t = ah*bl + bh*al;
if (neg)
fixquadnegate(q);
old = q->low;
q->low += al*bl;
if (q->low < old) q->high++;
old = q->low;
q->low += (t<<16);
if (q->low < old) q->high++;
q->high += ah*bh + (t>>16) + c;
if (neg)
fixquadnegate(q);
}
//extract a fix from a quad product
fix fixquadadjust(quadint *q)
{
return (q->high<<16) + (q->low>>16);
}
#define EPSILON (F1_0/100)
fix fixmul(fix a, fix b)
{
return (fix)((((fix64) a) * b) / 65536);
}
fix64 fixmul64(fix a, fix b)
{
return (fix64)((((fix64) a) * b) / 65536);
}
fix fixdiv(fix a, fix b)
{
return b ? (fix)((((fix64)a) *65536)/b) : 1;
}
fix fixmuldiv(fix a, fix b, fix c)
{
return c ? (fix)((((fix64)a)*b)/c) : 1;
}
//given cos & sin of an angle, return that angle.
//parms need not be normalized, that is, the ratio of the parms cos/sin must
//equal the ratio of the actual cos & sin for the result angle, but the parms
//need not be the actual cos & sin.
//NOTE: this is different from the standard C atan2, since it is left-handed.
fixang fix_atan2(fix cos,fix sin)
{
double d, dsin, dcos;
fixang t;
//Assert(!(cos==0 && sin==0));
//find smaller of two
dsin = (double)sin;
dcos = (double)cos;
d = sqrt((dsin * dsin) + (dcos * dcos));
if (d==0.0)
return 0;
if (labs(sin) < labs(cos)) { //sin is smaller, use arcsin
t = fix_asin((fix)((dsin / d) * 65536.0));
if (cos<0)
t = 0x8000 - t;
return t;
}
else {
t = fix_acos((fix)((dcos / d) * 65536.0));
if (sin<0)
t = -t;
return t;
}
}
int32_t fixdivquadlong(u_int32_t nl,u_int32_t nh,u_int32_t d)
{
int64_t n = (int64_t)nl | (((int64_t)nh) << 32 );
return (signed int) (n / ((int64_t)d));
}
static unsigned int fixdivquadlongu(uint nl, uint nh, uint d)
{
u_int64_t n = (u_int64_t)nl | (((u_int64_t)nh) << 32 );
return (unsigned int) (n / ((u_int64_t)d));
}
u_int32_t quad_sqrt(u_int32_t low,int32_t high)
{
int i, cnt;
u_int32_t r,old_r,t;
quadint tq;
if (high<0)
return 0;
if (high==0 && (int32_t)low>=0)
return long_sqrt((int32_t)low);
if (high & 0xff000000) {
cnt=12+16; i = high >> 24;
} else if (high & 0xff0000) {
cnt=8+16; i = high >> 16;
} else if (high & 0xff00) {
cnt=4+16; i = high >> 8;
} else {
cnt=0+16; i = high;
}
r = guess_table[i]<>cnt)&0xff]<>8)&0xff;
f = a&0xff;
if (s)
{
fix ss = sincos_table[i];
*s = (ss + (((sincos_table[i+1] - ss) * f)>>8))<<2;
}
if (c)
{
fix cc = sincos_table[i+64];
*c = (cc + (((sincos_table[i+64+1] - cc) * f)>>8))<<2;
}
}
//compute sine and cosine of an angle, filling in the variables
//either of the pointers can be NULL
//no interpolation
void fix_fastsincos(fix a,fix *s,fix *c)
{
int i;
i = (a>>8)&0xff;
if (s) *s = sincos_table[i] << 2;
if (c) *c = sincos_table[i+64] << 2;
}
//compute inverse sine
fixang fix_asin(fix v)
{
fix vv;
int i,f,aa;
vv = labs(v);
if (vv >= f1_0) //check for out of range
return 0x4000;
i = (vv>>8)&0xff;
f = vv&0xff;
aa = asin_table[i];
aa = aa + (((asin_table[i+1] - aa) * f)>>8);
if (v < 0)
aa = -aa;
return aa;
}
//compute inverse cosine
fixang fix_acos(fix v)
{
fix vv;
int i,f,aa;
vv = labs(v);
if (vv >= f1_0) //check for out of range
return 0;
i = (vv>>8)&0xff;
f = vv&0xff;
aa = acos_table[i];
aa = aa + (((acos_table[i+1] - aa) * f)>>8);
if (v < 0)
aa = 0x8000 - aa;
return aa;
}