345 lines
8.0 KiB
C
345 lines
8.0 KiB
C
//Fixed Point Math Routines
|
|
//Copyright 2/2/01 Dan East
|
|
#include "quakedef.h"
|
|
#include "FixedPointMath.h"
|
|
#include <math.h>
|
|
|
|
#if defined(DEBUG)&&defined(_X86_)
|
|
#include "windows.h"
|
|
#define FPM_VALIDATE
|
|
#endif
|
|
|
|
fixedpoint_t fpm_FromFloat(double f) {
|
|
|
|
# if defined(DEBUG)&&defined(FPM_VALIDATE)
|
|
fixedpoint_t fxp=(fixedpoint_t) (f*65536.0);
|
|
if (f) ASSERT(fxp);
|
|
# endif
|
|
|
|
return (fixedpoint_t) (f*65536.0);
|
|
}
|
|
|
|
|
|
float fpm_ToFloat(fixedpoint_t fxp) {
|
|
return (float)(fxp/65536.0);
|
|
}
|
|
|
|
fixedpoint_t fpm_FromLong(long l) {
|
|
# if defined(DEBUG)&&defined(FPM_VALIDATE)
|
|
if (l>0) ASSERT((l<<16)>0);
|
|
else if (l<0) ASSERT((l<<16)<0);
|
|
# endif
|
|
|
|
return l<<16;
|
|
}
|
|
|
|
long fpm_ToLong(fixedpoint_t fxp) {
|
|
if (fxp<0)
|
|
return -((long)((fxp^0xffffffff)>>16)+1);
|
|
else
|
|
return (fxp>>16)&0x0000ffff;
|
|
}
|
|
|
|
fixedpoint_t fpm_Add(fixedpoint_t fxp1, fixedpoint_t fxp2) {
|
|
# if defined(DEBUG)&&defined(FPM_VALIDATE)
|
|
if (fxp1>0&&fxp2>0) ASSERT((fxp1+fxp2)>0);
|
|
if (fxp1<0&&fxp2<0) ASSERT((fxp1+fxp2)<0);
|
|
# endif
|
|
|
|
return fxp1+fxp2;
|
|
}
|
|
|
|
fixedpoint_t fpm_Add3(fixedpoint_t fxp1, fixedpoint_t fxp2, fixedpoint_t fxp3) {
|
|
return fxp1+fxp2+fxp3;
|
|
}
|
|
/*
|
|
fixedpoint_t fpm_Inc(fixedpoint_t fxp) {
|
|
return fxp=fxp+FPM_FROMLONG(1);
|
|
}
|
|
*/
|
|
fixedpoint_t fpm_Sub(fixedpoint_t fxp1, fixedpoint_t fxp2) {
|
|
return fxp1-fxp2;
|
|
}
|
|
/*
|
|
fixedpoint_t fpm_Dec(fixedpoint_t &fxp) {
|
|
return fxp=fxp-FPM_FROMLONG(1);
|
|
}
|
|
*/
|
|
fixedpoint_t fpm_Mul(fixedpoint_t fxp1, fixedpoint_t fxp2) {
|
|
__int64 tmp=fxp1;
|
|
|
|
# if defined(DEBUG)&&defined(FPM_VALIDATE)
|
|
fixedpoint_t fxp;
|
|
tmp*=fxp2;
|
|
fxp=(fixedpoint_t)(tmp>>16);
|
|
if (fxp1&&fxp2) {
|
|
/*
|
|
//Dan: Temp hack
|
|
if (!fxp) {
|
|
if ((fxp1>0&&fxp2>0)||(fxp1<0&&fxp2<0)) fxp=1;
|
|
else fxp=-1;
|
|
return fxp;
|
|
}
|
|
*/
|
|
ASSERT(fxp);
|
|
if ((fxp1>0&&fxp2>0)||(fxp1<0&&fxp2<0)) ASSERT(fxp>0);
|
|
else ASSERT(fxp<0);
|
|
}
|
|
return fxp;
|
|
# endif
|
|
|
|
tmp*=fxp2;
|
|
|
|
return (fixedpoint_t)(tmp>>16);
|
|
}
|
|
|
|
fixedpoint_t fpm_Div(fixedpoint_t fxp1, fixedpoint_t fxp2) {
|
|
__int64 tmp=fxp1;
|
|
# if defined(DEBUG)&&defined(FPM_VALIDATE)
|
|
fixedpoint_t fxp;
|
|
|
|
ASSERT(fxp2);
|
|
tmp<<=16;
|
|
fxp=(fixedpoint_t)(tmp/fxp2);
|
|
|
|
if (fxp1) {
|
|
/*
|
|
//Dan: Temp hack
|
|
if (!fxp) {
|
|
if ((fxp1>0&&fxp2>0)||(fxp1<0&&fxp2<0)) fxp=1;
|
|
else fxp=-1;
|
|
return fxp;
|
|
}
|
|
*/
|
|
ASSERT(fxp);
|
|
if ((fxp1>0&&fxp2>0)||(fxp1<0&&fxp2<0)) ASSERT(fxp>0);
|
|
else ASSERT(fxp<0);
|
|
}
|
|
|
|
return fxp;
|
|
#endif
|
|
tmp<<=16;
|
|
return (fixedpoint_t)(tmp/fxp2);
|
|
}
|
|
|
|
fixedpoint_t fpm_DivInt(fixedpoint_t fxp1, long l) {
|
|
return fxp1/l;
|
|
}
|
|
|
|
fixedpoint_t fpm_Abs(fixedpoint_t fxp) {
|
|
return abs(fxp);
|
|
}
|
|
|
|
//TODO: could be more efficient
|
|
fixedpoint_t fpm_Ceil(fixedpoint_t fxp) {
|
|
if (fxp&0x0000ffff) {
|
|
if (fxp<=0) return -(fixedpoint_t)((-fxp)&0xffff0000);
|
|
return (fxp&0xffff0000)+FPM_FROMLONGC(1);
|
|
}
|
|
return fxp;
|
|
}
|
|
//TODO: could be more efficient
|
|
fixedpoint_t fpm_Floor(fixedpoint_t fxp) {
|
|
if (fxp&0x0000ffff) {
|
|
if (fxp<0) return -(long)(((-fxp)&0xffff0000)+FPM_FROMLONGC(1));
|
|
return fxp&0xffff0000;
|
|
}
|
|
return fxp;
|
|
}
|
|
|
|
//TODO: Implement sqrt mathematically instead of converting to float and back
|
|
fixedpoint_t fpm_Sqrt(fixedpoint_t fxp) {
|
|
return fpm_FromFloat(sqrt(fpm_ToFloat(fxp)));
|
|
}
|
|
|
|
fixedpoint_t fpm_Sqr(fixedpoint_t fxp) {
|
|
return fpm_Mul(fxp,fxp);
|
|
}
|
|
|
|
fixedpoint_t fpm_Inv(fixedpoint_t fxp) {
|
|
# if defined(DEBUG)&&defined(FPM_VALIDATE)
|
|
ASSERT(fxp);
|
|
# endif
|
|
|
|
return fpm_Div(FPM_FROMLONGC(1),fxp);
|
|
}
|
|
|
|
//TODO: Calc trig functions (or lookup) instead of converting to float and back
|
|
//These take radians
|
|
fixedpoint_t fpm_Sin(fixedpoint_t fxp) {
|
|
return fpm_FromFloat(sin(fpm_ToFloat(fxp)));
|
|
}
|
|
fixedpoint_t fpm_Cos(fixedpoint_t fxp) {
|
|
return fpm_FromFloat(cos(fpm_ToFloat(fxp)));
|
|
}
|
|
fixedpoint_t fpm_Tan(fixedpoint_t fxp) {
|
|
return fpm_FromFloat(tan(fpm_ToFloat(fxp)));
|
|
}
|
|
fixedpoint_t fpm_ATan(fixedpoint_t fxp) {
|
|
return fpm_FromFloat(atan(fpm_ToFloat(fxp)));
|
|
}
|
|
|
|
//These take degrees
|
|
fixedpoint_t fpm_SinDeg(fixedpoint_t fxp) {
|
|
return fpm_Sin(fpm_DivInt(fpm_Mul(fxp, FPM_PI), 180));
|
|
}
|
|
fixedpoint_t fpm_CosDeg(fixedpoint_t fxp) {
|
|
return fpm_Cos(fpm_DivInt(fpm_Mul(fxp, FPM_PI), 180));
|
|
}
|
|
fixedpoint_t fpm_TanDeg(fixedpoint_t fxp) {
|
|
return fpm_Tan(fpm_DivInt(fpm_Mul(fxp, FPM_PI), 180));
|
|
}
|
|
fixedpoint_t fpm_ATanDeg(fixedpoint_t fxp) {
|
|
return fpm_ATan(fpm_DivInt(fpm_Mul(fxp, FPM_PI), 180));
|
|
}
|
|
|
|
/*********************************************/
|
|
/* 8.24 routines: */
|
|
/*********************************************/
|
|
|
|
fixedpoint8_24_t fpm_FromFloat8_24(double f) {
|
|
# if defined(DEBUG)&&defined(FPM_VALIDATE)
|
|
if (f) ASSERT((fixedpoint_t) (f*16777216.0));
|
|
# endif
|
|
|
|
return (fixedpoint8_24_t) (f*16777216.0);
|
|
}
|
|
|
|
float fpm_ToFloat8_24(fixedpoint8_24_t fxp) {
|
|
return (float)(fxp/16777216.0);
|
|
}
|
|
|
|
fixedpoint8_24_t fpm_FromLong8_24(long l) {
|
|
# if defined(DEBUG)&&defined(FPM_VALIDATE)
|
|
if (l>0) ASSERT((l<<24)>0);
|
|
else if (l<0) ASSERT((l<<24)<0);
|
|
# endif
|
|
|
|
return l<<24;
|
|
}
|
|
|
|
long fpm_ToLong8_24(fixedpoint8_24_t fxp) {
|
|
if (fxp<0)
|
|
return -((long)((fxp^0xffffffff)>>24)+1);
|
|
else
|
|
return (fxp>>24)&0x000000ff;
|
|
}
|
|
|
|
fixedpoint8_24_t fpm_FromFixedPoint(fixedpoint_t fxp) {
|
|
return fxp<<8;
|
|
}
|
|
|
|
fixedpoint_t fpm_ToFixedPoint(fixedpoint8_24_t fxp) {
|
|
if (fxp<0)
|
|
return -((long)((fxp^0xffffffff)>>8)+1);
|
|
else
|
|
return (fxp>>8)&0x00ffffff;
|
|
}
|
|
|
|
fixedpoint8_24_t fpm_Add8_24(fixedpoint8_24_t fxp1, fixedpoint8_24_t fxp2) {
|
|
return fxp1+fxp2;
|
|
}
|
|
|
|
fixedpoint8_24_t fpm_Add38_24(fixedpoint8_24_t fxp1, fixedpoint8_24_t fxp2, fixedpoint8_24_t fxp3) {
|
|
return fxp1+fxp2+fxp3;
|
|
}
|
|
/*
|
|
fixedpoint_t fpm_Inc(fixedpoint_t fxp) {
|
|
return fxp=fxp+FPM_FROMLONG(1);
|
|
}
|
|
*/
|
|
fixedpoint8_24_t fpm_Sub8_24(fixedpoint8_24_t fxp1, fixedpoint8_24_t fxp2) {
|
|
return fxp1-fxp2;
|
|
}
|
|
/*
|
|
fixedpoint_t fpm_Dec(fixedpoint_t &fxp) {
|
|
return fxp=fxp-FPM_FROMLONG(1);
|
|
}
|
|
*/
|
|
fixedpoint8_24_t fpm_Mul8_24(fixedpoint8_24_t fxp1, fixedpoint8_24_t fxp2) {
|
|
__int64 tmp=fxp1;
|
|
|
|
# if defined(DEBUG)&&defined(FPM_VALIDATE)
|
|
fixedpoint8_24_t fxp;
|
|
tmp*=fxp2;
|
|
fxp=(fixedpoint8_24_t)(tmp>>24);
|
|
if (fxp1&&fxp2) {
|
|
ASSERT(fxp);
|
|
if ((fxp1>0&&fxp2>0)||(fxp1<0&&fxp2<0)) ASSERT(fxp>0);
|
|
else ASSERT(fxp<0);
|
|
}
|
|
return fxp;
|
|
# endif
|
|
|
|
tmp*=fxp2;
|
|
return (fixedpoint_t)(tmp>>24);
|
|
}
|
|
|
|
fixedpoint_t fpm_MulMixed8_24(fixedpoint8_24_t fxp1, fixedpoint_t fxp2) {
|
|
__int64 tmp=fxp1;
|
|
tmp*=fxp2;
|
|
|
|
return (fixedpoint_t)(tmp>>24);
|
|
}
|
|
|
|
fixedpoint8_24_t fpm_Div8_24(fixedpoint8_24_t fxp1, fixedpoint8_24_t fxp2) {
|
|
__int64 tmp=fxp1;
|
|
# if defined(DEBUG)&&defined(FPM_VALIDATE)
|
|
//The purpose of this is to cause an exception in Windows CE
|
|
//that will be handled right here by the debugger. Otherwise a
|
|
//divide by zero will be thrown which cannot be used to find
|
|
//where the exception actually occured with the debugger.
|
|
ASSERT(fxp2);
|
|
if (!fxp2)
|
|
(*(int *)fxp2)=0;
|
|
# endif
|
|
tmp<<=24;
|
|
return (fixedpoint8_24_t)(tmp/fxp2);
|
|
}
|
|
|
|
fixedpoint8_24_t fpm_DivInt8_24(fixedpoint8_24_t fxp1, long l) {
|
|
return fxp1/l;
|
|
}
|
|
|
|
fixedpoint8_24_t fpm_DivInt64_8_24(fixedpoint8_24_t fxp1, __int64 l) {
|
|
return (fixedpoint8_24_t) (fxp1/l);
|
|
}
|
|
|
|
fixedpoint8_24_t fpm_Abs8_24(fixedpoint8_24_t fxp) {
|
|
return abs(fxp);
|
|
}
|
|
|
|
//TODO: could be more efficient
|
|
fixedpoint8_24_t fpm_Ceil8_24(fixedpoint8_24_t fxp) {
|
|
if (fxp&0x00ffffff) {
|
|
if (fxp<=0) return (fxp&0xff000000);
|
|
return (fxp&0xff000000)+FPM_FROMLONGC8_24(1);
|
|
}
|
|
return fxp;
|
|
}
|
|
//TODO: could be more efficient
|
|
fixedpoint8_24_t fpm_Floor8_24(fixedpoint8_24_t fxp) {
|
|
if (fxp&0x00ffffff) {
|
|
if (fxp<0) return -(long)((fxp&0xff000000)+FPM_FROMLONG8_24(1));
|
|
return fxp&0xff000000;
|
|
}
|
|
return fxp;
|
|
}
|
|
|
|
//TODO: Implement sqrt mathematically instead of converting to float and back
|
|
fixedpoint8_24_t fpm_Sqrt8_24(fixedpoint8_24_t fxp) {
|
|
return fpm_FromFloat8_24(sqrt(fpm_ToFloat8_24(fxp)));
|
|
}
|
|
|
|
fixedpoint8_24_t fpm_Sqr8_24(fixedpoint8_24_t fxp) {
|
|
return fpm_Mul8_24(fxp,fxp);
|
|
}
|
|
|
|
fixedpoint8_24_t fpm_Inv8_24(fixedpoint8_24_t fxp) {
|
|
# if defined(DEBUG)&&defined(FPM_VALIDATE)
|
|
ASSERT(fxp);
|
|
# endif
|
|
|
|
return fpm_Div8_24(FPM_FROMLONGC8_24(1), fxp);
|
|
}
|