353 lines
No EOL
10 KiB
C++
353 lines
No EOL
10 KiB
C++
#include "TRACK.H"
|
|
extern int OFFSETHALF(FFT_Complex *c,int nx,int ny);
|
|
extern void FFT_TRANSFORM_HOUGH(FFT_Complex *c,int nx,int ny);
|
|
//#pragma optimize("",off)
|
|
#define SPECTRE_SIZE_MAX 12 // 1024
|
|
|
|
typedef struct tdst_FFT_
|
|
{
|
|
unsigned int BorderSizePo2; // Big map
|
|
FFT_Complex *pSpectroMap;
|
|
FFT_Complex FFT_COSTABLE[(1 << SPECTRE_SIZE_MAX) + 400] ;
|
|
FFT_Complex FFT_COSTABLE_Reversed[(1 << SPECTRE_SIZE_MAX) + 400] ;
|
|
} tdst_FFT;
|
|
|
|
void*WATERFFT_Alloc (unsigned int ulSize) { return CC_malloc(ulSize);}
|
|
void WATERFFT_Free (void *ptr) { CC_free(ptr);}
|
|
void WATERFFT_Memset0 (void *ptr,unsigned int size) { memset(ptr,0,size);}
|
|
void WATERFFT_SwapXY(FFT_Complex *c,int m,int i, int j)
|
|
{
|
|
int Counter;
|
|
FFT_Complex *pi,*pj,*piY,*pjY,SWAP;
|
|
/* 1 Swap Lines */
|
|
pi = c + (i<<m);
|
|
pj = c + (j<<m);
|
|
piY = c + i;
|
|
pjY = c + j;
|
|
Counter = 1 << m;
|
|
while (Counter--)
|
|
{
|
|
SWAP = *pi;
|
|
*pi = *pj;
|
|
*pj = SWAP;
|
|
pj++;pi++;
|
|
}
|
|
/* 2 Swap columns */
|
|
Counter = 1 << m;
|
|
while (Counter--)
|
|
{
|
|
SWAP = *piY;
|
|
*piY = *pjY;
|
|
*pjY = SWAP;
|
|
pjY+=1<<m;
|
|
piY+=1<<m;
|
|
}
|
|
}
|
|
void WATERFFT_Pretranspose_Table(FFT_Complex *c,int m)
|
|
{
|
|
int nn,i,j,k,i2;
|
|
/* Calculate the number of points */
|
|
nn = 1 << m;
|
|
/* Do the bit reversal */
|
|
i2 = nn >> 1;
|
|
j = 0;
|
|
for (i=0;i<nn-1;i++)
|
|
{
|
|
if (i < j) WATERFFT_SwapXY(c,m,i,j);
|
|
k = i2;
|
|
while (k <= j) {j -= k;k >>= 1;}
|
|
j += k;
|
|
}
|
|
}
|
|
_inline void WATERFFT_ComplexMulSubAdd_SQR(register FFT_Complex *p_xy00 , register FFT_Complex *p_xy10 , register FFT_Complex *p_xy01 , register FFT_Complex *p_xy11 , register FFT_Complex *p_CosTableX , register FFT_Complex *p_CosTableY )
|
|
{
|
|
FFT_Complex Tr0,Tr1,Tr2,Tr3;
|
|
|
|
Tr0.re = p_CosTableX->re * p_xy01->re - p_CosTableX->im * p_xy01->im;
|
|
Tr0.im = p_CosTableX->re * p_xy01->im + p_CosTableX->im * p_xy01->re;
|
|
Tr1.re = p_CosTableX->re * p_xy11->re - p_CosTableX->im * p_xy11->im;
|
|
Tr1.im = p_CosTableX->re * p_xy11->im + p_CosTableX->im * p_xy11->re;
|
|
|
|
Tr2.re = p_CosTableY->re * p_xy10->re - p_CosTableY->im * p_xy10->im;
|
|
Tr2.im = p_CosTableY->re * p_xy10->im + p_CosTableY->im * p_xy10->re;
|
|
Tr3.re = p_CosTableY->re * Tr1.re - p_CosTableY->im * Tr1.im;
|
|
Tr3.im = p_CosTableY->re * Tr1.im + p_CosTableY->im * Tr1.re;
|
|
p_xy10->re = p_xy00->re + Tr0.re - Tr3.re - Tr2.re;
|
|
p_xy11->re = p_xy00->re - Tr0.re + Tr3.re - Tr2.re;
|
|
p_xy01->re = p_xy00->re - Tr0.re - Tr3.re + Tr2.re;
|
|
p_xy00->re = p_xy00->re + Tr0.re + Tr3.re + Tr2.re;
|
|
p_xy10->im = p_xy00->im + Tr0.im - Tr3.im - Tr2.im;
|
|
p_xy11->im = p_xy00->im - Tr0.im + Tr3.im - Tr2.im;
|
|
p_xy01->im = p_xy00->im - Tr0.im - Tr3.im + Tr2.im;
|
|
p_xy00->im = p_xy00->im + Tr0.im + Tr3.im + Tr2.im;
|
|
}
|
|
void WATERFFT_ComplexMulSubAdd_SQR_DoubleXY(register FFT_Complex *p_xy00 , register FFT_Complex *p_xy10 , register FFT_Complex *p_xy01 , register FFT_Complex *p_xy11 , register FFT_Complex *p_CosTableX , register FFT_Complex *p_CosTableXP1 , register FFT_Complex *p_CosTableY , register FFT_Complex *p_CosTableYP1 , register int Pitch)
|
|
{
|
|
WATERFFT_ComplexMulSubAdd_SQR(p_xy00 , p_xy10 , p_xy01 , p_xy11 , p_CosTableX , p_CosTableY );
|
|
WATERFFT_ComplexMulSubAdd_SQR(p_xy00 + 1 , p_xy10 + 1 , p_xy01 + 1 , p_xy11 + 1 , p_CosTableXP1 , p_CosTableY );
|
|
WATERFFT_ComplexMulSubAdd_SQR(p_xy00 + Pitch , p_xy10 + Pitch , p_xy01 + Pitch , p_xy11 + Pitch , p_CosTableX , p_CosTableYP1 );
|
|
WATERFFT_ComplexMulSubAdd_SQR(p_xy00 + Pitch + 1, p_xy10 + Pitch + 1 , p_xy01 + Pitch + 1, p_xy11 + Pitch + 1, p_CosTableXP1 , p_CosTableYP1 );
|
|
}
|
|
|
|
extern int FFT2D(FFT_Complex *c,int nx,int ny,int dir);
|
|
|
|
void WATERFFT_FFT2D(FFT_Complex *p_xy,int m,FFT_Complex *FFT_COSTABLE)
|
|
{
|
|
FFT_Complex *p_CosTable;
|
|
unsigned int SPo2;
|
|
int Mpo2Counter;
|
|
/******************************** TRANSFORM *********************************/
|
|
WATERFFT_Pretranspose_Table(p_xy,m);
|
|
p_CosTable = FFT_COSTABLE;
|
|
SPo2 = 1;
|
|
{
|
|
unsigned int XCounter,YCounter,MASK,MASK_C,PITCH,TileCounter;
|
|
TileCounter = (1 << m)>>1;
|
|
MASK = ~(SPo2 - 1);
|
|
MASK_C = SPo2 - 1;
|
|
PITCH = (SPo2 << m);
|
|
for (YCounter = 0 ; YCounter < TileCounter ; YCounter++ )
|
|
{
|
|
FFT_Complex *p_Y;
|
|
FFT_Complex *p_CosTableY;
|
|
p_CosTableY = p_CosTable + (YCounter & MASK_C);
|
|
p_Y = p_xy + ((YCounter + (YCounter & MASK)) << m);
|
|
for (XCounter = 0 ; XCounter < TileCounter ; XCounter++ )
|
|
{
|
|
FFT_Complex *p_X,*p_Xnl;
|
|
p_X = p_Y + XCounter + (XCounter & MASK);
|
|
p_Xnl = p_X + PITCH;
|
|
WATERFFT_ComplexMulSubAdd_SQR(p_X , p_Xnl , p_X + SPo2 , p_Xnl + SPo2 , p_CosTable + (XCounter & MASK_C) , p_CosTableY );
|
|
}
|
|
}
|
|
p_CosTable += SPo2;
|
|
SPo2 <<= 1;
|
|
}//*/
|
|
for (Mpo2Counter = 1 ; Mpo2Counter < m ; Mpo2Counter ++)
|
|
{
|
|
unsigned int XCounter,YCounter,MASK,MASK_C,PITCH,TileCounter,LinePitch;
|
|
TileCounter = (1 << m)>>1;
|
|
MASK = ~(SPo2 - 1);
|
|
MASK_C = SPo2 - 1;
|
|
PITCH = (SPo2 << m);
|
|
LinePitch = 1 << m;
|
|
for (YCounter = 0 ; YCounter < TileCounter ; YCounter+=2 )
|
|
{
|
|
FFT_Complex *p_Y;
|
|
FFT_Complex *p_CosTableY,*p_CosTableYP1;
|
|
p_CosTableY = p_CosTable + (YCounter & MASK_C);
|
|
p_CosTableYP1 = p_CosTable + ((YCounter + 1) & MASK_C);
|
|
p_Y = p_xy + ((YCounter + (YCounter & MASK)) << m);
|
|
for (XCounter = 0 ; XCounter < TileCounter ; XCounter+=2 )
|
|
{
|
|
FFT_Complex *p_X,*p_Xnl;
|
|
p_X = p_Y + XCounter + (XCounter & MASK);
|
|
p_Xnl = p_X + PITCH;
|
|
WATERFFT_ComplexMulSubAdd_SQR_DoubleXY(p_X , p_Xnl , p_X + SPo2 , p_Xnl + SPo2 , p_CosTable + (XCounter & MASK_C) , p_CosTable + ((XCounter + 1) & MASK_C) , p_CosTableY , p_CosTableYP1 , LinePitch);
|
|
}
|
|
}
|
|
p_CosTable += SPo2;
|
|
SPo2 <<= 1;
|
|
}
|
|
}
|
|
void FFTDestroy(void *FFT)
|
|
{
|
|
tdst_FFT *p_FFT;
|
|
p_FFT = (tdst_FFT *)FFT;
|
|
if (p_FFT->pSpectroMap) WATERFFT_Free(p_FFT->pSpectroMap);
|
|
WATERFFT_Free(FFT);
|
|
}
|
|
void WATERFFT_FFT_COMPUTE_COST(int m,FFT_Complex *CosTable, float mult)
|
|
{
|
|
int nn,l,l1,l2;
|
|
TYPE_Of_PREC z;
|
|
FFT_Complex U,C;
|
|
nn = 1 << m;
|
|
l2 = 1;
|
|
C.re = -1.0;
|
|
C.im =0.0;
|
|
for (l=0;l<m;l++)
|
|
{
|
|
l1 = l2;l2 <<= 1;
|
|
U.re = 1.0f;
|
|
U.im = 0.0f;
|
|
while (l1--)
|
|
{
|
|
CosTable->re = U.re ;
|
|
CosTable->im = U.im ;
|
|
CosTable++;
|
|
z = U.re * C.re - U.im * C.im;
|
|
U.im = U.re * C.im + U.im * C.re;
|
|
U.re = z;
|
|
}
|
|
C.im = (TYPE_Of_PREC)(mult*sqrt((1.0 - C.re) / 2.0)); //
|
|
C.re = (TYPE_Of_PREC)(sqrt((1.0 + C.re) / 2.0));
|
|
}
|
|
}
|
|
tdst_FFT *WATERFFT_Init_Spectre(int SizePo2)
|
|
{
|
|
tdst_FFT *p_FFT;
|
|
p_FFT = (tdst_FFT *)WATERFFT_Alloc(sizeof (tdst_FFT ));
|
|
p_FFT->BorderSizePo2 = SizePo2;
|
|
p_FFT->pSpectroMap = (FFT_Complex*)WATERFFT_Alloc(sizeof(FFT_Complex) << (p_FFT->BorderSizePo2 << 1));
|
|
|
|
WATERFFT_FFT_COMPUTE_COST(p_FFT->BorderSizePo2,p_FFT->FFT_COSTABLE,1.0f);
|
|
WATERFFT_FFT_COMPUTE_COST(p_FFT->BorderSizePo2,p_FFT->FFT_COSTABLE_Reversed,-1.0f);
|
|
return p_FFT;
|
|
}
|
|
|
|
void *Image2FFT(int *Picture,int SizePo2,int Pitch)
|
|
{
|
|
tdst_FFT *p_FFT;
|
|
float Average = 0;
|
|
float AverageSqr = 0;
|
|
FFT_Complex *pLinePtr;
|
|
int CounterX,CounterY;
|
|
p_FFT = WATERFFT_Init_Spectre(SizePo2);
|
|
pLinePtr = p_FFT->pSpectroMap;
|
|
CounterY = 1 << p_FFT->BorderSizePo2;
|
|
while (CounterY--)
|
|
{
|
|
CounterX = 1 << p_FFT->BorderSizePo2;
|
|
while (CounterX--)
|
|
{
|
|
pLinePtr->re = pLinePtr->im = 0.f;
|
|
int V;
|
|
V = (*(Picture++)>>8) & 0xff;
|
|
//if (V < 0) V += 1<<2;
|
|
pLinePtr->re = (float)/*abs*/(V) / 256.0f;
|
|
pLinePtr->im = 0.0f;
|
|
|
|
Average += pLinePtr->re;
|
|
AverageSqr += pLinePtr->re * pLinePtr->re;
|
|
|
|
pLinePtr++;
|
|
}
|
|
Picture -= 1 << p_FFT->BorderSizePo2;
|
|
Picture += Pitch;
|
|
}
|
|
|
|
Average /= (1 << (p_FFT->BorderSizePo2 * 2));
|
|
AverageSqr /= (1 << (p_FFT->BorderSizePo2 * 2));
|
|
float Renorm = 1.0f / (sqrt(AverageSqr - Average * Average) + 0.05);
|
|
|
|
pLinePtr = p_FFT->pSpectroMap;
|
|
CounterY = 1 << p_FFT->BorderSizePo2;
|
|
while (CounterY--)
|
|
{
|
|
CounterX = 1 << p_FFT->BorderSizePo2;
|
|
while (CounterX--)
|
|
{
|
|
pLinePtr->re -= Average;
|
|
pLinePtr->re *= Renorm;
|
|
|
|
/* guauss envellop */
|
|
float RadiusX,RadiusY,SS,Gauss;
|
|
RadiusX = (float)((1 << (p_FFT->BorderSizePo2 - 1)) - CounterX) / (float)(1 << (p_FFT->BorderSizePo2 - 1));
|
|
RadiusY = (float)((1 << (p_FFT->BorderSizePo2 - 1)) - CounterY) / (float)(1 << (p_FFT->BorderSizePo2 - 1));
|
|
Gauss = sqrt(RadiusX * RadiusX + RadiusY * RadiusY) * 1.5f;
|
|
// if (Gauss > 1.0f) Gauss = 1.0f;
|
|
float Sygma = sqrt(0.2f);
|
|
float mu = 0.0f;
|
|
|
|
float a = 1.0f / (Sygma * sqrt(2.0f));
|
|
float b = mu;
|
|
float c = Sygma;
|
|
Gauss = a * exp(- (Gauss - b)*(Gauss - b) / (2*Sygma*Sygma)) ;
|
|
if (Gauss > 1.0f) Gauss = 1.0f;
|
|
pLinePtr->re *= Gauss ;
|
|
pLinePtr->im *= Gauss ;//*/
|
|
|
|
pLinePtr++;
|
|
}
|
|
Picture -= 1 << p_FFT->BorderSizePo2;
|
|
Picture += Pitch;
|
|
}
|
|
|
|
//OFFSETHALF(p_FFT->pSpectroMap,1 << p_FFT->BorderSizePo2,1 << p_FFT->BorderSizePo2);
|
|
FFT2D(p_FFT->pSpectroMap,1<<p_FFT->BorderSizePo2,1<<p_FFT->BorderSizePo2,0);
|
|
return (void*)p_FFT;
|
|
}
|
|
|
|
void FFT_TRANSFORM_HOUGH(FFT_Complex *c,int nx,int ny);
|
|
void FFT2Image(int *Picture,void *FFT,int Pitch)
|
|
{
|
|
FFT_Complex *pLinePtr;
|
|
int CounterX,CounterY;
|
|
tdst_FFT *p_FFT;
|
|
float Min,Max;
|
|
p_FFT =(tdst_FFT *)FFT;
|
|
|
|
pLinePtr = p_FFT->pSpectroMap;
|
|
CounterY = 1 << p_FFT->BorderSizePo2;
|
|
while (CounterY--)
|
|
{
|
|
CounterX = 1 << p_FFT->BorderSizePo2;
|
|
while (CounterX--)
|
|
{
|
|
float res;
|
|
|
|
pLinePtr->im *= 50.0f;
|
|
pLinePtr->re *= 50.0f;
|
|
pLinePtr->im += 128.0f;
|
|
pLinePtr->re += 128.0f;
|
|
/**/
|
|
/* pLinePtr->re *= pLinePtr->re;
|
|
pLinePtr->im *= pLinePtr->im;
|
|
pLinePtr->re += pLinePtr->im ;
|
|
pLinePtr->im = 0;
|
|
pLinePtr->re = (pLinePtr->re)/16.0f;//*/
|
|
pLinePtr++;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
OFFSETHALF(p_FFT->pSpectroMap,1 << p_FFT->BorderSizePo2,1 << p_FFT->BorderSizePo2);
|
|
//FFT_TRANSFORM_HOUGH(p_FFT->pSpectroMap,1 << p_FFT->BorderSizePo2,1 << p_FFT->BorderSizePo2);
|
|
//OFFSETHALF(p_FFT->pSpectroMap,1 << p_FFT->BorderSizePo2,1 << p_FFT->BorderSizePo2);
|
|
|
|
pLinePtr = p_FFT->pSpectroMap;
|
|
CounterY = 1 << p_FFT->BorderSizePo2;
|
|
while (CounterY--)
|
|
{
|
|
CounterX = 1 << p_FFT->BorderSizePo2;
|
|
while (CounterX--)
|
|
{
|
|
float res;
|
|
res = (float)pLinePtr->re;
|
|
if (res < 0) res = 0;
|
|
if (res >255) res = 255;
|
|
*Picture = res;
|
|
res = (float)pLinePtr->im;
|
|
if (res < 0) res = 0;
|
|
if (res >255) res = 255;
|
|
pLinePtr++;
|
|
*Picture++ |= (int)res <<8;
|
|
}
|
|
Picture -= 1 << p_FFT->BorderSizePo2;
|
|
Picture += Pitch;
|
|
}
|
|
|
|
FFTDestroy(p_FFT);
|
|
|
|
}
|
|
|
|
void FFTMultiply(void *FFT1,void *FFT2)
|
|
{
|
|
FFT_Complex Local;
|
|
int CounterX,CounterY;
|
|
FFT_Complex *p_FFT1,*p_FFT2;
|
|
p_FFT1 = ((tdst_FFT *)FFT1)->pSpectroMap;
|
|
p_FFT2 = ((tdst_FFT *)FFT2)->pSpectroMap;
|
|
CounterX = 1 << (((tdst_FFT *)FFT1)->BorderSizePo2 << 1);
|
|
while (CounterX--)
|
|
{
|
|
p_FFT1->re =- p_FFT1->re /*- p_FFT1->im * p_FFT2->im*/;
|
|
Local.re = p_FFT1->re * p_FFT2->re - p_FFT1->im * p_FFT2->im;
|
|
Local.im = p_FFT1->re * p_FFT2->im + p_FFT1->im * p_FFT2->re;
|
|
*p_FFT1 = Local;
|
|
p_FFT1++;
|
|
p_FFT2++;
|
|
}
|
|
} |