fadd, fmul, fmul, fdiv, fsin, fcos, fsqr, fsrqd, ftan ,f2i, i2f, f2s, s2f ...
Joshy
file: fixedpoint.bi
Code: Select all
'#define _DEBUG
#ifdef _DEBUG
#define dprint(msg) : scope : dim as integer h=FreeFile() : open err for output as #h:print #h,msg:close #h : end scope
#else
#define dprint(msg) :
#endif
' 32 bit 16,16 fixed point math
' 0111111111111111,000000000000000 = 32767
' ...
' 100000000000000,000000000000000 = 8192
' 10000000000000,000000000000000 = 4096
' 1000000000000,000000000000000 = 2048
' 100000000000,000000000000000 = 1024
' 10000000000,000000000000000 = 512
' 100000000,000000000000000 = 256
' 10000000,000000000000000 = 128
' 1000000,000000000000000 = 64
' 100000,000000000000000 = 32
' 10000,000000000000000 = 16
' 1000,000000000000000 = 8
' 100,000000000000000 = 4
' 10,000000000000000 = 2
' 1,000000000000000 = 1
' 0,111111111111111 = 0.9999847
' 0,100000000000000 = 0.5
' 0,010000000000000 = 0.25
' 0,001000000000000 = 0.125
' 0,000100000000000 = 0.0625
' 0,000010000000000 = 0.03125
' 0,000001000000000 = 0.015625
' 0,000000100000000 = 0.0078125
' 0,000000010000000 = 0.00390625
' 0,000000001000000 = 0.001953125
' 0,000000000100000 = 0.0009765625
' 0,000000000010000 = 0.00048828125
' 0,000000000001000 = 0.000244140625
' 0,000000000000100 = 0.0001220703125
' 0,000000000000010 = 0.00006103515625
' 0,000000000000001 = 0.000030517578125
' 0111111111111111,000000000000000 = 32767.0
' ...
' 0000000000000100,000000000000000 = 4.0
' 0000000000000010,000000000000000 = 2.0
' 0000000000000001,000000000000000 = 1.0
' 0000000000000000,000000000000000 = 0.0
' 1111111111111111,000000000000000 =-1.0
' 1111111111111110,000000000000000 =-2.0
' 1111111111111100,000000000000000 =-4.0
' ...
' 1000000000000000,0 =-32768.0
type FP32 as long
#define TAB_MAX (1024) ' !!! must be power of two !!!
#define TAB_MASK (TAB_MAX-1)
dim shared as FP32 FP32SINTAB(TAB_MASK)
dim shared as FP32 FP32COSTAB(TAB_MASK)
#define FONE 65536 ' 1.0 in fixed point
#define FPI 205887 ' PI in fixed point
#define FPI2 411775 ' PI*2.0 in fixed point
' r=S2F(x) single to fixed
#define S2F(x_) ((x_)*(1 shl 16))
' r=f/(1 shl 16) fixed to single
#define F2S(x_) ((x_)/65536.0f)
' r=integer to fixed
#define I2F(x_) ((x_) shl 16)
' r=fixed to integer
#define F2I(x_) ((x_) shr 16)
' r=rounded(fixed) to integer
#define RF2I(x_)(((x_)+&H8000) shr 16)
' r=x+y
#define FAdd(x_,y_) ((x_)+(y_))
' r=x-y
#define FSub(x_,y_) ((x_)-(y_))
' r=x*y
#define FMul(x_, y_) ( (clngint(x_)*(y_)) shr 16)
' r=x/y
#define FDiv(x_, y_) ( ((clngint(x_) shl 16)) / (y_))
' r=1.0/x
#define FInv(x_) (1 shl 16)/(x_)
' r=x^x
#define FSqrd(x_) FMul(x_,x_)
' create sin cos fixed point array
sub _module_init_ constructor
dim as single w,s=atn(4)/TAB_MAX
for i as integer =0 to TAB_MASK
dim as single si=sin(w),co=cos(w)
FP32SINTAB(i)=S2F(si)
FP32COSTAB(i)=S2F(co)
w+=s
next
end sub
' fixed point squarroot
function FSqr(x as FP32) as FP32
dim as FP32 t=any, q=0, b=&H40000000, r=x
while( b > &H40 )
t = q + b
if ( r >= t ) then r-=t : q=t+b
r shl= 1
b shr= 1
wend
q shr= 8
return q
end function
' fixed point sin
function FSin(ang as integer) as FP32
ang and=TAB_MASK
return FP32SINTAB(ang)
end function
' fixed point cos
function FCos(ang as integer) as FP32
ang and=TAB_MASK
return FP32SINTAB(ang)
end function
' fixed point tan
function FTan(ang as integer) as FP32
ang and=TAB_MASK
return (FDiv(FP32SINTAB(ang) shl 16, FP32COSTAB(ang)) shr 16)
end function
' fixed point cos and sin
sub FCosSin(ang as integer,pCos as FP32 ptr,pSin as FP32 ptr)
ang and=TAB_MASK
*pCos=FP32COSTAB(ang)
*pSin=FP32SINTAB(ang)
end sub
' fixed point 3D vector
type VECTOR
declare constructor
' pixel to VECTOR
declare constructor(byval ui as ulong)
declare constructor(byval sx as single,_
byval sy as single,_
byval sz as single)
declare constructor(byref fx as FP32,_
byref fy as FP32,_
byref fz as FP32)
declare constructor(byref v as VECTOR)
declare operator cast as string
' VECTOR to pixel color pset(x,y),vector
declare operator cast as ulong
declare operator +=(byref v as VECTOR)
declare operator -=(byref v as VECTOR)
declare operator *=(byref s as FP32)
declare operator /=(byref s as FP32)
declare function LengthSqrd as FP32
declare function Length as FP32
declare function Distance(byref v as VECTOR) as FP32
declare function Normalized as VECTOR
declare sub Normalize
as FP32 x=any,y=any,z=any
end type
dim shared as VECTOR _v
constructor VECTOR (byval ui as ulong)
dprint("VECTOR(pixel)")
dim as long r = ui and 255 : ui shr= 8
dim as long g = ui and 255 : ui shr= 8
dim as long b = ui and 255
x=I2F(r) : y=I2F(g) : z=I2F(b)
end constructor
constructor VECTOR
dprint("VECTOR()")
x=0:y=0:z=0
end constructor
constructor VECTOR(sx as single,_
sy as single,_
sz as single)
dprint("VECTOR(s,s,s)")
x=S2F(sx) : y=S2F(sy) : z=S2F(sz)
end constructor
constructor VECTOR(byref fx as FP32,_
byref fy as FP32,_
byref fz as FP32)
dprint("VECTOR(fp,fp,fp)")
x=fx : y=fy : z=fz
end constructor
constructor VECTOR(byref v as VECTOR)
dprint("VECTOR(VECTOR)")
x=v.x : y=v.y : z=v.z
end constructor
operator VECTOR.cast as ulong
dprint("pixel=VECTOR")
dim as ulong ur=any,ug=any,ub=any
dim as single s = F2S(x)
if s<0 then s = -s
if s=0 then
ur=0
elseif s>1 then
ur=255
else
ur=s*255
end if
s = F2S(y)
if s<0 then s = -s
if s=0 then
ug=0
elseif s>1 then
ug=255
else
ug=s*255
end if
s = F2S(z)
if s<0 then s = -s
if s=0 then
ub=0
elseif s>1 then
ub=255
else
ub=s*255
end if
operator = RGB(ur,ug,ub)
end operator
operator VECTOR.cast as string
return "V(" & F2S(x) & "," & F2S(y) & "," & F2S(z) & ")"
end operator
operator VECTOR.+=(byref v as VECTOR)
dprint("+=v")
x+=v.x : y+=v.x : z+=v.x
end operator
operator VECTOR.-=(byref v as VECTOR)
x-=v.x : y-=v.x : z-=v.x
end operator
operator VECTOR.*=(byref s as FP32)
dprint("*=s")
x=fmul(x,s)
y=fmul(y,s)
z=fmul(z,s)
end operator
operator VECTOR./=(byref s as FP32)
dprint("/=s")
if s=0 then
x=0:y=0:z=0
else
dim as FP32 inv=FInv(s)
x=fmul(x,inv)
y=fmul(y,inv)
z=fmul(z,inv)
end if
end operator
operator +(byref l as VECTOR,byref r as VECTOR) as VECTOR
dprint("v+v")
return type<VECTOR>(l.x+r.x, l.y+r.y, l.z+r.z)
end operator
operator -(byref l as VECTOR,byref r as VECTOR) as VECTOR
dprint("v-v")
return type<VECTOR>(l.x-r.x, l.y-r.y, l.z-r.z)
end operator
operator *(byref l as VECTOR,byref r as FP32) as VECTOR
dprint("v*s")
return type<VECTOR>(fmul(l.x,r),fmul(l.y,r),fmul(l.z,r))
end operator
operator *(byref l as FP32, byref r as VECTOR) as VECTOR
dprint("s*v")
return type<VECTOR>(fmul(l,r.x),fmul(l,r.y),fmul(l,r.z))
end operator
' dot product
operator *(byref l as VECTOR,byref r as VECTOR) as FP32
dprint("v*v")
return fmul(l.x,r.x)+fmul(l.y,r.y)+fmul(l.z,r.z)
end operator
' cross product
operator \(byref l as VECTOR,byref r as VECTOR) as VECTOR
dprint("v\v")
return type<VECTOR>(fmul(l.y,r.z)-fmul(l.z,r.y), _
fmul(l.z,r.x)-fmul(l.x,r.z), _
fmul(l.x,r.y)-fmul(l.y,r.x))
end operator
function VECTOR.LengthSqrd as FP32
dprint("VECTOR.LengthSqrd()")
return fsqrd(x)+fsqrd(y)+fsqrd(z)
end function
function VECTOR.Length as FP32
dprint("VECTOR.Length()")
return fsqr(fsqrd(x)+fsqrd(y)+fsqrd(z))
end function
function VECTOR.Distance(byref other as VECTOR) as FP32
dprint("VECTOR.Distance(VECTOR)")
_v = this-other
var _l = fsqrd(_v.x) + fsqrd(_v.y) + fsqrd(_v.z)
if _l<>0 then _l = fsqr(_l)
return _l
end function
function VECTOR.Normalized as VECTOR
dprint("VECTOR.Normalized()")
var _l = fsqrd(x)+fsqrd(y)+fsqrd(z)
if _l=0 then
_v.x=0:_v.y=0:_v.z=0
else
_l=finv(fsqr(_l))
_v.x=fmul(x,_l)
_v.y=fmul(y,_l)
_v.z=fmul(z,_l)
end if
return _v
end function
sub VECTOR.Normalize
dprint("VECTOR.Normalize")
var _l = fsqrd(x)+fsqrd(y)+fsqrd(z)
if _l=0 then
x=0 : y=0 : z=0
else
_l=FInv(fsqr(_l))
x=fmul(x,_l)
y=fmul(y,_l)
z=fmul(z,_l)
end if
end sub
' a 4x3 fixed point matrix
' last row asumed 0,0,0,1
type MATRIX
declare constructor
declare constructor(byref m as MATRIX)
declare constructor(byref r00 as FP32,byref r01 as FP32,byref r02 as FP32, _
byref r10 as FP32,byref r11 as FP32,byref r12 as FP32, _
byref r20 as FP32,byref r21 as FP32,byref r22 as FP32)
declare constructor(byref r00 as FP32,byref r01 as FP32,byref r02 as FP32,byref r03 as FP32, _
byref r10 as FP32,byref r11 as FP32,byref r12 as FP32,byref r13 as FP32, _
byref r20 as FP32,byref r21 as FP32,byref r22 as FP32,byref r23 as FP32)
declare constructor(byref a as VECTOR, _
byref b as VECTOR, _
byref c as VECTOR)
declare constructor(byref a as VECTOR, _
byref b as VECTOR, _
byref c as VECTOR, _
byref t as VECTOR)
declare sub Identity
declare sub Scale (byref s as FP32)
declare sub Scale (byref v as VECTOR)
declare sub Translate(byref v as VECTOR)
declare sub Pitch (byval ang as integer)
declare sub Yaw (byval ang as integer)
declare sub Roll (byval ang as integer)
declare sub PYR (byval p as integer, _
byval y as integer, _
byval r as integer)
declare function Rotate (byref v as VECTOR) as VECTOR
declare function Transform(byref v as VECTOR) as VECTOR
as FP32 m00=any,m01=any,m02=any,m03=any
as FP32 m10=any,m11=any,m12=any,m13=any
as FP32 m20=any,m21=any,m22=any,m23=any
end type
dim shared as MATRIX _p,_y,_r
constructor MATRIX
end constructor
constructor MATRIX(byref m as MATRIX)
dprint("MATRIX(MATRIX)")
m00=m.m00:m01=m.m01:m02=m.m02:m03=m.m03
m10=m.m10:m11=m.m11:m12=m.m12:m13=m.m13
m20=m.m20:m21=m.m21:m22=m.m22:m23=m.m23
end constructor
constructor MATRIX(byref r00 as FP32,byref r01 as FP32,byref r02 as FP32, _
byref r10 as FP32,byref r11 as FP32,byref r12 as FP32, _
byref r20 as FP32,byref r21 as FP32,byref r22 as FP32)
dprint("MATRIX(FP,...)")
m00=r00:m01=r01:m02=r02:m03=0
m10=r10:m11=r11:m12=r12:m13=0
m20=r20:m21=r21:m22=r22:m23=0
end constructor
constructor MATRIX(byref r00 as FP32,byref r01 as FP32,byref r02 as FP32,byref r03 as FP32, _
byref r10 as FP32,byref r11 as FP32,byref r12 as FP32,byref r13 as FP32, _
byref r20 as FP32,byref r21 as FP32,byref r22 as FP32,byref r23 as FP32)
dprint("MATRIX(FP,...)")
m00=r00:m01=r01:m02=r02:m03=r03
m10=r10:m11=r11:m12=r12:m13=r13
m20=r20:m21=r21:m22=r22:m23=r23
end constructor
constructor MATRIX(byref a as VECTOR, _
byref b as VECTOR, _
byref c as VECTOR)
dprint("MATRIX(VECTOR,...)")
m00=a.x : m01=a.y : m02=a.z : m03=0
m10=b.x : m11=b.y : m12=b.z : m13=0
m20=c.x : m21=c.y : m22=c.z : m23=0
end constructor
constructor MATRIX(byref a as VECTOR, _
byref b as VECTOR, _
byref c as VECTOR, _
byref t as VECTOR)
dprint("MATRIX(VECTOR,...)")
m00=a.x : m01=a.y : m02=a.z : m03=t.x
m10=b.x : m11=b.y : m12=b.z : m13=t.y
m20=c.x : m21=c.y : m22=c.z : m23=t.z
end constructor
sub MATRIX.Identity
dprint("MATRIX.Identity")
m00=FONE: m01=0 : m02=0 : m03=0
m10=0 : m11=FONE: m12=0 : m13=0
m20=0 : m21=0 : m22=FONE: m23=0
end sub
sub MATRIX.Scale(byref s as FP32)
dprint("MATRIX.Scale(s)")
m00=s: m01=0: m02=0: m03=0
m10=0: m11=s: m12=0: m13=0
m20=0: m21=0: m22=s: m23=0
end sub
sub MATRIX.Scale(byref v as VECTOR)
dprint("MATRIX.Scale(VECTOR)")
m00=v.x: m01=0 : m02=0 : m03=0
m10=0 : m11=v.y: m12=0 : m13=0
m20=0 : m21=0 : m22=v.z: m23=0
end sub
sub MATRIX.Translate(byref v as VECTOR)
dprint("MATRIX.Translate(VECTOR)")
m00=FONE: m01=0 : m02=0 : m03=v.x
m10=0 : m11=FONE: m12=0 : m13=v.y
m20=0 : m21=0 : m22=FONE: m23=v.z
end sub
sub MATRIX.Pitch(byval ang as integer)
dprint("MATRIX.Pitch(ang)")
ang and=TAB_MASK
var s=FP32SINTAB(ang)
var c=FP32COSTAB(ang)
m00=FONE: m01= 0 : m02= 0: m03=0
m10=0 : m11= c : m12= s: m13=0
m20=0 : m21=-s : m22=-c: m23=0
end sub
sub MATRIX.Yaw(byval ang as integer)
dprint("MATRIX.Yaw(ang)")
ang and=TAB_MASK
var s=FP32SINTAB(ang)
var c=FP32COSTAB(ang)
m00= c: m01= 0 : m02=-s: m03=0
m10= 0: m11= FONE: m12= 0: m13=0
m20=-s: m21= 0 : m22= c: m23=0
end sub
sub MATRIX.Roll(byval ang as integer)
dprint("MATRIX.Roll(ang)")
ang and=TAB_MASK
var s=FP32SINTAB(ang)
var c=FP32COSTAB(ang)
m00= c: m01= s: m02= 0 : m03=0
m10=-s: m11= c: m12= 0 : m13=0
m20= 0: m21= 0: m22=FONE: m23=0
end sub
operator *(byref l as MATRIX,byref r as MATRIX) as MATRIX
dprint("MATRIX*MATRIX")
_p.m00 = fmul(l.m00,r.m00) + fmul(l.m01,r.m10) + fmul(l.m02,r.m20)
_p.m01 = fmul(l.m00,r.m01) + fmul(l.m01,r.m11) + fmul(l.m02,r.m21)
_p.m02 = fmul(l.m00,r.m02) + fmul(l.m01,r.m12) + fmul(l.m02,r.m22)
_p.m03 = fmul(l.m00,r.m03) + fmul(l.m01,r.m13) + fmul(l.m02,r.m23) + l.m03
_p.m10 = fmul(l.m10,r.m00) + fmul(l.m11,r.m10) + fmul(l.m12,r.m20)
_p.m11 = fmul(l.m10,r.m01) + fmul(l.m11,r.m11) + fmul(l.m12,r.m21)
_p.m12 = fmul(l.m10,r.m02) + fmul(l.m11,r.m12) + fmul(l.m12,r.m22)
_p.m13 = fmul(l.m10,r.m03) + fmul(l.m11,r.m13) + fmul(l.m12,r.m23) + l.m13
_p.m20 = fmul(l.m20,r.m00) + fmul(l.m21,r.m10) + fmul(l.m22,r.m20)
_p.m21 = fmul(l.m20,r.m01) + fmul(l.m21,r.m11) + fmul(l.m22,r.m21)
_p.m22 = fmul(l.m20,r.m02) + fmul(l.m21,r.m12) + fmul(l.m22,r.m22)
_p.m23 = fmul(l.m20,r.m03) + fmul(l.m21,r.m13) + fmul(l.m22,r.m23) + l.m23
operator = _p
end operator
sub MATRIX.PYR(byval p as integer, _
byval y as integer, _
byval r as integer)
dprint("MATRIX.PYR(p,y,r)")
_p.Pitch(p)
_y.Yaw (y)
_r.Roll (r)
this=_p*_y*_r
end sub
function MATRIX.Rotate(byref v as VECTOR) as VECTOR
dprint("MATRIX.Rotate")
return type<VECTOR>(fmul(m00,v.x) + fmul(m01,v.y) + fmul(m02,v.z), _
fmul(m10,v.x) + fmul(m11,v.y) + fmul(m12,v.z), _
fmul(m20,v.x) + fmul(m21,v.y) + fmul(m22,v.z))
end function
function MATRIX.Transform(byref v as VECTOR) as VECTOR
dprint("MATRIX.Transform")
return type<VECTOR>(fmul(m00,v.x) + fmul(m01,v.y) + fmul(m02,v.z) + m03, _
fmul(m10,v.x) + fmul(m11,v.y) + fmul(m12,v.z) + m13, _
fmul(m20,v.x) + fmul(m21,v.y) + fmul(m22,v.z) + m23)
end function