665 lines
21 KiB
C++
665 lines
21 KiB
C++
#include "memory.h"
|
|
#include "HeadTracker.h"
|
|
#include "Interest.h"
|
|
|
|
typedef struct GC_C_
|
|
{
|
|
float X,Y;
|
|
float XC,YC,Div;
|
|
u32 PITCH;
|
|
u32 Counter;
|
|
u32 Or;
|
|
} GC_C;
|
|
|
|
#define MOLDEC 16
|
|
#define BORDER_LIMIT 11
|
|
|
|
void GET_BLOB(GC_C *GG,s32 *Pixel,s32 And,s32 AddX,s32 AddY)
|
|
{
|
|
if (GG->Counter > 128) return;
|
|
Pixel += AddX + AddY * GG->PITCH;
|
|
if (!(*Pixel & And)) return;
|
|
GG->Or |= *Pixel;
|
|
GG->Counter ++;
|
|
GG->X += AddX;
|
|
GG->Y += AddY;
|
|
float Weight;
|
|
Weight = (*Pixel>>16) + 1;
|
|
GG->XC += GG->X * Weight;
|
|
GG->YC += GG->Y * Weight;
|
|
GG->Div += Weight;
|
|
*Pixel &= ~And;
|
|
GET_BLOB(GG,Pixel,And,+1,0);
|
|
GET_BLOB(GG,Pixel,And,-1,0);
|
|
GET_BLOB(GG,Pixel,And,+0,+1);
|
|
GET_BLOB(GG,Pixel,And,-0,-1);
|
|
GET_BLOB(GG,Pixel,And,+1,+1);
|
|
GET_BLOB(GG,Pixel,And,-1,+1);
|
|
GET_BLOB(GG,Pixel,And,+1,-1);
|
|
GET_BLOB(GG,Pixel,And,-1,-1);//*/
|
|
GG->X -= AddX;
|
|
GG->Y -= AddY;
|
|
}
|
|
|
|
|
|
int BDUG_Thresh1 = 2;
|
|
int BDUG_Thresh2 = 0;
|
|
int BDUG_Thresh3 = 5; // Colors nbit T
|
|
int BDUG_Thresh4 = 5; // Colors nbit T
|
|
int BDUG_BLUR3 = 2;
|
|
int BDUG_Thresh5 = 0;
|
|
int BDUG_BLUR1 = 2;
|
|
int BDUG_BLUR2 = 4;
|
|
|
|
void COMPUTE_FlagMap();
|
|
|
|
//DBG_FAST_TUNNING(2,0,5,5,2,1,1,2)
|
|
void FAST_TUNNING(int T1,int T2,int B1,int B2,int T3,int T4,int T5,int T6)
|
|
{
|
|
BDUG_BLUR1 = B1;
|
|
BDUG_BLUR2 = B2;
|
|
BDUG_Thresh1 = T1;
|
|
BDUG_Thresh2 = T2;
|
|
BDUG_Thresh3 = T3;
|
|
BDUG_Thresh4 = T4;
|
|
BDUG_Thresh5 = T6;
|
|
BDUG_BLUR3 = T5;
|
|
|
|
COMPUTE_FlagMap();
|
|
}
|
|
|
|
u32 FlagMap[65536>>5];
|
|
|
|
#define MOLDEC_MASK ((1<<MOLDEC)-1)
|
|
|
|
void COMPUTE_FlagMap()
|
|
{
|
|
u32 Counter = 65536;
|
|
memset(FlagMap,0,sizeof(FlagMap));
|
|
while (Counter--)
|
|
{
|
|
u32 Countor;
|
|
u32 RES,RESM,RESE;
|
|
Countor = BDUG_Thresh4;
|
|
RES = Counter;
|
|
while (Countor--)
|
|
{
|
|
RESE = RES >> 1;
|
|
RESM = RES << (MOLDEC - 1);
|
|
RESE |= RESM;
|
|
RESE &= 0xffff;
|
|
RES &= RESE;
|
|
}
|
|
if (GetBitNumber(RES) > BDUG_Thresh3)
|
|
FlagMap[Counter>>5] |= 1<<(Counter&31);
|
|
else
|
|
FlagMap[Counter>>5] &= ~(1<<(Counter&31));
|
|
|
|
RES = (Counter >> 8) & 0xff;
|
|
RES |= Counter << 8;
|
|
/*
|
|
if ((GetBitNumber(Counter & RES) < 10))
|
|
FlagMap[Counter>>5] &= ~(1<<(Counter&31));
|
|
else
|
|
FlagMap[Counter>>5] |= 1<<(Counter&31);
|
|
*/
|
|
}
|
|
}
|
|
|
|
void COPY_SQUARE(MAP *MapSRC,MAP *MapDST)
|
|
{
|
|
u32 SQUARET[4096];
|
|
for (int i = 0 ; i < 4096 ; i ++)
|
|
SQUARET[i] = (i*i)>>9;
|
|
s32 *LAST,*DST,*SRC;
|
|
DST = MapDST->GetBase();
|
|
SRC = MapSRC->GetBase();
|
|
for (u32 SizeY = 0; SizeY < MapSRC->SY;SizeY ++)
|
|
{
|
|
u32 Current;
|
|
LAST = DST + MapSRC->SX;
|
|
Current = 0;
|
|
while (DST < LAST)
|
|
{
|
|
*(DST++) = SQUARET[*(SRC++)];
|
|
}
|
|
SRC+=MapSRC->PITCH-MapSRC->SX;
|
|
DST+=MapDST->PITCH-MapSRC->SX;
|
|
}
|
|
}
|
|
|
|
void ORbRound(MAP *MapSRC,MAP *MapDST)
|
|
{
|
|
s32 *LAST,*DST,*SRC;
|
|
for (u32 SizeY = 2; SizeY < MapSRC->SY - 2;SizeY ++)
|
|
{
|
|
SRC = MapSRC->GetBase() + SizeY * MapSRC->PITCH + 2;
|
|
DST = MapDST->GetBase() + SizeY * MapDST->PITCH + 2;
|
|
LAST = SRC + MapSRC->SX - 2 * 2;
|
|
while (SRC < LAST)
|
|
{
|
|
|
|
*DST = *SRC;
|
|
if (*DST & 0xffff0000)
|
|
{
|
|
|
|
*DST |= *(SRC+1);
|
|
*DST |= *(SRC-1);
|
|
*DST |= *(SRC+MapSRC->PITCH);
|
|
*DST |= *(SRC-MapSRC->PITCH);
|
|
|
|
*DST |= *(SRC+1+MapSRC->PITCH);
|
|
*DST |= *(SRC+1-MapSRC->PITCH);
|
|
*DST |= *(SRC-1+MapSRC->PITCH);
|
|
*DST |= *(SRC-1-MapSRC->PITCH);//*/
|
|
}
|
|
SRC++;
|
|
DST++;
|
|
}
|
|
}
|
|
}
|
|
|
|
void FAST_Interest(WORKINGSPACE *WSPC)
|
|
{
|
|
MAP CurrentMapVirt;
|
|
MAP *CurrentMap;
|
|
|
|
MAP WORK0,WORK1,WORK2,WORK3,WORK4;
|
|
|
|
static u32 Init = 1;
|
|
CurrentMap = WSPC->Current_WSP->SRC_BW;
|
|
CurrentMapVirt = *CurrentMap;
|
|
CurrentMapVirt.staticBase = 1;
|
|
CurrentMap = &CurrentMapVirt;
|
|
|
|
WORK0 = *WSPC->WORK[0]; WORK0.staticBase = 1;
|
|
WORK1 = *WSPC->WORK[1]; WORK1.staticBase = 1;
|
|
WORK2 = *WSPC->WORK[2]; WORK2.staticBase = 1;
|
|
WORK3 = *WSPC->WORK[3]; WORK3.staticBase = 1;
|
|
WORK4 = *WSPC->WORK[4]; WORK4.staticBase = 1;
|
|
|
|
s32 MinX,MinY,MaxX,MaxY;
|
|
MinX = MinY = 0;
|
|
MaxX = CurrentMap->SX;
|
|
MaxY = CurrentMap->SY;
|
|
|
|
if (MinX < WSPC->LastMin.x) MinX = WSPC->LastMin.x;
|
|
if (MinY < WSPC->LastMin.y) MinY = WSPC->LastMin.y;
|
|
if (MaxX > WSPC->LastMax.x) MaxX = WSPC->LastMax.x;
|
|
if (MaxY > WSPC->LastMax.y) MaxY = WSPC->LastMax.y;
|
|
MaxX += 4;MaxX &= ~3;
|
|
MinX &= ~3;
|
|
if (MaxX > CurrentMap->SX) MaxX = CurrentMap->SX;
|
|
|
|
CurrentMap->MoveBase(MinY * CurrentMap->PITCH + MinX);
|
|
CurrentMap->SX = MaxX - MinX;
|
|
CurrentMap->SY = MaxY - MinY;
|
|
|
|
WORK0.MoveBase(MinY * WORK0.PITCH + MinX);
|
|
WORK0.SX = MaxX - MinX;
|
|
WORK0.SY = MaxY - MinY;
|
|
|
|
WORK1.MoveBase(MinY * WORK1.PITCH + MinX);
|
|
WORK1.SX = MaxX - MinX;
|
|
WORK1.SY = MaxY - MinY;
|
|
|
|
WORK2.MoveBase(MinY * WORK2.PITCH + MinX);
|
|
WORK2.SX = MaxX - MinX;
|
|
WORK2.SY = MaxY - MinY;
|
|
|
|
WORK3.MoveBase(MinY * WORK3.PITCH + MinX);
|
|
WORK3.SX = MaxX - MinX;
|
|
WORK3.SY = MaxY - MinY;
|
|
|
|
WORK4.MoveBase(MinY * WORK3.PITCH + MinX);
|
|
WORK4.SX = MaxX - MinX;
|
|
WORK4.SY = MaxY - MinY;
|
|
|
|
COPY(CurrentMap,(&WORK0));
|
|
|
|
if (Init) COMPUTE_FlagMap();
|
|
Init = 0;
|
|
|
|
LF_GaussianFilter((&WORK0),(&WORK1),(&WORK3),BDUG_BLUR1,BDUG_BLUR3);
|
|
SUB((&WORK0),(&WORK1));
|
|
LF_GaussianFilter((&WORK0),(&WORK2),(&WORK3),BDUG_BLUR2,BDUG_BLUR3);
|
|
SUB((&WORK0),(&WORK2));
|
|
SUB((&WORK2),(&WORK1));
|
|
COPY((&WORK2),(&WORK0));
|
|
|
|
|
|
SET((&WORK3),0);
|
|
s32 RoundT[16],*pRT;
|
|
{
|
|
/*
|
|
MODE 16 MODE 12 MODE 8
|
|
- - - - - - - - - - - - - - - - - - - - - - - - - - -
|
|
- - - B C D - - - - - - - - - - - - - - - - - - - - -
|
|
- - A - - - E - - - - - 8 9 A - - - - - - - - - - - -
|
|
- 9 - - - - - F - - - 7 - - - B - - - - - 5 6 7 - - -
|
|
- 8 - - X - - 0 - - - 6 - X - 0 - - - - - 4 X 0 - - -
|
|
- 7 - - - - - 1 - - - 5 - - - 1 - - - - - 3 2 1 - - -
|
|
- - 6 - - - 2 - - - - - 4 3 2 - - - - - - - - - - - -
|
|
- - - 5 4 3 - - - - - - - - - - - - - - - - - - - - -
|
|
- - - - - - - - - - - - - - - - - - - - - - - - - - -
|
|
*/
|
|
s32 PITCH;
|
|
PITCH = (&WORK3)->PITCH;
|
|
pRT = RoundT;
|
|
*(pRT++) = 3 + 0 * PITCH;
|
|
*(pRT++) = 3 + 1 * PITCH;
|
|
*(pRT++) = 2 + 2 * PITCH;
|
|
*(pRT++) = 1 + 3 * PITCH;
|
|
*(pRT++) = 0 + 3 * PITCH;
|
|
*(pRT++) = - 1 + 3 * PITCH;
|
|
*(pRT++) = - 2 + 2 * PITCH;
|
|
*(pRT++) = - 3 + 1 * PITCH;
|
|
*(pRT++) = - 3 - 0 * PITCH;
|
|
*(pRT++) = - 3 - 1 * PITCH;
|
|
*(pRT++) = - 2 - 2 * PITCH;
|
|
*(pRT++) = - 1 - 3 * PITCH;
|
|
*(pRT++) = 0 - 3 * PITCH;
|
|
*(pRT++) = 1 - 3 * PITCH;
|
|
*(pRT++) = 2 - 2 * PITCH;
|
|
*(pRT++) = 3 - 1 * PITCH;
|
|
}
|
|
|
|
|
|
//////////////////////////////////////////////////
|
|
// First -> Compute neighbourought flags
|
|
{
|
|
int BDUG_ThreshCorrected;
|
|
static float MATCH_ADD = 0.0f;
|
|
#define MATCH_BLENDER_DOWN 0.99f
|
|
#define MATCH_BLENDER_UP 0.998f
|
|
#define OM_MATCH_BLENDER (1.0f - MATCH_BLENDER_UP)
|
|
if ((WSPC->INTERP_MatchingPercent < 75) && (WSPC->Previous_WSP->nTps > 200))
|
|
MATCH_ADD = MATCH_ADD + OM_MATCH_BLENDER * (int)((75.0f - WSPC->INTERP_MatchingPercent));
|
|
else
|
|
MATCH_ADD = (MATCH_ADD * MATCH_BLENDER_DOWN);
|
|
BDUG_ThreshCorrected = BDUG_Thresh1 + MATCH_ADD;
|
|
|
|
|
|
|
|
BDUG_Thresh2 = (BDUG_ThreshCorrected * 2);
|
|
|
|
for (int y = BORDER_LIMIT +1; y < CurrentMap->SY - BORDER_LIMIT-1; y++)
|
|
{
|
|
int x = BORDER_LIMIT +1;
|
|
s32 *VMi,*Vdesr;
|
|
VMi = (&WORK1)->GetBase() + x + (y) * (&WORK1)->PITCH;
|
|
Vdesr = (&WORK3)->GetBase() + x + (y) * (&WORK3)->PITCH;
|
|
for (; x < CurrentMap->SX - BORDER_LIMIT-1; x++)
|
|
{
|
|
u32 VA;
|
|
u32 Loaded,RES,RES2;
|
|
RES2=RES =0;
|
|
|
|
|
|
VA = *(VMi) + BDUG_ThreshCorrected ;
|
|
pRT = RoundT;
|
|
Loaded = ((u32)(*(VMi + *(pRT++))) - VA);
|
|
RES |= Loaded >> 31; RES<<=1;
|
|
RES2|= ((u32)((int)Loaded + BDUG_Thresh2)) >> 31; RES2<<=1;
|
|
Loaded = ((u32)(*(VMi + *(pRT++))) - VA);
|
|
RES |= Loaded >> 31; RES<<=1;
|
|
RES2|= ((u32)((int)Loaded + BDUG_Thresh2)) >> 31; RES2<<=1;
|
|
Loaded = ((u32)(*(VMi + *(pRT++))) - VA);
|
|
RES |= Loaded >> 31; RES<<=1;
|
|
RES2|= ((u32)((int)Loaded + BDUG_Thresh2)) >> 31; RES2<<=1;
|
|
Loaded = ((u32)(*(VMi + *(pRT++))) - VA);
|
|
RES |= Loaded >> 31; RES<<=1;
|
|
RES2|= ((u32)((int)Loaded + BDUG_Thresh2)) >> 31; RES2<<=1;
|
|
Loaded = ((u32)(*(VMi + *(pRT++))) - VA);
|
|
RES |= Loaded >> 31; RES<<=1;
|
|
RES2|= ((u32)((int)Loaded + BDUG_Thresh2)) >> 31; RES2<<=1;
|
|
Loaded = ((u32)(*(VMi + *(pRT++))) - VA);
|
|
RES |= Loaded >> 31; RES<<=1;
|
|
RES2|= ((u32)((int)Loaded + BDUG_Thresh2)) >> 31; RES2<<=1;
|
|
Loaded = ((u32)(*(VMi + *(pRT++))) - VA);
|
|
RES |= Loaded >> 31; RES<<=1;
|
|
RES2|= ((u32)((int)Loaded + BDUG_Thresh2)) >> 31; RES2<<=1;
|
|
Loaded = ((u32)(*(VMi + *(pRT++))) - VA);
|
|
RES |= Loaded >> 31; RES<<=1;
|
|
RES2|= ((u32)((int)Loaded + BDUG_Thresh2)) >> 31; RES2<<=1;
|
|
Loaded = ((u32)(*(VMi + *(pRT++))) - VA);
|
|
RES |= Loaded >> 31; RES<<=1;
|
|
RES2|= ((u32)((int)Loaded + BDUG_Thresh2)) >> 31; RES2<<=1;
|
|
Loaded = ((u32)(*(VMi + *(pRT++))) - VA);
|
|
RES |= Loaded >> 31; RES<<=1;
|
|
RES2|= ((u32)((int)Loaded + BDUG_Thresh2)) >> 31; RES2<<=1;
|
|
Loaded = ((u32)(*(VMi + *(pRT++))) - VA);
|
|
RES |= Loaded >> 31; RES<<=1;
|
|
RES2|= ((u32)((int)Loaded + BDUG_Thresh2)) >> 31; RES2<<=1;
|
|
Loaded = ((u32)(*(VMi + *(pRT++))) - VA);
|
|
RES |= Loaded >> 31; RES<<=1;
|
|
RES2|= ((u32)((int)Loaded + BDUG_Thresh2)) >> 31; RES2<<=1;
|
|
Loaded = ((u32)(*(VMi + *(pRT++))) - VA);
|
|
RES |= Loaded >> 31; RES<<=1;
|
|
RES2|= ((u32)((int)Loaded + BDUG_Thresh2)) >> 31; RES2<<=1;
|
|
Loaded = ((u32)(*(VMi + *(pRT++))) - VA);
|
|
RES |= Loaded >> 31; RES<<=1;
|
|
RES2|= ((u32)((int)Loaded + BDUG_Thresh2)) >> 31; RES2<<=1;
|
|
Loaded = ((u32)(*(VMi + *(pRT++))) - VA);
|
|
RES |= Loaded >> 31; RES<<=1;
|
|
RES2|= ((u32)((int)Loaded + BDUG_Thresh2)) >> 31; RES2<<=1;
|
|
Loaded = ((u32)(*(VMi + *(pRT++))) - VA);
|
|
RES |= Loaded >> 31; RES<<=1;
|
|
RES2|= ((u32)((int)Loaded + BDUG_Thresh2)) >> 31; RES2<<=1;
|
|
|
|
RES >>= 1;
|
|
RES2 >>= 1;
|
|
RES ^= MOLDEC_MASK;
|
|
|
|
*(Vdesr) = 0;
|
|
if (FlagMap[RES>>5] & (1 << (RES & 0x1f))) *(Vdesr) = 0x0004;
|
|
if (FlagMap[RES2>>5] & (1 << (RES2 & 0x1f))) *(Vdesr) |= 0x0400;
|
|
if (*(Vdesr)) *(Vdesr) |= (GetBitNumber(RES)+GetBitNumber(RES2))<<16;
|
|
|
|
|
|
VMi++;
|
|
Vdesr++;
|
|
}
|
|
}
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
// Third -> extract "BLOB" centers
|
|
for (int y = BORDER_LIMIT +1; y < CurrentMap->SY - BORDER_LIMIT-1; y++)
|
|
{
|
|
s32 *VMa;
|
|
int x = BORDER_LIMIT+1 ;
|
|
VMa = (&WORK3)->GetBase() + x + (y) * (&WORK3)->PITCH;
|
|
for (; x < CurrentMap->SX - BORDER_LIMIT-1; x++)
|
|
{
|
|
GC_C GG;
|
|
if (*VMa )
|
|
{
|
|
/* Find Black Point */
|
|
GG.Div = GG.XC = GG.YC = GG.X = GG.Y = 0.0f;
|
|
GG.Counter = 0;GG.Or = 0;
|
|
GG.PITCH = (&WORK3)->PITCH;
|
|
GET_BLOB(&GG,(&WORK3)->GetBase(),7,x,y);
|
|
if ((GG.Counter > 1))
|
|
{
|
|
//////////////////////////////////////////////////
|
|
// GG contain the new point.
|
|
Vector P;
|
|
P.x = GG.XC / GG.Div;
|
|
P.y = GG.YC / GG.Div;
|
|
memset(&WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps], 0 , sizeof(TRACKEDPOINT));
|
|
WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps].BigE = GG.Counter;
|
|
WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps].X = P.x;
|
|
WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps].Y = P.y;
|
|
WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps].PreviousIndex = 0xffffffff;
|
|
WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps].NextIndex = 0xffffffff;
|
|
|
|
s32 *Error,EMin,EMax;
|
|
Error = (&WORK0)->GetBase() + (int)P.x + (int)P.y * (&WORK0)->PITCH;
|
|
EMin = EMax = *Error;
|
|
#define ERROR_RADIUS 2
|
|
if (EMin > *(Error - ERROR_RADIUS - ERROR_RADIUS*(&WORK0)->PITCH) ) EMin = *(Error - ERROR_RADIUS - ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
if (EMin > *(Error - ERROR_RADIUS ) ) EMin = *(Error - ERROR_RADIUS ) ;
|
|
if (EMin > *(Error - ERROR_RADIUS + ERROR_RADIUS*(&WORK0)->PITCH) ) EMin = *(Error - ERROR_RADIUS + ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
if (EMin > *(Error - 0 - ERROR_RADIUS*(&WORK0)->PITCH) ) EMin = *(Error - 0 - ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
if (EMin > *(Error - 0 + ERROR_RADIUS*(&WORK0)->PITCH) ) EMin = *(Error - 0 + ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
if (EMin > *(Error + ERROR_RADIUS - ERROR_RADIUS*(&WORK0)->PITCH) ) EMin = *(Error + ERROR_RADIUS - ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
if (EMin > *(Error + ERROR_RADIUS ) ) EMin = *(Error + ERROR_RADIUS ) ;
|
|
if (EMin > *(Error + ERROR_RADIUS + ERROR_RADIUS*(&WORK0)->PITCH) ) EMin = *(Error + ERROR_RADIUS + ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
|
|
if (EMax < *(Error - ERROR_RADIUS - ERROR_RADIUS*(&WORK0)->PITCH) ) EMax = *(Error - ERROR_RADIUS - ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
if (EMax < *(Error - ERROR_RADIUS ) ) EMax = *(Error - ERROR_RADIUS ) ;
|
|
if (EMax < *(Error - ERROR_RADIUS + ERROR_RADIUS*(&WORK0)->PITCH) ) EMax = *(Error - ERROR_RADIUS + ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
if (EMax < *(Error - 0 - ERROR_RADIUS*(&WORK0)->PITCH) ) EMax = *(Error - 0 - ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
if (EMax < *(Error - 0 + ERROR_RADIUS*(&WORK0)->PITCH) ) EMax = *(Error - 0 + ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
if (EMax < *(Error + ERROR_RADIUS - ERROR_RADIUS*(&WORK0)->PITCH) ) EMax = *(Error + ERROR_RADIUS - ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
if (EMax < *(Error + ERROR_RADIUS ) ) EMax = *(Error + ERROR_RADIUS ) ;
|
|
if (EMax < *(Error + ERROR_RADIUS + ERROR_RADIUS*(&WORK0)->PITCH) ) EMax = *(Error + ERROR_RADIUS + ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
|
|
WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps].BigE = EMax - EMin;
|
|
|
|
|
|
|
|
WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps].X += MinX;
|
|
WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps].Y += MinY;
|
|
|
|
if (WSPC->Current_WSP->nTps<MAX_POINTS) WSPC->Current_WSP->nTps++;
|
|
}
|
|
/* Find White Point */
|
|
GG.Div = GG.XC = GG.YC = GG.X = GG.Y = 0.0f;
|
|
GG.Counter = 0;GG.Or = 0;
|
|
GG.PITCH = (&WORK3)->PITCH;
|
|
GET_BLOB(&GG,(&WORK3)->GetBase(),7<<8,x,y);
|
|
if ((GG.Counter > 1))
|
|
{
|
|
//////////////////////////////////////////////////
|
|
// GG contain the new point.
|
|
Vector P;
|
|
P.x = GG.XC / GG.Div;
|
|
P.y = GG.YC / GG.Div;
|
|
memset(&WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps], 0 , sizeof(TRACKEDPOINT));
|
|
WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps].BigE = GG.Counter;
|
|
WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps].X = P.x;
|
|
WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps].Y = P.y;
|
|
WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps].PreviousIndex = 0xffffffff;
|
|
WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps].NextIndex = 0xffffffff;
|
|
|
|
s32 *Error,EMin,EMax;
|
|
Error = (&WORK0)->GetBase() + (int)P.x + (int)P.y * (&WORK0)->PITCH;
|
|
EMin = EMax = *Error;
|
|
if (EMin > *(Error - ERROR_RADIUS - ERROR_RADIUS*(&WORK0)->PITCH) ) EMin = *(Error - ERROR_RADIUS - ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
if (EMin > *(Error - ERROR_RADIUS ) ) EMin = *(Error - ERROR_RADIUS ) ;
|
|
if (EMin > *(Error - ERROR_RADIUS + ERROR_RADIUS*(&WORK0)->PITCH) ) EMin = *(Error - ERROR_RADIUS + ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
if (EMin > *(Error - 0 - ERROR_RADIUS*(&WORK0)->PITCH) ) EMin = *(Error - 0 - ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
if (EMin > *(Error - 0 + ERROR_RADIUS*(&WORK0)->PITCH) ) EMin = *(Error - 0 + ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
if (EMin > *(Error + ERROR_RADIUS - ERROR_RADIUS*(&WORK0)->PITCH) ) EMin = *(Error + ERROR_RADIUS - ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
if (EMin > *(Error + ERROR_RADIUS ) ) EMin = *(Error + ERROR_RADIUS ) ;
|
|
if (EMin > *(Error + ERROR_RADIUS + ERROR_RADIUS*(&WORK0)->PITCH) ) EMin = *(Error + ERROR_RADIUS + ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
|
|
if (EMax < *(Error - ERROR_RADIUS - ERROR_RADIUS*(&WORK0)->PITCH) ) EMax = *(Error - ERROR_RADIUS - ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
if (EMax < *(Error - ERROR_RADIUS ) ) EMax = *(Error - ERROR_RADIUS ) ;
|
|
if (EMax < *(Error - ERROR_RADIUS + ERROR_RADIUS*(&WORK0)->PITCH) ) EMax = *(Error - ERROR_RADIUS + ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
if (EMax < *(Error - 0 - ERROR_RADIUS*(&WORK0)->PITCH) ) EMax = *(Error - 0 - ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
if (EMax < *(Error - 0 + ERROR_RADIUS*(&WORK0)->PITCH) ) EMax = *(Error - 0 + ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
if (EMax < *(Error + ERROR_RADIUS - ERROR_RADIUS*(&WORK0)->PITCH) ) EMax = *(Error + ERROR_RADIUS - ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
if (EMax < *(Error + ERROR_RADIUS ) ) EMax = *(Error + ERROR_RADIUS ) ;
|
|
if (EMax < *(Error + ERROR_RADIUS + ERROR_RADIUS*(&WORK0)->PITCH) ) EMax = *(Error + ERROR_RADIUS + ERROR_RADIUS*(&WORK0)->PITCH) ;
|
|
|
|
WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps].BigE = EMax - EMin;
|
|
|
|
|
|
WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps].X += MinX;
|
|
WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps].Y += MinY;
|
|
|
|
if (WSPC->Current_WSP->nTps<MAX_POINTS) WSPC->Current_WSP->nTps++;
|
|
}
|
|
|
|
}
|
|
VMa++;
|
|
}
|
|
}
|
|
}
|
|
|
|
static s32 recurse_iDX,recurse_iDY,recurse_DX,recurse_DY,recurse_Number,recurse_Error;
|
|
#define PARSER(func) func(1,0);func(-1,0);func(0,1);func(0,-1);
|
|
|
|
void recurse_GET_GCenter(s32 *Pixel,s32 Pitch)
|
|
{
|
|
*Pixel &= ~1;
|
|
#define GOTHER(a,b) \
|
|
Pixel += a + b * Pitch;\
|
|
if (*Pixel & 1) \
|
|
{\
|
|
recurse_DX+=a;recurse_DY+=b;recurse_Number++;\
|
|
recurse_Error += *Pixel>>2;\
|
|
recurse_iDX += recurse_DX;recurse_iDY += recurse_DY;\
|
|
recurse_GET_GCenter(Pixel,Pitch);\
|
|
recurse_DX-=a;recurse_DY-=b;\
|
|
}\
|
|
Pixel -= a + b * Pitch;
|
|
|
|
PARSER(GOTHER);
|
|
|
|
}
|
|
|
|
void FAST_Interest2(WORKINGSPACE *WSPC)
|
|
{
|
|
MAP CurrentMapVirt;
|
|
MAP *CurrentMap;
|
|
|
|
MAP WORK0,WORK1,WORK2,WORK3,WORK4;
|
|
|
|
static u32 Init = 1;
|
|
CurrentMap = WSPC->Current_WSP->SRC_BW;
|
|
CurrentMapVirt = *CurrentMap;
|
|
CurrentMapVirt.staticBase = 1;
|
|
CurrentMap = &CurrentMapVirt;
|
|
if (WSPC->ZBUFFERMAP)
|
|
{
|
|
WSPC->LastMin = Vector(0,0,0);
|
|
WSPC->LastMax = Vector(WSPC->DRAWMAP->SX,WSPC->DRAWMAP->SY,0);
|
|
}
|
|
|
|
WORK0 = *WSPC->WORK[0]; WORK0.staticBase = 1;
|
|
WORK1 = *WSPC->WORK[1]; WORK1.staticBase = 1;
|
|
WORK2 = *WSPC->WORK[2]; WORK2.staticBase = 1;
|
|
WORK3 = *WSPC->WORK[3]; WORK3.staticBase = 1;
|
|
WORK4 = *WSPC->WORK[4]; WORK4.staticBase = 1;
|
|
|
|
s32 MinX,MinY,MaxX,MaxY;
|
|
MinX = MinY = 0;
|
|
MaxX = CurrentMap->SX;
|
|
MaxY = CurrentMap->SY;
|
|
|
|
if (MinX < WSPC->LastMin.x) MinX = WSPC->LastMin.x;
|
|
if (MinY < WSPC->LastMin.y) MinY = WSPC->LastMin.y;
|
|
if (MaxX > WSPC->LastMax.x) MaxX = WSPC->LastMax.x;
|
|
if (MaxY > WSPC->LastMax.y) MaxY = WSPC->LastMax.y;
|
|
MaxX += 4;MaxX &= ~3;
|
|
MinX &= ~3;
|
|
if (MaxX > CurrentMap->SX) MaxX = CurrentMap->SX;
|
|
|
|
CurrentMap->MoveBase(MinY * CurrentMap->PITCH + MinX);
|
|
CurrentMap->SX = MaxX - MinX;
|
|
CurrentMap->SY = MaxY - MinY;
|
|
|
|
WORK0.MoveBase(MinY * WORK0.PITCH + MinX);
|
|
WORK0.SX = MaxX - MinX;
|
|
WORK0.SY = MaxY - MinY;
|
|
|
|
WORK1.MoveBase(MinY * WORK1.PITCH + MinX);
|
|
WORK1.SX = MaxX - MinX;
|
|
WORK1.SY = MaxY - MinY;
|
|
|
|
WORK2.MoveBase(MinY * WORK2.PITCH + MinX);
|
|
WORK2.SX = MaxX - MinX;
|
|
WORK2.SY = MaxY - MinY;
|
|
|
|
WORK3.MoveBase(MinY * WORK3.PITCH + MinX);
|
|
WORK3.SX = MaxX - MinX;
|
|
WORK3.SY = MaxY - MinY;
|
|
|
|
WORK4.MoveBase(MinY * WORK3.PITCH + MinX);
|
|
WORK4.SX = MaxX - MinX;
|
|
WORK4.SY = MaxY - MinY;
|
|
|
|
COPY(CurrentMap,&WORK0);
|
|
MAP32_BLUR_Inplace(&WORK0 , 1 , 1);
|
|
MAP32_SHIFT(&WORK0,-4);
|
|
|
|
COPY(&WORK0,&WORK2);
|
|
MAP32_BLUR_Inplace(&WORK2 , 3 , 1);
|
|
SUB(&WORK2,&WORK0);
|
|
MAP32_SQR(&WORK0);
|
|
COPY(&WORK0,&WORK2);
|
|
MAP32_BLUR_Inplace(&WORK0 , 2 , 1);
|
|
MAP32_BLUR_Inplace(&WORK2 , 3 , 1);
|
|
|
|
s32 *WORK0BASE = WORK0.GetBase();
|
|
s32 *WORK2BASE = WORK2.GetBase();
|
|
s32 *WORK0BASE_LEND;
|
|
s32 *WORK0BASE_MEND = WORK0BASE + WORK0.PITCH*WORK0.SY;
|
|
|
|
while (WORK0BASE < WORK0BASE_MEND)
|
|
{
|
|
WORK0BASE_LEND = WORK0BASE + WORK0.SX;
|
|
while (WORK0BASE < WORK0BASE_LEND)
|
|
{
|
|
float Factor,Factor2;
|
|
Factor = (float)(*WORK0BASE);
|
|
Factor2 = (float)(*WORK2BASE);
|
|
*WORK0BASE = 0;
|
|
*WORK2BASE &= ~1;
|
|
if (Factor > (Factor2) * 1.4)
|
|
{
|
|
if (*WORK2BASE > 10000)
|
|
{
|
|
*WORK2BASE |= 1;
|
|
}
|
|
}
|
|
WORK0BASE++;
|
|
WORK2BASE++;
|
|
}//*/
|
|
WORK0BASE += WORK0.PITCH-WORK0.SX;
|
|
WORK2BASE += WORK2.PITCH-WORK2.SX;
|
|
}
|
|
WORK0BASE = WORK0.GetBase();
|
|
WORK2BASE = WORK2.GetBase();
|
|
|
|
|
|
SET_BORDER(&WORK2,16,0);
|
|
for (int Y = 2 ; Y < WORK2.SY - 2; Y++)
|
|
for (int X = 2 ; X < WORK2.SX - 2; X++)
|
|
{
|
|
if (WORK2BASE[X + Y * WORK2.PITCH] & 1)
|
|
{
|
|
recurse_DX = recurse_iDX = X; recurse_DY = recurse_iDY = Y;
|
|
recurse_Number = 1;
|
|
recurse_Error = 0;
|
|
recurse_GET_GCenter(&WORK2BASE[X + Y * WORK2.PITCH],WORK2.PITCH);
|
|
if ((recurse_Number>2) /*&& ((float)recurse_Error/(float)recurse_Number > 100.0f)*/)
|
|
{
|
|
void DrawCross(MAP *image, POINT p, u32 color, int len, char orient = 3);
|
|
POINT p;
|
|
p.x = ((float)recurse_iDX / (float)recurse_Number) + MinX;
|
|
p.y = ((float)recurse_iDY / (float)recurse_Number) + MinY;
|
|
//if ((WSPC->ZBUFFERMAP == NULL) || ((((float *)WSPC->ZBUFFERMAP->GetBase())[p.x + p.y * WSPC->ZBUFFERMAP->PITCH] < 1000.0) && (((float *)WSPC->ZBUFFERMAP->GetBase())[p.x + p.y * WSPC->ZBUFFERMAP->PITCH] != 0)))
|
|
{
|
|
WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps].BigE = ((float)recurse_Error / (float)recurse_Number);
|
|
//DrawCross(CurrentMap,p,0xff00,2);
|
|
memset(&WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps], 0 , sizeof(TRACKEDPOINT));
|
|
WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps].X = p.x ;
|
|
WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps].Y = p.y ;
|
|
WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps].PreviousIndex = 0xffffffff;
|
|
WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps].NextIndex = 0xffffffff;
|
|
if (WSPC->ZBUFFERMAP)
|
|
{
|
|
WSPC->Current_WSP->TPS[WSPC->Current_WSP->nTps].fZFromZbuffer = ((float *)WSPC->ZBUFFERMAP->GetBase())[p.x + p.y * WSPC->ZBUFFERMAP->PITCH] / 12.0f;
|
|
}
|
|
if (WSPC->Current_WSP->nTps<MAX_POINTS) WSPC->Current_WSP->nTps++;
|
|
|
|
/*char moncul[2048];
|
|
sprintf(moncul,"%.2f",((float *)WSPC->ZBUFFERMAP->GetBase())[p.x + p.y * WSPC->ZBUFFERMAP->PITCH]);
|
|
DRawTEXT_Soft_B((u8*)moncul,(u32*)WSPC->RENDERMAP->GetBase(),p.x, p.y, WSPC->RENDERMAP->PITCH, WSPC->RENDERMAP->SY, 0xff00f0f0);*/
|
|
}
|
|
}
|
|
}
|
|
}//*/
|
|
|
|
//COPY_RGBA(&WORK0,WSPC->RENDERMAP);
|
|
}
|
|
|
|
static int __cdecl TRACKEDPOINT_Compare(const void *elem1, const void *elem2 )
|
|
{
|
|
if (((TRACKEDPOINT *)elem2)->BigE < ((TRACKEDPOINT *)elem1)->BigE)
|
|
return -1;
|
|
else
|
|
return 1;
|
|
}
|
|
|
|
|
|
void FIND_INTEREST_WSCP2(WORKINGSPACE *WSPC)
|
|
{
|
|
WSPC->Current_WSP->nTps = 0;
|
|
FAST_Interest2(WSPC);
|
|
}
|