1397 lines
36 KiB
C++
1397 lines
36 KiB
C++
#include "TRACK.h"
|
|
#include "Rrasters.h"
|
|
#if 0
|
|
|
|
static unsigned char *TEXT[1024];
|
|
|
|
#define TRACKING_PREC 4.0f
|
|
#define OPTIMIZE
|
|
static s32 Map[512 * 512 * 4];
|
|
static s32 Map1[512 * 512 * 4];
|
|
static s32 Map2[512 * 512 * 4];
|
|
static s32 Map3[512 * 512 * 4];
|
|
static s32 Map4[512 * 512 * 4];
|
|
MATRIX FACE_FROMHAAR;
|
|
#define BORDER_LIMIT 1
|
|
|
|
#define DEVIATION_BASE 96
|
|
|
|
#define WINDOW_SUM_FACTOR_II 0.85f
|
|
#define WINDOW_SUM_FACTOR_II2 1.0f
|
|
#define FIRST_SEARCH_LEVEL 0//2
|
|
//#define WINDOW_SUM_FACTOR 0.4f // 0.3f
|
|
|
|
#define MAKE_II
|
|
|
|
typedef struct
|
|
{
|
|
float X,Y;
|
|
float W;
|
|
float Scale;
|
|
float Alpha;
|
|
u32 Age;
|
|
float ScaleV;
|
|
/* Tracking History */
|
|
u32 LastIndex;
|
|
u32 YesNoSir;
|
|
u32 FSP;
|
|
u32 FSPB[32];
|
|
} TRACKEDPOINT_FACE ;
|
|
|
|
|
|
static float BigX,BigY;
|
|
static s32 BigW,BigE,ALLPoints,Currentbase = 0,SOLVXmax,SOLVYmax;
|
|
#define MAX_DETECTIONS (4000)
|
|
static TRACKEDPOINT_FACE ALLXYW[MAX_DETECTIONS*3];
|
|
//#define WEIGHTED_COORDS
|
|
|
|
|
|
|
|
static void COPY_TOFLAT_II(MAP *MapSRC,MAP *MapDST)
|
|
{
|
|
s32 *LAST,*DST,*SRC;
|
|
DST = MapDST->BASE;
|
|
SRC = MapSRC->BASE;
|
|
LAST = DST + MapDST->SX;
|
|
while (DST < LAST)
|
|
{
|
|
*(DST++) = 0;
|
|
*(SRC++) &= 0xfffffffe;
|
|
}
|
|
SRC+=MapSRC->PITCH-MapSRC->SX;
|
|
DST+=MapDST->PITCH-MapDST->SX;
|
|
|
|
for (s32 SizeY = 1; SizeY < MapSRC->SY ;SizeY ++)
|
|
{
|
|
LAST = DST + MapDST->SX ;
|
|
|
|
*(DST++) = 0;
|
|
*(SRC++) &= 0xfffffffe;
|
|
|
|
while (DST < LAST)
|
|
{
|
|
*(DST) =((*(SRC) & 0xff0000) + ((*(SRC) & 0xff00)<<9) + ((*(SRC) & 0xff)<<16))>>(16 + 2);
|
|
*(DST) += *(DST-MapDST->PITCH);
|
|
*(DST) += *(DST-1) - *(DST-1-MapDST->PITCH);
|
|
*(DST++);
|
|
*(SRC++) &= 0xfffffffe;
|
|
}
|
|
SRC+=MapSRC->PITCH-MapDST->SX;
|
|
DST+=MapDST->PITCH-MapDST->SX;
|
|
}
|
|
}
|
|
|
|
static void COPY_TOFLAT_II_II2(MAP *MapSRC,MAP *MapDSTII,MAP *MapDSTII2)
|
|
{
|
|
s32 *LAST,*DST,*DST2,*SRC;
|
|
SRC = MapSRC->BASE;
|
|
DST = MapDSTII->BASE;
|
|
DST2= MapDSTII2->BASE;
|
|
LAST = DST + MapDSTII->SX;
|
|
while (DST < LAST)
|
|
{
|
|
*(DST++) = 0;
|
|
*(DST2++) = 0;
|
|
*(SRC++) &= 0xfffffffe;
|
|
}
|
|
SRC+=MapSRC->PITCH-MapSRC->SX;
|
|
DST+=MapDSTII->PITCH-MapDSTII->SX;
|
|
DST2+=MapDSTII2->PITCH-MapDSTII2->SX;
|
|
|
|
for (s32 SizeY = 1; SizeY < MapSRC->SY ;SizeY ++)
|
|
{
|
|
LAST = DST + MapDSTII->SX ;
|
|
|
|
*(DST++) = 0;
|
|
*(DST2++) = 0;
|
|
*(SRC++) &= 0xfffffffe;
|
|
|
|
while (DST < LAST)
|
|
{
|
|
*(DST) =((*(SRC) & 0xff0000) + ((*(SRC) & 0xff00)<<9) + ((*(SRC) & 0xff)<<16))>>(16 + 2);
|
|
*(DST2) = *(DST)**(DST);
|
|
|
|
*(DST) += *(DST-MapDSTII->PITCH);
|
|
*(DST) += *(DST-1) - *(DST-1-MapDSTII->PITCH);
|
|
|
|
*(DST2)+= *(DST2-MapDSTII2->PITCH);
|
|
*(DST2)+= *(DST2-1) - *(DST2-1-MapDSTII2->PITCH);
|
|
|
|
*(SRC++) &= 0xfffffffe;
|
|
*(DST++);
|
|
*(DST2++);
|
|
}
|
|
SRC+=MapSRC->PITCH -MapDSTII->SX;
|
|
DST+=MapDSTII->PITCH -MapDSTII->SX;
|
|
DST2+=MapDSTII2->PITCH -MapDSTII->SX;
|
|
}
|
|
}
|
|
|
|
void COPY_II_II2(MAP *MapSRC,MAP *MapDST,MAP *MapDST2)
|
|
{
|
|
s32 *LAST,*DST,*DST2,*SRC;
|
|
DST = MapDST->BASE;
|
|
DST2 = MapDST2->BASE;
|
|
SRC = MapSRC->BASE;
|
|
LAST = DST + MapDST->SX;
|
|
while (DST < LAST)
|
|
{
|
|
*(DST++) = 0;
|
|
*(DST2++) = 0;
|
|
*(SRC++) &= 0xfffffffe;
|
|
}
|
|
SRC+=MapSRC->PITCH-MapSRC->SX;
|
|
DST+=MapDST->PITCH-MapDST->SX;
|
|
DST2+=MapDST2->PITCH-MapDST2->SX;
|
|
|
|
for (s32 SizeY = 1; SizeY < MapSRC->SY ;SizeY ++)
|
|
{
|
|
LAST = DST + MapDST->SX ;
|
|
|
|
*(DST++) = 0;
|
|
*(DST2++) = 0;
|
|
*(SRC++) &= 0xfffffffe;
|
|
|
|
while (DST < LAST)
|
|
{
|
|
*(DST) =*(SRC);
|
|
*(DST2) = *(DST)**(DST);
|
|
|
|
*(DST) += *(DST-MapDST->PITCH);
|
|
*(DST) += *(DST-1) - *(DST-1-MapDST->PITCH);
|
|
|
|
*(DST2) += *(DST2-MapDST2->PITCH);
|
|
*(DST2) += *(DST2-1) - *(DST2-1-MapDST2->PITCH);
|
|
|
|
|
|
*(DST++);
|
|
*(DST2++);
|
|
*(SRC++) &= 0xfffffffe;
|
|
}
|
|
SRC+=MapSRC->PITCH-MapSRC->SX;
|
|
DST+=MapDST->PITCH-MapDST->SX;
|
|
DST2+=MapDST2->PITCH-MapDST2->SX;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
static void COPY_TOFLAT(MAP *MapSRC,MAP *MapDST)
|
|
{
|
|
s32 *LAST,*DST,*SRC;
|
|
DST = MapDST->BASE;
|
|
SRC = MapSRC->BASE;
|
|
for (s32 SizeY = 0; SizeY < MapSRC->SY ;SizeY ++)
|
|
{
|
|
LAST = DST + MapDST->SX ;
|
|
while (DST < LAST)
|
|
{
|
|
*(DST) =((*(SRC) & 0xff0000) + ((*(SRC) & 0xff00)<<9) + ((*(SRC) & 0xff)<<16))>>(16 + 2);
|
|
*(DST++);
|
|
*(SRC++);
|
|
}
|
|
SRC+=MapSRC->PITCH-MapDST->SX;
|
|
DST+=MapDST->PITCH-MapDST->SX;
|
|
}
|
|
}
|
|
|
|
void NORMALIZE_II(MAP *MapSRC,MAP *MapDST)
|
|
{
|
|
long long mid;
|
|
double deviation;
|
|
double deviationDouble;
|
|
int counterXY;
|
|
int result;
|
|
|
|
|
|
COPY_TOFLAT(MapSRC, MapDST);
|
|
|
|
s32 *LAST,*DST,*SRC;
|
|
DST = MapDST->BASE;
|
|
SRC = MapSRC->BASE;
|
|
|
|
mid = 0;
|
|
for (s32 SizeY = 0; SizeY < MapSRC->SY ;SizeY ++)
|
|
{
|
|
LAST = DST + MapDST->SX ;
|
|
while (DST < LAST)
|
|
{
|
|
mid += *(DST++);
|
|
}
|
|
DST+=MapDST->PITCH-MapDST->SX;
|
|
}
|
|
mid /= MapSRC->SY*MapDST->SX;
|
|
|
|
deviation = 0.0;
|
|
DST = MapDST->BASE;
|
|
SRC = MapSRC->BASE;
|
|
for (s32 SizeY = 0; SizeY < MapSRC->SY ;SizeY ++)
|
|
{
|
|
LAST = DST + MapDST->SX ;
|
|
while (DST < LAST)
|
|
{
|
|
deviation += (*(DST) - mid)*(*(DST) - mid);
|
|
DST++;
|
|
}
|
|
DST+=MapDST->PITCH-MapDST->SX;
|
|
}
|
|
deviationDouble = (double)deviation/(MapDST->SY*MapDST->PITCH);
|
|
deviation = (int)sqrt(deviationDouble);
|
|
|
|
if (deviation == 0)
|
|
return;
|
|
|
|
DST = MapDST->BASE;
|
|
SRC = MapSRC->BASE;
|
|
LAST = DST + MapDST->SX;
|
|
while (DST < LAST)
|
|
{
|
|
*(DST++) = 0;
|
|
}
|
|
DST+=MapDST->PITCH-MapDST->SX;
|
|
|
|
for (s32 SizeY = 0; SizeY < MapSRC->SY ;SizeY ++)
|
|
{
|
|
LAST = DST + MapDST->SX ;
|
|
while (DST < LAST)
|
|
{
|
|
*(DST) = 127 + (*(DST) - mid)*DEVIATION_BASE/deviation;
|
|
if (*(DST) > 255) *(DST) = 255;
|
|
if (*(DST) < 0) *(DST) = 0;
|
|
#ifdef MAKE_II
|
|
*(DST) += *(DST-MapDST->PITCH);
|
|
*(DST) += *(DST-1) - *(DST-1-MapDST->PITCH);
|
|
#endif
|
|
*(DST++);
|
|
}
|
|
DST+=MapDST->PITCH-MapDST->SX;
|
|
}
|
|
}
|
|
|
|
unsigned char NormalizationTable[256*256];
|
|
extern void SHIFT(MAP *MapDST,s32 SHIFT);
|
|
extern void SUB(MAP *MapSRC,MAP *MapDST);
|
|
|
|
void RETINEX(MAP *CurrentMap)
|
|
{
|
|
static s32 *WorkingBuf = NULL;
|
|
static u32 WorkingBufSize = 0;
|
|
|
|
MAP M1,M2,M3;
|
|
|
|
if (CurrentMap->SX * CurrentMap->SY == 0) return;
|
|
/************ Compute normalization table **************/
|
|
if (WorkingBufSize == 0)
|
|
{
|
|
for (int X=0 ; X < 256 ; X ++)
|
|
for (int Y=0 ; Y < 256 ; Y ++)
|
|
{
|
|
if(Y)
|
|
{
|
|
float Value;
|
|
Value =
|
|
NormalizationTable[(X<<8)+Y] = Y/* + 256.0f/(float)Y*/;
|
|
}
|
|
else
|
|
NormalizationTable[(X<<8)+Y] = 0;
|
|
}
|
|
}
|
|
|
|
/**************** Init working maps **************/
|
|
if (WorkingBufSize < CurrentMap->SX * CurrentMap->SY * 4 * 3)
|
|
{
|
|
if(WorkingBuf) CC_free(WorkingBuf);
|
|
WorkingBufSize = CurrentMap->SX * CurrentMap->SY;
|
|
WorkingBuf = (s32*)CC_malloc(WorkingBufSize * 4 * 3);
|
|
memset(WorkingBuf,0,WorkingBufSize * 4 * 3);
|
|
}
|
|
M1 = M2 = M3 = *CurrentMap;
|
|
M3.PITCH = M2.PITCH = M1.PITCH = CurrentMap->SX;
|
|
M1.BASE = WorkingBuf;
|
|
M2.BASE = WorkingBuf + CurrentMap->SX * CurrentMap->SY;
|
|
M3.BASE = WorkingBuf + CurrentMap->SX * CurrentMap->SY * 2;
|
|
|
|
/**************** compute average & average2 **************/
|
|
s32 Average;
|
|
COPY_BW(CurrentMap,CurrentMap);
|
|
SHIFT(CurrentMap,2);
|
|
LF_GaussianFilter(CurrentMap,&M1,&M2,20,1);
|
|
SUB(CurrentMap,&M1);
|
|
/*LF_GaussianFilter(&M1,&M2,&M3,8,1);*/
|
|
COPY(&M1,&M2);
|
|
ABS(&M2);
|
|
LF_GaussianFilter(&M2,&M2,&M3,20,1);
|
|
//SUB(CurrentMap,&M2);
|
|
//SUB(CurrentMap,&M2);
|
|
//COPY(CurrentMap,&M2);
|
|
//LF_GaussianFilter(&M2,&M2,&M3,8,1);
|
|
MAP MA;
|
|
MA = M2;
|
|
MA.BASE += MA.SX >> 2;
|
|
MA.BASE +=(MA.SY >> 2) * MA.PITCH;
|
|
MA.SX >>= 1;
|
|
MA.SY >>= 1;
|
|
Average = GET_AVERAGE(&MA)>>1;
|
|
|
|
if (Average < 2) Average = 2;
|
|
|
|
/**************** normalize **************/
|
|
|
|
{
|
|
s32 *LAST,*DST,*AveMap,*LocNoi;
|
|
AveMap = M1.BASE;
|
|
LocNoi = M2.BASE;
|
|
DST = CurrentMap->BASE;
|
|
for (u32 SizeY = 0; SizeY < CurrentMap->SY;SizeY ++)
|
|
{
|
|
LAST = DST + CurrentMap->SX;
|
|
while (DST < LAST)
|
|
{
|
|
if (*LocNoi < Average) *LocNoi = Average+1*(Average-*LocNoi);
|
|
*DST = 128+(((*AveMap*((64<<8) / *LocNoi)))>>10);//*/(s32)NormalizationTable[((128+*AveMap)<<8) + *LocNoi];
|
|
*DST |= (*DST<<8)|(*DST<<16);
|
|
DST++;
|
|
AveMap++;
|
|
LocNoi++;
|
|
}
|
|
AveMap += M1.PITCH-M1.SX;
|
|
LocNoi += M2.PITCH-M2.SX;
|
|
DST+=CurrentMap->PITCH-CurrentMap->SX;
|
|
}
|
|
}
|
|
}
|
|
|
|
void RETINEX_II(MAP *MapSRC,MAP *MapDST)
|
|
{
|
|
s32 *LAST,*DST;
|
|
|
|
COPY(MapSRC, MapDST);
|
|
RETINEX(MapDST);
|
|
|
|
NORMALIZE_II(MapDST, MapDST);
|
|
return;
|
|
|
|
DST = MapDST->BASE;
|
|
LAST = DST + MapDST->SX ;
|
|
while (DST < LAST)
|
|
{
|
|
*(DST++) = 0;
|
|
}
|
|
DST+=MapDST->PITCH-MapDST->SX;
|
|
|
|
for (s32 SizeY = 1; SizeY < MapSRC->SY ;SizeY ++)
|
|
{
|
|
LAST = DST + MapDST->SX ;
|
|
while (DST < LAST)
|
|
{
|
|
(*DST) = (*DST) & 255;
|
|
#ifdef MAKE_II
|
|
*(DST) += *(DST-MapDST->PITCH);
|
|
*(DST) += *(DST-1) - *(DST-1-MapDST->PITCH);
|
|
#endif
|
|
DST++;
|
|
}
|
|
DST+=MapDST->PITCH-MapDST->SX;
|
|
}
|
|
}
|
|
|
|
|
|
static int __cdecl TRACKEDPOINT_FACE_Compare(const void *elem1, const void *elem2 )
|
|
{
|
|
if (((TRACKEDPOINT_FACE *)elem2)->X < ((TRACKEDPOINT_FACE *)elem1)->X)
|
|
return -1;
|
|
else
|
|
return 1;
|
|
}
|
|
|
|
static void REPORT_COLORIMETRY(MAP *MapSRC,MAP *MapDST)
|
|
{
|
|
s32 *LAST,*DST,*SRC;
|
|
s32 RS,GS,BS,RD,GD,BD;
|
|
DST = MapDST->BASE; SRC = MapSRC->BASE;
|
|
RD = GD = BD = 0;
|
|
RS = GS = BS = 0;
|
|
for (s32 SizeY = 0; SizeY < MapSRC->SY;SizeY ++)
|
|
{
|
|
u32 Current;
|
|
LAST = DST + MapSRC->SX - (MapSRC->SX>>2);
|
|
Current = 0;
|
|
DST += MapSRC->SX>>2;
|
|
SRC += MapSRC->SX>>2;
|
|
while (DST < LAST)
|
|
{
|
|
RS += (*(SRC) >> 16 ) & 0xff;
|
|
GS += (*(SRC) >> 8 ) & 0xff;
|
|
BS += (*(SRC) >> 0 ) & 0xff;
|
|
RD += (*(DST) >> 16 ) & 0xff;
|
|
GD += (*(DST) >> 8 ) & 0xff;
|
|
BD += (*(DST) >> 0 ) & 0xff;
|
|
SRC++;
|
|
DST++;
|
|
}
|
|
DST += MapSRC->SX>>2;
|
|
SRC += MapSRC->SX>>2;
|
|
SRC+=MapSRC->PITCH-MapSRC->SX;
|
|
DST+=MapDST->PITCH-MapSRC->SX;
|
|
}
|
|
RS /= MapSRC->SY * MapSRC->SX / 2;
|
|
GS /= MapSRC->SY * MapSRC->SX / 2;
|
|
BS /= MapSRC->SY * MapSRC->SX / 2;
|
|
RD /= MapSRC->SY * MapSRC->SX / 2;
|
|
GD /= MapSRC->SY * MapSRC->SX / 2;
|
|
BD /= MapSRC->SY * MapSRC->SX / 2;
|
|
RD -= RS;
|
|
GD -= GS;
|
|
BD -= BS;
|
|
DST = MapDST->BASE; SRC = MapSRC->BASE;
|
|
for (s32 SizeY = 0; SizeY < MapSRC->SY;SizeY ++)
|
|
{
|
|
u32 Current;
|
|
LAST = DST + MapSRC->SX;
|
|
Current = 0;
|
|
while (DST < LAST)
|
|
{
|
|
RS = (*(DST) >> 16 ) & 0xff;
|
|
GS = (*(DST) >> 8 ) & 0xff;
|
|
BS = (*(DST) >> 0 ) & 0xff;
|
|
RS -= RD;
|
|
GS -= GD;
|
|
BS -= BD;
|
|
if (RS < 0) RS = 0;
|
|
if (GS < 0) GS = 0;
|
|
if (BS < 0) BS = 0;
|
|
if (RS > 255) RS = 255;
|
|
if (GS > 255) GS = 255;
|
|
if (BS > 255) BS = 255;
|
|
*DST = (RS<<16) + (GS<<8) + BS;
|
|
SRC++;
|
|
DST++;
|
|
}
|
|
SRC+=MapSRC->PITCH-MapSRC->SX;
|
|
DST+=MapDST->PITCH-MapSRC->SX;
|
|
}
|
|
|
|
}
|
|
|
|
/*u32 FeatureID_STACK[4096<<4];
|
|
u32 *FeatureID_STACK_Pos = NULL;*/
|
|
static inline double DETECT_Feature(s32 *Ptr,HAAR_Feature *pFeatures,u32 Num,u32 Pitch,double WindowSum)
|
|
{
|
|
double All;
|
|
HAAR_Feature *pFeaturesLast;
|
|
s32 *Ptr2;
|
|
All = 0;
|
|
pFeaturesLast = pFeatures + Num;
|
|
while (pFeatures < pFeaturesLast)
|
|
{
|
|
double Summ;
|
|
double BigSumm;
|
|
HAAR_RECT *pFeat;
|
|
pFeat = pFeatures->Feat;
|
|
#ifdef OPTIMIZE
|
|
Ptr2 = Ptr+pFeat->Offset0;
|
|
Summ = (double)*(Ptr2) - (double)*(Ptr2+pFeat->Offset1) - (double)*(Ptr2+pFeat->Offset2) + (double)*(Ptr2+pFeat->Offset1+pFeat->Offset2);
|
|
BigSumm = Summ * (double)pFeat->W;
|
|
pFeat++;
|
|
Ptr2 = Ptr+pFeat->Offset0;
|
|
Summ = (double)*(Ptr2) - (double)*(Ptr2+pFeat->Offset1) - (double)*(Ptr2+pFeat->Offset2) + (double)*(Ptr2+pFeat->Offset1+pFeat->Offset2);
|
|
BigSumm += Summ * (double)pFeat->W;
|
|
pFeat++;
|
|
if (pFeat->W)
|
|
{
|
|
Ptr2 = Ptr+pFeat->Offset0;
|
|
Summ = (double)*(Ptr2) - (double)*(Ptr2+pFeat->Offset1) - (double)*(Ptr2+pFeat->Offset2) + (double)*(Ptr2+pFeat->Offset1+pFeat->Offset2);
|
|
BigSumm += Summ * (double)pFeat->W;
|
|
}
|
|
BigSumm -= (double)pFeatures->Thresh * (double)WindowSum;
|
|
//*FeatureID_STACK_Pos = ((*((u32 *)&BigSumm + 1))>>31)^1;
|
|
All += pFeatures->LeftRight[((*((u32 *)&BigSumm + 1))>>31)^1];
|
|
pFeatures++;
|
|
#else
|
|
Summ = (double)*(Ptr + pFeat->Max.X + pFeat->Max.Y * Pitch);
|
|
Summ -= (double)*(Ptr + pFeat->Max.X + pFeat->Min.Y * Pitch );
|
|
Summ -= (double)*(Ptr + pFeat->Min.X + pFeat->Max.Y * Pitch);
|
|
Summ += (double)*(Ptr + pFeat->Min.X + pFeat->Min.Y * Pitch );
|
|
BigSumm = ((double)Summ) * (double)pFeat->W;
|
|
pFeat++;
|
|
|
|
Summ = (double)*(Ptr + pFeat->Max.X + pFeat->Max.Y * Pitch);
|
|
Summ -= (double)*(Ptr + pFeat->Max.X + pFeat->Min.Y * Pitch );
|
|
Summ -= (double)*(Ptr + pFeat->Min.X + pFeat->Max.Y * Pitch);
|
|
Summ += (double)*(Ptr + pFeat->Min.X + pFeat->Min.Y * Pitch );
|
|
BigSumm += Summ * (double)pFeat->W;
|
|
pFeat++;
|
|
if (pFeat->W)
|
|
{
|
|
Summ = (double)*(Ptr + pFeat->Max.X + pFeat->Max.Y * Pitch);
|
|
Summ -= (double)*(Ptr + pFeat->Max.X + pFeat->Min.Y * Pitch);
|
|
Summ -= (double)*(Ptr + pFeat->Min.X + pFeat->Max.Y * Pitch);
|
|
Summ += (double)*(Ptr + pFeat->Min.X + pFeat->Min.Y * Pitch);
|
|
BigSumm += ((double)Summ) * (double)pFeat->W;
|
|
}
|
|
if (BigSumm > pFeatures->Thresh * WindowSum)
|
|
All += pFeatures->LeftRight[1];
|
|
else
|
|
All += pFeatures->LeftRight[0];
|
|
pFeatures++;
|
|
#endif
|
|
}
|
|
|
|
return All;
|
|
}
|
|
|
|
static inline double DETECT_Feature_OptimizedOnly(s32 *Ptr,HAAR_Feature *pFeatures,u32 Num,u32 Pitch, double WindowSum, double WindowDeviationOpt /*DEVIATION_BASE / WindowDeviation*/, u32 totalElmts)
|
|
{
|
|
double All;
|
|
u32 localZone;
|
|
HAAR_Feature *pFeaturesLast;
|
|
s32 *Ptr2;
|
|
All = 0;
|
|
pFeaturesLast = pFeatures + Num;
|
|
while (pFeatures < pFeaturesLast)
|
|
{
|
|
double Summ;
|
|
double BigSumm;
|
|
HAAR_RECT *pFeat;
|
|
pFeat = pFeatures->Feat;
|
|
localZone = pFeat->Offset1*pFeat->Offset2/Pitch;
|
|
Ptr2 = Ptr+pFeat->Offset0;
|
|
Summ = (double)*(Ptr2) - (double)*(Ptr2+pFeat->Offset1) - (double)*(Ptr2+pFeat->Offset2) + (double)*(Ptr2+pFeat->Offset1+pFeat->Offset2);
|
|
Summ = (Summ - localZone*WindowSum/totalElmts)* WindowDeviationOpt + localZone*128;
|
|
BigSumm = Summ * (double)pFeat->W;
|
|
pFeat++;
|
|
localZone = pFeat->Offset1*pFeat->Offset2/Pitch;
|
|
Ptr2 = Ptr+pFeat->Offset0;
|
|
Summ = (double)*(Ptr2) - (double)*(Ptr2+pFeat->Offset1) - (double)*(Ptr2+pFeat->Offset2) + (double)*(Ptr2+pFeat->Offset1+pFeat->Offset2);
|
|
Summ = (Summ - localZone*WindowSum/totalElmts)* WindowDeviationOpt + localZone*128;
|
|
BigSumm += Summ * (double)pFeat->W;
|
|
pFeat++;
|
|
if (pFeat->W)
|
|
{
|
|
localZone = pFeat->Offset1*pFeat->Offset2/Pitch;
|
|
Ptr2 = Ptr+pFeat->Offset0;
|
|
Summ = (double)*(Ptr2) - (double)*(Ptr2+pFeat->Offset1) - (double)*(Ptr2+pFeat->Offset2) + (double)*(Ptr2+pFeat->Offset1+pFeat->Offset2);
|
|
Summ = (Summ - localZone*WindowSum/totalElmts)* WindowDeviationOpt + localZone*128;
|
|
BigSumm += Summ * (double)pFeat->W;
|
|
}
|
|
BigSumm -= (double)pFeatures->Thresh * (double)WindowSum;
|
|
//*FeatureID_STACK_Pos = ((*((u32 *)&BigSumm + 1))>>31)^1;
|
|
All += pFeatures->LeftRight[((*((u32 *)&BigSumm + 1))>>31)^1];
|
|
pFeatures++;
|
|
}
|
|
|
|
return All;
|
|
}
|
|
|
|
static u32 DETECT(s32 *Ptr,HAAR_Classifier *p_HAAR,u32 Pitch,double WindowSumm )
|
|
{
|
|
u32 REturnV,Dept;
|
|
HAAR_Stage *p_Current;
|
|
p_Current = p_HAAR->p_Stages;
|
|
REturnV = 0;
|
|
Dept = 0;
|
|
// FeatureID_STACK_Pos = &FeatureID_STACK[0];
|
|
while (p_Current)
|
|
{
|
|
#ifdef OPTIMIZE
|
|
HAAR_SCALAR Res;
|
|
Res = (HAAR_SCALAR)DETECT_Feature(Ptr,p_Current->pStageFeatures,p_Current->ulNumberOfFeatures,Pitch,WindowSumm) - p_Current->StageThresh;
|
|
REturnV = (*(u32*)&Res)>>31;
|
|
//*(FeatureID_STACK_Pos++) = REturnV<<8;
|
|
p_Current = p_Current->pNext[REturnV];
|
|
#else
|
|
HAAR_SCALAR Res;
|
|
if (DETECT_Feature(Ptr,p_Current->pStageFeatures,p_Current->ulNumberOfFeatures,Pitch,WindowSumm) >
|
|
p_Current->StageThresh )
|
|
{
|
|
REturnV = 0;
|
|
p_Current = p_Current->pNext[0];
|
|
} else
|
|
{
|
|
REturnV = 1;
|
|
p_Current = p_Current->pNext[1];
|
|
}
|
|
#endif
|
|
}
|
|
return REturnV^1;
|
|
}
|
|
|
|
static u32 DETECT_OptimizedOnly(s32 *Ptr,HAAR_Classifier *p_HAAR,u32 Pitch, double WindowSumm, double WindowDeviationOpt, u32 totalElmts)
|
|
{
|
|
u32 REturnV,Dept;
|
|
HAAR_Stage *p_Current;
|
|
p_Current = p_HAAR->p_Stages;
|
|
REturnV = 0;
|
|
Dept = 0;
|
|
// FeatureID_STACK_Pos = &FeatureID_STACK[0];
|
|
while (p_Current)
|
|
{
|
|
HAAR_SCALAR Res;
|
|
Res = (HAAR_SCALAR)DETECT_Feature_OptimizedOnly(Ptr,p_Current->pStageFeatures,p_Current->ulNumberOfFeatures, Pitch,WindowSumm, WindowDeviationOpt, totalElmts) - p_Current->StageThresh;
|
|
REturnV = (*(u32*)&Res)>>31;
|
|
//*(FeatureID_STACK_Pos++) = REturnV<<8;
|
|
p_Current = p_Current->pNext[REturnV];
|
|
}
|
|
return REturnV^1;
|
|
}
|
|
|
|
static void TAGADDR(s32 *DST,u32 Value)
|
|
{
|
|
Value |= 0x00010000;
|
|
if (*DST & 1)
|
|
{
|
|
*DST >>= 1;
|
|
*DST += Value ;
|
|
*DST <<= 1;
|
|
*DST |= 1;
|
|
} else *DST = (Value<<1)|1;
|
|
|
|
|
|
}
|
|
|
|
static void HAAR_II_DO(MAP *MapSRC,MAP *MapDST,HAAR_Classifier *p_HAAR, u32 V_STEP, u32 H_STEP , u32 Value2Tag, u32 Po2)
|
|
{
|
|
s32 *BIGLAST,*LAST,*DST,*SRC,X,Offset,SizeY ;
|
|
DST = MapDST->BASE;
|
|
SRC = MapSRC->BASE;
|
|
u32 p1,p2;
|
|
p1 = p_HAAR->WindowSize.X ;
|
|
p2 = MapSRC->PITCH*(p_HAAR->WindowSize.Y );
|
|
Offset = (p_HAAR->WindowSize.X >> 1) + ((p_HAAR->WindowSize.Y>>1) * MapDST->PITCH);
|
|
BIGLAST = DST + (MapDST->SY) * MapDST->PITCH;
|
|
SizeY = 0;
|
|
if (!V_STEP) V_STEP = 1;
|
|
if (!H_STEP) H_STEP = 1;
|
|
Offset -= (H_STEP >> 1) + ((V_STEP>>1) * MapDST->PITCH);
|
|
while (DST < BIGLAST)
|
|
{
|
|
LAST = DST + MapDST->SX;
|
|
X = 0;
|
|
while (DST < LAST)
|
|
{
|
|
double WindowSumm ;
|
|
WindowSumm = (double)*(SRC) - (double)*(SRC + p1) - (double)*(SRC + p2) + (double)*(SRC + p1 + p2);
|
|
WindowSumm*= WINDOW_SUM_FACTOR_II;
|
|
if (DETECT(SRC,p_HAAR,MapSRC->PITCH,WindowSumm))
|
|
{
|
|
if (ALLPoints < MAX_DETECTIONS)
|
|
{
|
|
memset(&ALLXYW[ALLPoints],0,sizeof(TRACKEDPOINT_FACE));
|
|
ALLXYW[ALLPoints].X = (float)(X<<(Po2+8));
|
|
ALLXYW[ALLPoints].Y = (float)(SizeY<<(Po2+8));
|
|
ALLXYW[ALLPoints].W = 1.0f;
|
|
ALLXYW[ALLPoints].Scale = (float)(Value2Tag<<(8+Po2));
|
|
ALLXYW[ALLPoints].Alpha = 0.0f;
|
|
ALLXYW[ALLPoints].Age = 1;
|
|
ALLXYW[ALLPoints].ScaleV = 1.0f;
|
|
ALLPoints++;
|
|
*DST = 0xffffff;
|
|
}
|
|
};
|
|
X+=H_STEP;
|
|
DST+=H_STEP;
|
|
SRC+=H_STEP;//*/
|
|
}
|
|
SizeY += V_STEP;
|
|
SRC+=V_STEP*MapSRC->PITCH-MapSRC->SX;
|
|
DST+=V_STEP*MapDST->PITCH-MapDST->SX;
|
|
}
|
|
}
|
|
|
|
static void HAAR_II_II2_DO(MAP *MapSRC,MAP *MapSRC2,MAP *MapDST,HAAR_Classifier *p_HAAR, u32 V_STEP, u32 H_STEP , u32 Value2Tag, u32 Po2)
|
|
{
|
|
s32 *BIGLAST,*LAST,*DST,*SRC,*SRC2,X,Offset,SizeY ;
|
|
DST = MapDST->BASE;
|
|
SRC = MapSRC->BASE;
|
|
SRC2 = MapSRC2->BASE;
|
|
u32 p1,p2,zoneLen;
|
|
p1 = p_HAAR->WindowSize.X ;
|
|
p2 = MapSRC->PITCH*(p_HAAR->WindowSize.Y );
|
|
zoneLen = p_HAAR->WindowSize.X*p_HAAR->WindowSize.Y;
|
|
Offset = (p_HAAR->WindowSize.X >> 1) + ((p_HAAR->WindowSize.Y>>1) * MapDST->PITCH);
|
|
BIGLAST = DST + (MapDST->SY) * MapDST->PITCH;
|
|
SizeY = 0;
|
|
if (!V_STEP) V_STEP = 1;
|
|
if (!H_STEP) H_STEP = 1;
|
|
Offset -= (H_STEP >> 1) + ((V_STEP>>1) * MapDST->PITCH);
|
|
while (DST < BIGLAST)
|
|
{
|
|
LAST = DST + MapDST->SX;
|
|
X = 0;
|
|
while (DST < LAST)
|
|
{
|
|
double WindowSumm;
|
|
double WindowAverage;
|
|
double WindowDeviation;
|
|
|
|
WindowSumm = (double)*(SRC) - (double)*(SRC + p1) - (double)*(SRC + p2) + (double)*(SRC + p1 + p2);
|
|
WindowAverage = WindowSumm / zoneLen;
|
|
WindowDeviation = ((double)*(SRC2) - (double)*(SRC2 + p1) - (double)*(SRC2 + p2) + (double)*(SRC2 + p1 + p2)) -
|
|
2*WindowAverage*WindowSumm;
|
|
WindowDeviation = WindowDeviation/zoneLen + WindowAverage*WindowAverage;
|
|
|
|
if (WindowDeviation > 0.0f)
|
|
{
|
|
WindowDeviation = DEVIATION_BASE/sqrt(WindowDeviation);
|
|
|
|
WindowSumm*=WINDOW_SUM_FACTOR_II2;
|
|
if (DETECT_OptimizedOnly(SRC,p_HAAR,MapSRC->PITCH,WindowSumm, WindowDeviation, zoneLen))
|
|
{
|
|
if (ALLPoints < MAX_DETECTIONS)
|
|
{
|
|
memset(&ALLXYW[ALLPoints],0,sizeof(TRACKEDPOINT_FACE));
|
|
ALLXYW[ALLPoints].X = (float)(X<<(Po2+8));
|
|
ALLXYW[ALLPoints].Y = (float)(SizeY<<(Po2+8));
|
|
ALLXYW[ALLPoints].W = 1.0f;
|
|
ALLXYW[ALLPoints].Scale = (float)(Value2Tag<<(8+Po2));
|
|
ALLXYW[ALLPoints].Alpha = 0.0f;
|
|
ALLXYW[ALLPoints].Age = 1;
|
|
ALLXYW[ALLPoints].ScaleV = 1.0f;
|
|
ALLPoints++;
|
|
*DST = 0xffffff;
|
|
}
|
|
};
|
|
}
|
|
X+=H_STEP;
|
|
DST+=H_STEP;
|
|
SRC+=H_STEP;//*/
|
|
SRC2+=H_STEP;//*/
|
|
}
|
|
SizeY += V_STEP;
|
|
SRC+=V_STEP*MapSRC->PITCH-MapSRC->SX;
|
|
SRC2+=V_STEP*MapSRC2->PITCH-MapSRC2->SX;
|
|
DST+=V_STEP*MapDST->PITCH-MapDST->SX;
|
|
}
|
|
}
|
|
|
|
static void FIND_LOWER(MAP *MapDST,u32 Radius,s32 *XRes,s32 *YRes)
|
|
{
|
|
s32 *BIGLAST,*LAST,*DST,X,Y ;
|
|
double Mini;
|
|
Mini = 256.0f*256.0f*256.0f*256.0f*256.0f;
|
|
DST = MapDST->BASE;
|
|
u32 p1,p2;
|
|
p1 = Radius ;
|
|
p2 = MapDST->PITCH*(Radius);
|
|
BIGLAST = DST + (MapDST->SY) * MapDST->PITCH;
|
|
Y = 0;
|
|
while (DST < BIGLAST)
|
|
{
|
|
LAST = DST + MapDST->SX;
|
|
X = 0;
|
|
while (DST < LAST)
|
|
{
|
|
double WindowSumm ;
|
|
WindowSumm = (double)*(DST) - (double)*(DST + p1) - (double)*(DST + p2) + (double)*(DST + p1 + p2);
|
|
if (WindowSumm < Mini)
|
|
{
|
|
*XRes = X + (Radius>>1);
|
|
*YRes = Y + (Radius>>1);
|
|
Mini = WindowSumm;
|
|
};
|
|
X++;
|
|
DST++;
|
|
}
|
|
Y ++;
|
|
DST+=MapDST->PITCH-MapDST->SX;
|
|
}
|
|
}
|
|
|
|
static void HAAR_II(MAP *MapSRC,MAP *MapDST,HAAR_Classifier *p_HAAR,s32 XMIN,s32 YMIN,s32 XMAX,s32 YMAX,u32 Po2,u32 V_STEP, u32 H_STEP, u32 Value2Tag)
|
|
{
|
|
MAP SaveMapSRC,SaveMapDST;
|
|
SaveMapSRC = *MapSRC;
|
|
SaveMapDST = *MapDST;
|
|
|
|
//DrawQuad(MapDSTNeg,0,0,p_HAAR->WindowSize.X<<Po2,p_HAAR->WindowSize.Y<<Po2,0xff0000,1);
|
|
XMAX >>= Po2;
|
|
YMAX >>= Po2;
|
|
XMIN >>= Po2;
|
|
YMIN >>= Po2;
|
|
|
|
XMAX += p_HAAR->WindowSize.X >>1;
|
|
YMAX += p_HAAR->WindowSize.Y >>1;
|
|
XMIN -= p_HAAR->WindowSize.X >>1;
|
|
YMIN -= p_HAAR->WindowSize.Y >>1;
|
|
|
|
|
|
if (XMIN < 0) XMIN = 0;
|
|
if (YMIN < 0) YMIN = 0;
|
|
|
|
if (XMAX > MapSRC->SX) XMAX = MapSRC->SX ;
|
|
if (YMAX > MapSRC->SY) YMAX = MapSRC->SY ;
|
|
|
|
//DrawQuad(MapDSTNeg,XMIN,YMIN,XMAX,YMAX,0xff00,2);
|
|
|
|
XMAX -= p_HAAR->WindowSize.X;
|
|
YMAX -= p_HAAR->WindowSize.Y;
|
|
|
|
|
|
TRACKEDPOINT_FACE *p_Last;
|
|
|
|
p_Last = &ALLXYW[ALLPoints];
|
|
SaveMapSRC.BASE += XMIN + YMIN * SaveMapSRC.PITCH;
|
|
SaveMapDST.BASE += XMIN + YMIN * SaveMapDST.PITCH;
|
|
SaveMapSRC.SX = SaveMapDST.SX = XMAX - XMIN;
|
|
SaveMapSRC.SY = SaveMapDST.SY = YMAX - YMIN;
|
|
HAAR_II_DO(&SaveMapSRC,&SaveMapDST,p_HAAR,V_STEP,H_STEP,Value2Tag,Po2);
|
|
while (p_Last < &ALLXYW[ALLPoints])
|
|
{
|
|
p_Last->X += (XMIN + (p_HAAR->WindowSize.X>>1))<<(8 + Po2);
|
|
p_Last->Y += (YMIN + (p_HAAR->WindowSize.Y>>1))<<(8 + Po2);
|
|
p_Last++;
|
|
}
|
|
}
|
|
|
|
static void HAAR_II_II2(MAP *MapSRC,MAP *MapSRC2,MAP *MapDST,HAAR_Classifier *p_HAAR,s32 XMIN,s32 YMIN,s32 XMAX,s32 YMAX,u32 Po2,u32 V_STEP, u32 H_STEP, u32 Value2Tag)
|
|
{
|
|
MAP SaveMapSRC,SaveMapSRC2,SaveMapDST;
|
|
SaveMapSRC = *MapSRC;
|
|
SaveMapSRC2 = *MapSRC2;
|
|
SaveMapDST = *MapDST;
|
|
|
|
//DrawQuad(MapDSTNeg,0,0,p_HAAR->WindowSize.X<<Po2,p_HAAR->WindowSize.Y<<Po2,0xff0000,1);
|
|
XMAX >>= Po2;
|
|
YMAX >>= Po2;
|
|
XMIN >>= Po2;
|
|
YMIN >>= Po2;
|
|
|
|
XMAX += p_HAAR->WindowSize.X >>1;
|
|
YMAX += p_HAAR->WindowSize.Y >>1;
|
|
XMIN -= p_HAAR->WindowSize.X >>1;
|
|
YMIN -= p_HAAR->WindowSize.Y >>1;
|
|
|
|
|
|
if (XMIN < 0) XMIN = 0;
|
|
if (YMIN < 0) YMIN = 0;
|
|
|
|
if (XMAX > MapSRC->SX) XMAX = MapSRC->SX ;
|
|
if (YMAX > MapSRC->SY) YMAX = MapSRC->SY ;
|
|
|
|
//DrawQuad(MapDSTNeg,XMIN,YMIN,XMAX,YMAX,0xff00,2);
|
|
|
|
XMAX -= p_HAAR->WindowSize.X;
|
|
YMAX -= p_HAAR->WindowSize.Y;
|
|
|
|
|
|
TRACKEDPOINT_FACE *p_Last;
|
|
|
|
p_Last = &ALLXYW[ALLPoints];
|
|
SaveMapSRC.BASE += XMIN + YMIN * SaveMapSRC.PITCH;
|
|
SaveMapSRC2.BASE+= XMIN + YMIN * SaveMapSRC2.PITCH;
|
|
SaveMapDST.BASE += XMIN + YMIN * SaveMapDST.PITCH;
|
|
SaveMapSRC.SX = SaveMapSRC2.SX = SaveMapDST.SX = XMAX - XMIN;
|
|
SaveMapSRC.SY = SaveMapSRC2.SY = SaveMapDST.SY = YMAX - YMIN;
|
|
HAAR_II_II2_DO(&SaveMapSRC,&SaveMapSRC2,&SaveMapDST,p_HAAR,V_STEP,H_STEP,Value2Tag,Po2);
|
|
while (p_Last < &ALLXYW[ALLPoints])
|
|
{
|
|
p_Last->X += (XMIN + (p_HAAR->WindowSize.X>>1))<<(8 + Po2);
|
|
p_Last->Y += (YMIN + (p_HAAR->WindowSize.Y>>1))<<(8 + Po2);
|
|
p_Last++;
|
|
}
|
|
}
|
|
|
|
|
|
static s32 AAAA = 0;
|
|
static void COPY_PATCH_P(MAP *MapSRC,MAP *MapDST,s32 SX,s32 SY,s32 DX,s32 DY,u32 SizeX,u32 SizeY,u32 DSizeX,u32 DSizeY,float Rot128,float SCLAEV)
|
|
{
|
|
s32 *LAST,*DST,*DST2,*SRC,*BIGLAST;
|
|
s32 SYS,SYS2,SXS2,SXS3,SXI3,SYS3,SYI3,SXI,SYI,SYI2,SXI2;
|
|
s32 ScaleROTTable[1024],*p_ScaleROTTable;
|
|
s32 ScaleROTTableLenght[1024],*p_ScaleROTTableLenght;
|
|
float CosA,SinA;
|
|
|
|
CosA = cosf(Rot128);
|
|
SinA = sinf(Rot128);
|
|
|
|
//AAAA+= 1<<13 ;
|
|
SX -= (int)((CosA * (DSizeX>>1)) - (SinA * (DSizeY>>1))) ;
|
|
SY -= (int)((SinA * (DSizeX>>1)) + (CosA * (DSizeY>>1))) ;
|
|
SX <<= 8;
|
|
SY <<= 8;
|
|
|
|
SYS = 0;
|
|
SXI = (int)(65536.0f * ((float)DSizeX / (float)SizeX ));
|
|
SYI = (int)(65536.0f * ((float)DSizeY / (float)SizeY ));
|
|
|
|
/* Compute Scale X */
|
|
SXI3 = -(int)((float)SXI * SinA * SCLAEV);
|
|
SYI3 = (int)((float)SXI * CosA * SCLAEV);
|
|
SXS3 = SX;
|
|
SYS3 = SY;
|
|
|
|
/* Compute raster X */
|
|
|
|
SXI2 = (int)((float)SXI * CosA);
|
|
SYI2 = (int)((float)SXI * SinA);
|
|
SXS2 = 0;
|
|
SYS2 = 0;
|
|
|
|
/* Rasterize */
|
|
DX -= SizeX>>1;
|
|
DY -= SizeY>>1;
|
|
DX>>=8;DY>>=8;
|
|
if (DX < 0 ) DX = 0;
|
|
if (DY < 0 ) DY = 0;
|
|
|
|
p_ScaleROTTable = (s32 *)ScaleROTTable;
|
|
p_ScaleROTTableLenght = (s32*)ScaleROTTableLenght;
|
|
DST = MapDST->BASE + DY * MapDST->PITCH + DX;
|
|
BIGLAST = MapDST->BASE + MapDST->SY * MapDST->PITCH;
|
|
SizeX>>=8;SizeY>>=8;
|
|
while (SizeY--)
|
|
{
|
|
DST2 = DST;
|
|
SRC = MapSRC->BASE;
|
|
LAST = DST + SizeX;
|
|
SXS2 = SXS3 + SXI2 * SizeX;
|
|
SYS2 = SYS3 + SYI2 * SizeX;
|
|
while (((SXS2>>16 >= MapSRC->SX-1) || (SXS2>>16 < 1) ||
|
|
(SYS2>>16 >= MapSRC->SY-1) || (SYS2>>16 < 1)) && (DST < LAST))
|
|
{
|
|
SXS2 -= SXI2;
|
|
SYS2 -= SYI2;
|
|
LAST--;
|
|
}
|
|
SXS2 = SXS3;
|
|
SYS2 = SYS3;
|
|
while (((SXS2>>16 >= MapSRC->SX-1) || (SXS2>>16 < 1) ||
|
|
(SYS2>>16 >= MapSRC->SY-1) || (SYS2>>16 < 1)) && (DST < LAST))
|
|
{
|
|
SXS2 += SXI2;
|
|
SYS2 += SYI2;
|
|
DST++;
|
|
}
|
|
|
|
while (DST < LAST)
|
|
{
|
|
//if ((DST > MapDST->BASE) && (DST < BIGLAST))
|
|
*(DST) = (*(SRC+(SXS2>>16) + (SYS2>>16)*MapSRC->PITCH)) & 0xfffffffe;
|
|
SXS2 += SXI2;
|
|
SYS2 += SYI2;
|
|
DST++;
|
|
}
|
|
SXS3 += SXI3;
|
|
SYS3 += SYI3;
|
|
DST = DST2 + MapDST->PITCH;
|
|
}
|
|
}
|
|
|
|
static void COPY_PATCH_P_MipMap(MAP *MapSRC,MAP *MapDST,s32 SX,s32 SY,s32 DX,s32 DY,u32 SizeX,u32 SizeY,u32 DSizeX,u32 DSizeY,float Rot128,float SCLAEV)
|
|
{
|
|
while (SizeX<<1 < DSizeX)
|
|
{
|
|
MapSRC++;
|
|
DSizeX>>= 1;
|
|
DSizeY>>= 1;
|
|
SX>>=1;
|
|
SY>>=1;
|
|
}
|
|
COPY_PATCH_P(MapSRC,MapDST,SX,SY,DX,DY,SizeX,SizeY,DSizeX,DSizeY,Rot128,SCLAEV);
|
|
}
|
|
|
|
void COPY_PATCH(MAP *MapSRC,MAP *MapDST,s32 SX,s32 SY,s32 DX,s32 DY,u32 SizeX,u32 SizeY,u32 DSizeX,u32 DSizeY)
|
|
{
|
|
COPY_PATCH_P(MapSRC,MapDST,SX<<8,SY<<8,DX<<8,DY<<8,SizeX<<8,SizeY<<8,DSizeX<<8,DSizeY<<8,0.0f,1.0f);
|
|
}
|
|
extern "C" {
|
|
void COPY_PATCH_FROM_JADE(u32 *SRCPix,u32 *DSTPix,
|
|
u32 SRCPitch,u32 DSTPitch,
|
|
u32 SRC_SX,u32 DST_SX,
|
|
u32 SRC_SY,u32 DST_SY)
|
|
{
|
|
MAP MapSRC,MapDST;
|
|
u32 *Parser,*ParserL,NDST_SY ;
|
|
NDST_SY = (DST_SX * SRC_SY) / SRC_SX;
|
|
DSTPix += DSTPitch * (DST_SY - NDST_SY) / 2;
|
|
DST_SY = NDST_SY;
|
|
|
|
MapSRC.BASE = (s32 *)SRCPix;
|
|
MapSRC.PITCH = SRCPitch;
|
|
MapSRC.SX = SRC_SX;
|
|
MapSRC.SY = SRC_SY;
|
|
MapDST.BASE = (s32 *)DSTPix;
|
|
MapDST.PITCH = DSTPitch;
|
|
MapDST.SX = DST_SX;
|
|
MapDST.SY = DST_SY;
|
|
COPY_PATCH_P(&MapSRC,&MapDST,SRC_SX<<7,SRC_SY<<7,DST_SX<<7,DST_SY<<7,DST_SX<<8,DST_SY<<8,SRC_SX<<8,SRC_SY<<8,0.0f,1.0f);
|
|
|
|
/* Swap red and Blue */
|
|
Parser = DSTPix;
|
|
ParserL = Parser + DST_SX * DST_SY;
|
|
while (Parser < ParserL)
|
|
{
|
|
*(Parser++) = ((*Parser & 0xff00) | ((*Parser>>16) & 0xff) | ((*Parser<<16) & 0xff0000)) | 0xff000000;
|
|
*(Parser++) = ((*Parser & 0xff00) | ((*Parser>>16) & 0xff) | ((*Parser<<16) & 0xff0000)) | 0xff000000;
|
|
*(Parser++) = ((*Parser & 0xff00) | ((*Parser>>16) & 0xff) | ((*Parser<<16) & 0xff0000)) | 0xff000000;
|
|
*(Parser++) = ((*Parser & 0xff00) | ((*Parser>>16) & 0xff) | ((*Parser<<16) & 0xff0000)) | 0xff000000;
|
|
}
|
|
}
|
|
};
|
|
|
|
static void COPY_O2_II(MAP *MapSRC,MAP *MapDST)
|
|
{
|
|
s32 *LAST,*DST,*SRC,*SRC2;
|
|
DST = MapDST->BASE;
|
|
SRC = MapSRC->BASE;
|
|
for (s32 SizeY = 0; SizeY < MapDST->SY;SizeY ++)
|
|
{
|
|
LAST = DST + MapDST->SX;
|
|
SRC2 = SRC + MapSRC->PITCH;
|
|
while (DST < LAST)
|
|
{
|
|
*(DST++) = (*(SRC) + *(SRC + 1) + *(SRC2) + *(SRC2 + 1))>>(2+2); // +2 -> II
|
|
SRC += 2;
|
|
SRC2 += 2;
|
|
}
|
|
*(DST) = *(DST-1);
|
|
SRC+=(MapSRC->PITCH-MapSRC->SX);
|
|
SRC+= MapSRC->PITCH;
|
|
DST+=MapDST->PITCH-MapDST->SX;
|
|
}
|
|
LAST = DST + MapDST->SX;
|
|
while (DST < LAST)
|
|
{
|
|
*(DST) = *(DST-MapDST->PITCH);
|
|
DST++;
|
|
}
|
|
*(DST) = *(DST-1);
|
|
}
|
|
|
|
static void COPY_O2(MAP *MapSRC,MAP *MapDST)
|
|
{
|
|
s32 *LAST,*DST,*SRC,*SRC2;
|
|
DST = MapDST->BASE;
|
|
SRC = MapSRC->BASE;
|
|
for (s32 SizeY = 0; SizeY < MapDST->SY;SizeY ++)
|
|
{
|
|
LAST = DST + MapDST->SX;
|
|
SRC2 = SRC + MapSRC->PITCH;
|
|
while (DST < LAST)
|
|
{
|
|
*(DST++) = (*(SRC) + *(SRC + 1) + *(SRC2) + *(SRC2 + 1))>>(2); // +2 -> II
|
|
SRC += 2;
|
|
SRC2 += 2;
|
|
}
|
|
SRC+=(MapSRC->PITCH-MapSRC->SX);
|
|
SRC+= MapSRC->PITCH;
|
|
DST+=MapDST->PITCH-MapDST->SX;
|
|
}
|
|
}
|
|
|
|
#define MAX_FACES 8 // PO2 !!!
|
|
#define HISTORY 40
|
|
static TRACKEDPOINT_FACE FACES[MAX_FACES * HISTORY];
|
|
static u32 FACESNUM = 0;
|
|
|
|
static void MERGE_OVERLAPPED_POINTS()
|
|
{
|
|
/* Merge Overlapped detections */
|
|
qsort( &ALLXYW[0], ALLPoints, sizeof(TRACKEDPOINT_FACE), &TRACKEDPOINT_FACE_Compare);//*/
|
|
TRACKEDPOINT_FACE *p_Current,*p_Last,*p2Comp;
|
|
p_Current = &ALLXYW[0];
|
|
p_Last = p_Current + ALLPoints;
|
|
while (p_Current < p_Last - 1)
|
|
{
|
|
if (p_Current->Scale) // Skip merged
|
|
{
|
|
p2Comp = p_Current + 1;
|
|
while (p2Comp < p_Last)
|
|
{
|
|
if (((p2Comp->X - (p2Comp->Scale*0.5f)) < p_Current->X) && ((p2Comp->X + (p2Comp->Scale*0.5)) > p_Current->X))
|
|
{
|
|
if (((p2Comp->Y - (p2Comp->Scale*0.5f)) < p_Current->Y) && ((p2Comp->Y + (p2Comp->Scale*0.5f)) > p_Current->Y))
|
|
{
|
|
float Divisor;
|
|
Divisor = 1.0f / ((float)p_Current->W + (float)p2Comp->W);
|
|
/* Merge */
|
|
{
|
|
p_Current->X = (((float)p_Current->W * (float)p_Current->X + (float)p2Comp->W * (float)p2Comp->X) * Divisor);
|
|
p_Current->Y = (((float)p_Current->W * (float)p_Current->Y + (float)p2Comp->W * (float)p2Comp->Y) * Divisor);
|
|
p_Current->Scale = (((float)p_Current->W * (float)p_Current->Scale + (float)p2Comp->W * (float)p2Comp->Scale) * Divisor);
|
|
p_Current->Alpha = (((float)p_Current->W * (float)p_Current->Alpha +
|
|
(float)p2Comp->W * (float)p2Comp->Alpha) * Divisor);
|
|
p_Current->ScaleV = (((float)p_Current->W * (float)p_Current->ScaleV +
|
|
(float)p2Comp->W * (float)p2Comp->ScaleV) * Divisor);
|
|
p_Current->W += p2Comp->W;
|
|
}
|
|
p2Comp->W = 0;
|
|
}
|
|
}
|
|
else
|
|
p2Comp = p_Last; // Break loop, only if X is out of range
|
|
p2Comp++;
|
|
}
|
|
}
|
|
p_Current++;
|
|
}
|
|
p_Current = &ALLXYW[0];
|
|
ALLPoints = 0;
|
|
#define PIXEL_SURF_TRESH 1
|
|
while (p_Current < p_Last)
|
|
{
|
|
if ((p_Current->W >= PIXEL_SURF_TRESH)/* || (p_Current->Age == 0)*/)
|
|
{
|
|
p_Current->LastIndex = ALLPoints;
|
|
ALLXYW[ALLPoints++] = *p_Current;
|
|
}
|
|
p_Current++;
|
|
}
|
|
|
|
if (ALLPoints > MAX_FACES) ALLPoints = MAX_FACES;
|
|
memmove((void *)(FACES+MAX_FACES),FACES,sizeof(TRACKEDPOINT_FACE)*MAX_FACES*(HISTORY - 1));
|
|
memcpy(FACES,ALLXYW,sizeof(TRACKEDPOINT_FACE)*MAX_FACES);
|
|
FACESNUM = ALLPoints;
|
|
while(FACESNUM<MAX_FACES)
|
|
{
|
|
FACES[FACESNUM++].LastIndex = -1;
|
|
}
|
|
FACESNUM = ALLPoints;
|
|
}
|
|
|
|
/********/
|
|
/********/
|
|
#define Number_Of_MipMapSubLevels 2
|
|
#define GROW_FACTOR powf(2,1.0f / (Number_Of_MipMapSubLevels + 1))
|
|
#define SEARCH_WINDOW_SIZE_H 1
|
|
#define SEARCH_WINDOW_SIZE_V 1
|
|
|
|
|
|
|
|
static s32 MapII2[512 * 512 * 4];
|
|
static s32 MapS4Test[512 * 512 * 4];
|
|
|
|
Vector Point2DTo3D(POINT P2D, float PZ, MAP *map)
|
|
{
|
|
Vector MouseV;
|
|
MouseV.x = 2.0f*((float)P2D.x) - map->SX;
|
|
MouseV.y = 2.0f*((float)P2D.y) - map->SY;
|
|
MouseV.z = PZ;
|
|
MouseV.x *= 1.0f / map->SX;
|
|
MouseV.y *= 1.0f / map->SX;
|
|
|
|
MouseV.x *= PZ / GetCurrentFocale();
|
|
MouseV.y *= PZ / GetCurrentFocale();
|
|
|
|
return MouseV;
|
|
}
|
|
|
|
POINT Point3DTo2D(Vector V, MAP *map)
|
|
{
|
|
POINT Ret;
|
|
float Ooz;
|
|
|
|
Ooz = 1.0f / V.z;
|
|
Ret.x = 0.5f*(GetCurrentFocale() * V.x * Ooz * map->SX + map->SX);
|
|
Ret.y = 0.5f*(GetCurrentFocale() * V.y * Ooz * map->SX + map->SY);
|
|
|
|
return Ret;
|
|
}
|
|
|
|
|
|
#define LAST_FEATURE 4
|
|
#define NB_FEATURES 8
|
|
|
|
void DrawCross(MAP *image, POINT p, u32 color, int len, char orient = 3);
|
|
|
|
#define SEARCH_SX 160
|
|
#define SEARCH_SY 160
|
|
#define FACE_SEARCH_SX 100 // WARNING FACE_SEARCH_SX <= SEARCH_SX
|
|
#define FACE_SEARCH_SY 100 // WARNING FACE_SEARCH_SY <= SEARCH_SY
|
|
#define PHIL_SEARCH_SX 100 // WARNING PHIL_SEARCH_SX <= SEARCH_SX
|
|
#define PHIL_SEARCH_SY 100 // WARNING PHIL_SEARCH_SX <= SEARCH_SX
|
|
|
|
static u32 featColors[] = {0, 0xFF0000, 0x00FF00, 0x0000FF, 0x0000FF, 0x00FFFF };
|
|
static RECT featBounds[] = {{0,0,0,0}, {0, 50, 100, 100}, {25, 25, 75, 75}, {0, 0, 50, 50}, {50, 0, 100, 50}};
|
|
static int MapBuffer[SEARCH_SX*SEARCH_SY];
|
|
static int MapBuffer2[PHIL_SEARCH_SX*PHIL_SEARCH_SY];
|
|
|
|
void GetPerpendicularAxe(POINT p1, POINT p2, POINT *p, POINT *dir)
|
|
{
|
|
p->x = (p1.x + p2.x)/2;
|
|
p->y = (p1.y + p2.y)/2;
|
|
|
|
dir->x = p2.y - p1.y;
|
|
dir->y = p2.x - p1.x;
|
|
}
|
|
|
|
void GetSymetricPoint(POINT p1, POINT p, POINT dir, POINT *p2)
|
|
{
|
|
float norm;
|
|
Vector nDir(dir.x, dir.y, 0);
|
|
Vector p1p(p.x-p1.x, p.y-p1.y,0);
|
|
|
|
nDir.Normalize();
|
|
p1p -= (nDir*(p1p*nDir));
|
|
|
|
p2->x = p1.x + p1p.x*2;
|
|
p2->y = p1.y + p1p.y*2;
|
|
}
|
|
|
|
void GetIntersectPoint(POINT p1, POINT p2, POINT p, POINT dir, POINT *pI)
|
|
{
|
|
float norm;
|
|
Vector nDir(dir.x, dir.y, 0);
|
|
Vector p1p(p.x-p1.x, p.y-p1.y, 0);
|
|
Vector p2p(p.x-p2.x, p.y-p2.y, 0);
|
|
Vector p1p2(p2.x-p1.x, p2.y-p1.y, 0);
|
|
|
|
nDir.Normalize();
|
|
p1p -= nDir*(p1p*nDir);
|
|
p2p -= nDir*(p2p*nDir);
|
|
|
|
p1p2 *= p1p.Norm()/(p1p.Norm()+p2p.Norm());
|
|
pI->x = p1.x + p1p2.x;
|
|
pI->y = p1.y + p1p2.y;
|
|
}
|
|
|
|
#define FACTOR 1.2f
|
|
#define HEAD_FACTOR 1.1f
|
|
void GetTrianglePoint(POINT p1, POINT p2, POINT dir, POINT *p3, int dirMod)
|
|
{
|
|
POINT mid, newMid;
|
|
float lenght;
|
|
Vector nDir(dir.x, dir.y, 0);
|
|
nDir.Normalize();
|
|
|
|
mid.x = (p1.x+p2.x)/2;
|
|
mid.y = (p1.y+p2.y)/2;
|
|
|
|
lenght = sqrt((float)((p1.x-mid.x)*(p1.x-mid.x)+(p1.y-mid.y)*(p1.y-mid.y)));
|
|
nDir *= lenght * FACTOR;
|
|
|
|
if (nDir.y * dirMod > 0)
|
|
{
|
|
p3->x = mid.x + nDir.x;
|
|
p3->y = mid.y + nDir.y;
|
|
} else
|
|
{
|
|
p3->x = mid.x - nDir.x;
|
|
p3->y = mid.y - nDir.y;
|
|
}
|
|
}
|
|
|
|
|
|
#define MIN_OK 0.0f
|
|
|
|
void ScaleRect(RECT *rect, float scale)
|
|
{
|
|
rect->left = rect->left*scale;
|
|
rect->top = rect->top*scale;
|
|
rect->right = rect->right*scale;
|
|
rect->bottom= rect->bottom*scale;
|
|
}
|
|
|
|
#define IMG_FACTOR ((float)SEARCH_SX/96)
|
|
|
|
u32 Feature_DETECT(MAP *CurrentMap,POINT *pDst,u32 MAXNUMBER, u32 BASE/*0:Eye 1:Mooth*/);
|
|
|
|
void AddPhilFeatures(MAP *baseMap, POINT Features[NB_FEATURES][8], int FeaturesNb[NB_FEATURES], RECT *faceRect)
|
|
{
|
|
MAP baseInMap;
|
|
MAP PhiMap;
|
|
|
|
baseInMap = *baseMap;
|
|
baseInMap.SY /= 2;
|
|
baseInMap.BASE += baseInMap.SY * baseInMap.PITCH;
|
|
|
|
PhiMap.BASE = MapBuffer2;
|
|
PhiMap.PITCH =
|
|
PhiMap.SX = PHIL_SEARCH_SX;
|
|
PhiMap.SY = PHIL_SEARCH_SX * baseInMap.SY / baseInMap.SX;
|
|
|
|
COPY_OX_RGBA(&baseInMap, &PhiMap);
|
|
|
|
FeaturesNb[5] = Feature_DETECT(&PhiMap, Features[5], 8,0);
|
|
for (int featPt=0; featPt<FeaturesNb[5]; featPt++)
|
|
{
|
|
Features[5][featPt].x = (Features[5][featPt].x )*(faceRect->right-faceRect->left)/PHIL_SEARCH_SX + faceRect->left;
|
|
Features[5][featPt].y = (Features[5][featPt].y +PhiMap.SY )*(faceRect->bottom-faceRect->top)/PHIL_SEARCH_SX + faceRect->top;
|
|
}
|
|
|
|
if (FeaturesNb[5] > 0 && FeaturesNb[5] <= 2)
|
|
{
|
|
for (int featPt=0; featPt<FeaturesNb[5]; featPt++)
|
|
{
|
|
BOOL sameFeatFound = false;
|
|
for (int featEye = 0; featEye <FeaturesNb[1]; featEye++)
|
|
{
|
|
if (((Features[1][featEye].x-Features[5][featPt].x)*(Features[1][featEye].x-Features[5][featPt].x) +
|
|
(Features[1][featEye].y-Features[5][featPt].y)*(Features[1][featEye].y-Features[5][featPt].y)) <= 8)
|
|
{
|
|
sameFeatFound = true;
|
|
Features[1][featEye].x = (Features[1][featEye].x + Features[5][featPt].x)/2;
|
|
Features[1][featEye].y = (Features[1][featEye].y + Features[5][featPt].y)/2;
|
|
}
|
|
}
|
|
if (!sameFeatFound)
|
|
{
|
|
for (int featEye = 0; featEye <FeaturesNb[1]; featEye++)
|
|
{
|
|
if ((Features[1][featEye].x - ((faceRect->left+faceRect->right)/2))*
|
|
(Features[5][featPt].x - ((faceRect->left+faceRect->right)/2)) > 0)
|
|
{
|
|
sameFeatFound = true;
|
|
}
|
|
}
|
|
}
|
|
if (!sameFeatFound)
|
|
{
|
|
Features[1][FeaturesNb[1]++] = Features[5][featPt];
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
#endif
|
|
int MatchFeatures(MAP *imageMap, POINT *imagePoints, Vector *FeatVect, int nbPoint, int mode, MATRIX *resultMatrix, Vector *resultScale)
|
|
{
|
|
extern MATRIX MatrixSrc;
|
|
extern u32 From_Scratch;
|
|
u32 POsit_2D(Vector *ImagePoints , Vector *DataPoints , Vector *CameraPos , u32 ulNumberOfPoints , u32 LasKeyIndex , u32 SizeX, u32 SizeY);
|
|
u32 POsit_3D(Vector *ImagePoints , Vector *DataPoints , Vector *CameraPos , u32 ulNumberOfPoints , u32 LasKeyIndex , u32 SizeX, u32 SizeY);
|
|
float Compute_Matrix_E(MATRIX *MatrixSrc, Vector *ImagePoints , Vector *DataPoints , u32 ulNumberOfPoints ,u32 LasKeyIndex);
|
|
|
|
Vector ImageVectPoint[32];
|
|
Vector DataPoints[32];
|
|
|
|
for (int pts=0; pts<nbPoint; pts++)
|
|
{
|
|
ImageVectPoint[pts].x = ((float)imagePoints[pts].x - imageMap->SX/2)/(imageMap->SX/2);
|
|
ImageVectPoint[pts].y = ((float)imagePoints[pts].y - imageMap->SY/2)/(imageMap->SX/2);
|
|
}
|
|
|
|
Vector currentScale;
|
|
float error, bestError = 1000.0f;
|
|
int ret, bestRet = 0;
|
|
|
|
currentScale.z = 1.0f;
|
|
for (currentScale.x=1.0f; currentScale.x<1.4f; currentScale.x+=0.1f)
|
|
{
|
|
for (currentScale.y=1.0f; currentScale.y<1.4f; currentScale.y+=0.1f)
|
|
{
|
|
for (int pts = 0; pts < nbPoint; pts++)
|
|
{
|
|
DataPoints[pts] = currentScale & FeatVect[pts];
|
|
}
|
|
|
|
From_Scratch = 1;
|
|
MatrixSrc.Identity();
|
|
if (mode == 2)
|
|
ret = POsit_2D(ImageVectPoint, DataPoints , NULL, nbPoint, 0, imageMap->SX, imageMap->SY);
|
|
else
|
|
ret = POsit_3D(ImageVectPoint, DataPoints , NULL, nbPoint, 0, imageMap->SX, imageMap->SY);
|
|
|
|
error = Compute_Matrix_E(&MatrixSrc, ImageVectPoint , DataPoints , nbPoint, 0);
|
|
if (ret > bestRet || ((ret == bestRet) && error < bestError))
|
|
{
|
|
*resultMatrix = MatrixSrc;
|
|
bestError = error;
|
|
bestRet = ret;
|
|
*resultScale = currentScale;
|
|
}
|
|
From_Scratch = 0;
|
|
}
|
|
}
|
|
return bestRet;
|
|
}
|