812 lines
20 KiB
C++
812 lines
20 KiB
C++
#include "SCENE.h"
|
|
|
|
#define MYDEBUG
|
|
|
|
#define BORDER_LIMIT 6
|
|
#define RAIDUS 6
|
|
#define RAIDUSD 4
|
|
|
|
typedef struct GC_C_
|
|
{
|
|
float X,Y;
|
|
float XC,YC,Div;
|
|
u32 PITCH;
|
|
u32 Counter;
|
|
u32 Or;
|
|
} GC_C;
|
|
|
|
#define NB_LEVELS 3
|
|
|
|
/* Valeur OK a remettre si jamais ça cafouille *******
|
|
#define FERN_POINTS (16)
|
|
|
|
#define FERN_PATCH_SX 8
|
|
#define FERN_PATCH_SY 8
|
|
#define FERN_MAX_CORRELATION (16)
|
|
#define FERN_MAX_PT_DISTANCE (5*5)
|
|
#define INTER_LEVEL_SEARCH 2
|
|
*****************************************************/
|
|
|
|
#define FERN_POINTS (16)
|
|
|
|
#define FERN_PATCH_SX 10
|
|
#define FERN_PATCH_SY 10
|
|
#define FERN_MAX_CORRELATION (16)
|
|
#define FERN_MAX_PT_DISTANCE (5*5)
|
|
#define INTER_LEVEL_SEARCH 2
|
|
#define FERN_MAX_LIMIT 48
|
|
|
|
|
|
static BOOL FERN_INIT = true;
|
|
static POINT FERN_CATCH[FERN_POINTS];
|
|
|
|
typedef struct
|
|
{
|
|
POINT coords;
|
|
POINT coordsNext;
|
|
u32 weight;
|
|
u32 tag; // 1 -> featPoint, 2-> usedPoint
|
|
u8 FernMask[FERN_POINTS];
|
|
} ZoneInfo;
|
|
|
|
typedef struct
|
|
{
|
|
int size;
|
|
u32 count;
|
|
ZoneInfo *src;
|
|
POINT P;
|
|
|
|
int nbTrackPoints;
|
|
POINT *trackPoints;
|
|
int trackSearch;
|
|
} GGPoint;
|
|
|
|
#define MIN_POINTS_RATIO 4
|
|
#define MAX_TRACK_POINTS 256
|
|
|
|
static BOOL GGPInit = true;
|
|
static GGPoint GGP[MAX_TRACK_POINTS][NB_LEVELS];
|
|
|
|
#define MAX_IMG_SIZE 512
|
|
|
|
static s32 M6InSave[2 * MAX_IMG_SIZE * MAX_IMG_SIZE];
|
|
static s32 M6BaseSave[2 * MAX_IMG_SIZE * MAX_IMG_SIZE];
|
|
static s32 ImgBaseSave[2 * MAX_IMG_SIZE * MAX_IMG_SIZE];
|
|
|
|
void GET_GC(GC_C *GG,s32 *Pixel,s32 And,s32 AddX,s32 AddY);
|
|
|
|
inline u32 FernListEq(u8 *FernList1, u8 *FernList2)
|
|
{
|
|
int Counter = FERN_POINTS;
|
|
u32 result = 0;
|
|
while(Counter--)
|
|
result += GetBitNumber(FernList1[Counter] ^ FernList2[Counter]);
|
|
return result;
|
|
}
|
|
|
|
inline bool PointsInitRandom(BOOL *isInit, POINT *list, int nbPoints, int SX, int SY)
|
|
{
|
|
if (!*isInit) return false;
|
|
|
|
int qtInNb = nbPoints / 4;
|
|
int isAllreadyPresent;
|
|
|
|
SX /= 2;
|
|
SY /= 2;
|
|
for (int qt=0; qt<4; qt++)
|
|
{
|
|
for (int qtIn=0; qtIn<qtInNb; qtIn++)
|
|
{
|
|
do
|
|
{
|
|
list[qtIn + qt*qtInNb].x = (rand()* SX) / RAND_MAX;
|
|
list[qtIn + qt*qtInNb].y = (rand()* SY) / RAND_MAX;
|
|
|
|
isAllreadyPresent = false;
|
|
for (int qtInIn=0; !isAllreadyPresent && qtInIn<qtIn; qtInIn++)
|
|
{
|
|
if ((list[qtIn + qt*qtInNb].x == list[qtInIn + qt*qtInNb].x) &&
|
|
(list[qtIn + qt*qtInNb].y == list[qtInIn + qt*qtInNb].y))
|
|
isAllreadyPresent = true;
|
|
}
|
|
} while (isAllreadyPresent);
|
|
if (qt / 2) list[qtIn + qt*qtInNb].x = -list[qtIn + qt*qtInNb].x;
|
|
if (qt % 2) list[qtIn + qt*qtInNb].y = -list[qtIn + qt*qtInNb].y;
|
|
}
|
|
}
|
|
*isInit = false;
|
|
return true;
|
|
}
|
|
|
|
inline void PointsInitRec(POINT *list, int nbPoints, RECT bounds, int dir)
|
|
{
|
|
int nbPoints1, nbPoints2;
|
|
RECT bounds1, bounds2;
|
|
BOOL way;
|
|
|
|
if (nbPoints <= 0) return ;
|
|
if (nbPoints == 1)
|
|
{
|
|
list->x = (bounds.left + bounds.right)/2;
|
|
list->y = (bounds.top + bounds.bottom)/2;
|
|
if (dir)
|
|
{
|
|
list->x += (bounds.left + bounds.right) & 0x1;
|
|
list->y += (bounds.top + bounds.bottom) & 0x1;
|
|
}
|
|
return;
|
|
}
|
|
|
|
nbPoints1 =
|
|
nbPoints2 = nbPoints/2;
|
|
|
|
if (dir) nbPoints2 += nbPoints&0x1;
|
|
else nbPoints1 += nbPoints&0x1;
|
|
|
|
bounds1 =
|
|
bounds2 = bounds;
|
|
|
|
if ((bounds.right - bounds.left) == (bounds.bottom - bounds.top))
|
|
way = dir;
|
|
else if ((bounds.right - bounds.left) > (bounds.bottom - bounds.top))
|
|
way = 1;
|
|
else
|
|
way = 0;
|
|
|
|
if (way)
|
|
{
|
|
bounds1.right = (bounds.right + bounds.left)/2;
|
|
bounds2.left = bounds1.right + 1;
|
|
} else
|
|
{
|
|
bounds1.bottom = (bounds.top + bounds.bottom)/2;
|
|
bounds2.top = bounds1.bottom + 1;
|
|
}
|
|
|
|
PointsInitRec(list, nbPoints1, bounds1, !dir);
|
|
PointsInitRec(list+nbPoints1, nbPoints2, bounds2, !dir);
|
|
}
|
|
|
|
inline bool PointsInit(BOOL *isInit, POINT *list, int nbPoints, int SX, int SY)
|
|
{
|
|
if (!*isInit) return false;
|
|
|
|
RECT bounds;
|
|
list[0].x = list[0].y = 0;
|
|
|
|
bounds.right = SX/2 + (SX&0x1);
|
|
bounds.left = -SX/2;
|
|
bounds.bottom = SY/2;
|
|
bounds.top = -SY/2 - (SX&0x1);
|
|
PointsInitRec(list+1, nbPoints-1, bounds, 0);
|
|
|
|
*isInit = false;
|
|
return true;
|
|
}
|
|
|
|
|
|
void GetFern(MAP *map, int x, int y, u8 *FernList)
|
|
{
|
|
int xIn, yIn;
|
|
PointsInit(&FERN_INIT, FERN_CATCH, FERN_POINTS, FERN_PATCH_SX, FERN_PATCH_SY);
|
|
|
|
int Counter = FERN_POINTS;
|
|
memset(FernList, 0, FERN_POINTS*sizeof(u8));
|
|
while (Counter--)
|
|
{
|
|
xIn = FERN_CATCH[Counter].x + x;
|
|
yIn = FERN_CATCH[Counter].y + y;
|
|
if ((xIn > BORDER_LIMIT) &&
|
|
(yIn > BORDER_LIMIT) &&
|
|
(map->SX - xIn > BORDER_LIMIT) &&
|
|
(map->SY - yIn > BORDER_LIMIT))
|
|
FernList[Counter] = *(map->GetBase() + xIn + yIn*map->PITCH) & 0xFF;
|
|
}
|
|
}
|
|
|
|
|
|
void MGet(MAP *CurrentMap, MAP *M6Ptr)
|
|
{
|
|
static MAP WorkMap;
|
|
static u32 WorkBufSize = 0;
|
|
MAP M1,M2,M3,M4,M5,M6;
|
|
|
|
if (WorkBufSize < 512 + (CurrentMap->SX + 16)* CurrentMap->SY)
|
|
{
|
|
WorkMap.SetSize(CurrentMap->SX*3, CurrentMap->SY*2, 1);
|
|
}
|
|
|
|
M1 = M2 = M3 = M4 = M5 = M6 = *CurrentMap;
|
|
M6.PITCH = M5.PITCH = M4.PITCH = M3.PITCH = M2.PITCH = M1.PITCH = CurrentMap->SX;
|
|
M6.SetBase(WorkMap.GetBase());
|
|
M5.SetBase(WorkMap.GetBase() + CurrentMap->SX * CurrentMap->SY);
|
|
M4.SetBase(WorkMap.GetBase() + CurrentMap->SX * CurrentMap->SY * 2);
|
|
M3.SetBase(WorkMap.GetBase() + CurrentMap->SX * CurrentMap->SY * 3);
|
|
M2.SetBase(WorkMap.GetBase() + CurrentMap->SX * CurrentMap->SY * 4);
|
|
M1.SetBase(WorkMap.GetBase() + CurrentMap->SX * CurrentMap->SY * 5);
|
|
|
|
|
|
// maxi compute
|
|
COPY_BW(CurrentMap,&M1);
|
|
SHIFT(&M1,2);
|
|
|
|
//COPY_RGBA(&M1,CurrentMap);
|
|
//SET(CurrentMap, 0);
|
|
|
|
LF_GaussianFilter(&M1,&M1,&M3,0,3);
|
|
LF_GaussianFilter(&M1,&M2,&M3,0,3);
|
|
|
|
MAX_CROSS(&M1,&M5);
|
|
MAX(&M5,&M3);
|
|
|
|
MIN_CROSS(&M1,&M5);
|
|
MIN(&M5,&M4);
|
|
|
|
//DARKER(CurrentMap);
|
|
|
|
ADD_VALUE(&M3,3);
|
|
ADD_VALUE(&M4,-3);
|
|
SET(&M5,0);
|
|
|
|
|
|
|
|
//////////////////////////////////////////////////
|
|
// First -> Compute neighbourought flags
|
|
|
|
{
|
|
s32 *m1,*m2,*m3,*m4,*m5,*m6;
|
|
for (int y = BORDER_LIMIT; y < CurrentMap->SY - BORDER_LIMIT; y++)
|
|
{
|
|
for (int x = BORDER_LIMIT; x < CurrentMap->SX - BORDER_LIMIT; x++)
|
|
{
|
|
s32 *VMa,*VMi,VA;
|
|
u32 RES,RESM,RESE;
|
|
RES =RESM= 0;
|
|
VMa = M4.GetBase() + x + (y) * M3.PITCH;
|
|
VMi = M3.GetBase() + x + (y) * M3.PITCH;
|
|
VA = *(M2.GetBase() + x + (y) * M2.PITCH) ;
|
|
if (*VMi - *VMa > 10)
|
|
{
|
|
if ((*(VMa - RAIDUSD - RAIDUSD * M2.PITCH)) > VA) RES |= 1;
|
|
if ((*(VMa - RAIDUS * M2.PITCH)) > VA) RES |= 2;
|
|
if ((*(VMa + RAIDUSD - RAIDUSD * M2.PITCH)) > VA) RES |= 4;
|
|
if ((*(VMa + RAIDUS )) > VA) RES |= 8;
|
|
if ((*(VMa + RAIDUSD + RAIDUSD * M2.PITCH)) > VA) RES |= 16;
|
|
if ((*(VMa + RAIDUS * M2.PITCH)) > VA) RES |= 32;
|
|
if ((*(VMa - RAIDUSD + RAIDUSD * M2.PITCH)) > VA) RES |= 64;
|
|
if ((*(VMa - RAIDUS )) > VA) RES |= 128;
|
|
|
|
*(M5.GetBase() + x + y * M5.PITCH) = RES | ((*VMi - *VMa) << 8);
|
|
}
|
|
*(M2.GetBase() + x + y * M5.PITCH) = RES | ((*VMi - *VMa) << 8);
|
|
|
|
}
|
|
}
|
|
}
|
|
ORb(&M5,&M1);
|
|
ORb(&M1,&M6);
|
|
|
|
SET(&M1,0);
|
|
COPY(&M5,&M1);
|
|
|
|
//////////////////////////////////////////////////
|
|
// Second -> Compute non - symetry in flags
|
|
{
|
|
for (int y = BORDER_LIMIT; y < CurrentMap->SY - BORDER_LIMIT; y++)
|
|
{
|
|
for (int x = BORDER_LIMIT; x < CurrentMap->SX - BORDER_LIMIT; x++)
|
|
{
|
|
s32 *VMa,*VMi,VA;
|
|
u32 RES,RESM,RESE;
|
|
RES =RESM= 0;
|
|
RES = *(M5.GetBase() + x + (y) * M5.PITCH) & 0xff;
|
|
*(M5.GetBase() + x + y * M5.PITCH) = *(M2.GetBase() + x + y * M2.PITCH) & 0xff00;
|
|
*(M2.GetBase() + x + y * M2.PITCH) &= 0xff;
|
|
u32 Color;
|
|
|
|
if (GetBitNumber(RES) > 7)
|
|
{
|
|
*(M5.GetBase() + x + y * M5.PITCH) |= 0x4;
|
|
}
|
|
else
|
|
{
|
|
u32 RESO;
|
|
RESO = RES ;
|
|
RESO <<= 4;
|
|
RESO |= RES>>4;
|
|
RESO ^= RES;
|
|
RESO &= 0xff;
|
|
|
|
RESE = RES >> 1;
|
|
RESE |= RES << 7;
|
|
RES &= RESE;
|
|
RESE = RES >> 1;
|
|
RESE |= RES << 7;
|
|
RES &= RESE;
|
|
|
|
if (GetBitNumber(RES) > 3)
|
|
{
|
|
*(M5.GetBase() + x + y * M5.PITCH) |= 0x2;
|
|
} else
|
|
if (GetBitNumber(RES) > 2)
|
|
{
|
|
*(M5.GetBase() + x + y * M5.PITCH) |= 0x1;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (M6Ptr)
|
|
*M6Ptr = M6;
|
|
}
|
|
|
|
void MGet(MAP *CurrentMap, MAP *M6Ptr, RECT *bounds)
|
|
{
|
|
MAP map = *CurrentMap;
|
|
map.SX = bounds->right - bounds->left;
|
|
map.SY = bounds->bottom - bounds->top;
|
|
map.SetBase(CurrentMap->GetBase() + bounds->left + bounds->top*map.PITCH);
|
|
|
|
MGet(&map, M6Ptr);
|
|
|
|
s32 *newPtrIn = M6InSave + bounds->left + bounds->top*CurrentMap->PITCH;
|
|
memset(M6InSave, 0, CurrentMap->PITCH*CurrentMap->SY*4);
|
|
for (int i=0; i<M6Ptr->SY; i++, newPtrIn+=CurrentMap->PITCH)
|
|
{
|
|
memcpy(newPtrIn, M6Ptr->GetBase()+i*M6Ptr->PITCH, M6Ptr->PITCH*4);
|
|
}
|
|
|
|
M6Ptr->SX = CurrentMap->SX;
|
|
M6Ptr->SY = CurrentMap->SY;
|
|
M6Ptr->PITCH = CurrentMap->PITCH;
|
|
M6Ptr->SetBase(M6InSave);
|
|
}
|
|
|
|
|
|
int SearchNewBary(GGPoint *GGPPtr, POINT *vMove)
|
|
{
|
|
POINT nextP;
|
|
POINT midBase, midDest, midError;
|
|
|
|
int error;
|
|
int usedPoints;
|
|
BOOL isFirst = true;
|
|
BOOL isLast = false;
|
|
|
|
do
|
|
{
|
|
midBase.x = midBase.y = 0;
|
|
midDest.x = midDest.y = 0;
|
|
usedPoints = 0;
|
|
|
|
for (int ct=0; ct<GGPPtr->count; ct++)
|
|
{
|
|
GGPPtr->src[ct].tag &= 0x1;
|
|
if (GGPPtr->src[ct].coordsNext.x != 0 ||
|
|
GGPPtr->src[ct].coordsNext.y != 0)
|
|
{
|
|
if (!isFirst)
|
|
{
|
|
nextP.x = GGPPtr->src[ct].coords.x + vMove->x;
|
|
nextP.y = GGPPtr->src[ct].coords.y + vMove->y;
|
|
|
|
if (((GGPPtr->src[ct].coordsNext.x-nextP.x)*(GGPPtr->src[ct].coordsNext.x-nextP.x) +
|
|
(GGPPtr->src[ct].coordsNext.y-nextP.y)*(GGPPtr->src[ct].coordsNext.y-nextP.y)) > error)
|
|
continue;
|
|
}
|
|
|
|
midBase.x += GGPPtr->src[ct].coords.x;
|
|
midBase.y += GGPPtr->src[ct].coords.y;
|
|
midDest.x += GGPPtr->src[ct].coordsNext.x;
|
|
midDest.y += GGPPtr->src[ct].coordsNext.y;
|
|
GGPPtr->src[ct].tag |= 0x2;
|
|
usedPoints++;
|
|
}
|
|
}
|
|
if (usedPoints == 0 || usedPoints < GGPPtr->nbTrackPoints/MIN_POINTS_RATIO/*MIN_POINTS_FOUNDS*/)
|
|
return 0;
|
|
|
|
midBase.x /= usedPoints;
|
|
midBase.y /= usedPoints;
|
|
midDest.x /= usedPoints;
|
|
midDest.y /= usedPoints;
|
|
|
|
vMove->x = midDest.x - midBase.x;
|
|
vMove->y = midDest.y - midBase.y;
|
|
|
|
// calc mid error
|
|
midError.x = midError.y = 0;
|
|
usedPoints = 0;
|
|
for (int ct=0; ct<GGPPtr->count; ct++)
|
|
{
|
|
if (GGPPtr->src[ct].tag & 0x2)
|
|
{
|
|
nextP.x = GGPPtr->src[ct].coords.x + vMove->x;
|
|
nextP.y = GGPPtr->src[ct].coords.y + vMove->y;
|
|
|
|
midError.x += (GGPPtr->src[ct].coordsNext.x - nextP.x)*(GGPPtr->src[ct].coordsNext.x - nextP.x);
|
|
midError.y += (GGPPtr->src[ct].coordsNext.y - nextP.y)*(GGPPtr->src[ct].coordsNext.y - nextP.y);
|
|
usedPoints++;
|
|
}
|
|
}
|
|
midError.x /= usedPoints;
|
|
midError.y /= usedPoints;
|
|
error = midError.x + midError.y;
|
|
isFirst = false;
|
|
} while (error > FERN_MAX_PT_DISTANCE);
|
|
// if (isLast) error = 0;
|
|
// if (error <= FERN_MAX_PT_DISTANCE)
|
|
// isLast = true;
|
|
//} while (error > 0);
|
|
// } while (error > FERN_MAX_PT_DISTANCE);
|
|
return usedPoints;
|
|
}
|
|
|
|
int SearchNewCenter(POINT *vMove, GGPoint *GGPPtr)
|
|
{
|
|
return SearchNewBary(GGPPtr, vMove);
|
|
// maybe add new traitment ...
|
|
}
|
|
|
|
void SearchBestPoint(GGPoint *GGPPtr, MAP *M6, int lvlOO)
|
|
{
|
|
u32 MaskEqu;
|
|
u8 FERN_IN[FERN_POINTS];
|
|
RECT searchRect;
|
|
int lvl, lvlOut;
|
|
int searchZone;
|
|
|
|
|
|
//SEARCH_PATCH_SX_BASE
|
|
for (int ggpIdx=0; ggpIdx<GGPPtr[0].count; ggpIdx++)
|
|
{
|
|
if (lvlOO < 0)
|
|
lvlOut = NB_LEVELS-1;
|
|
else
|
|
lvlOut = lvlOO;
|
|
while (lvlOut >= 0 && !(GGPPtr[lvlOut].src[ggpIdx].tag & 0x1)) lvlOut--;
|
|
if (lvlOut < 0) continue;
|
|
|
|
GGPPtr[0].src[ggpIdx].weight = FERN_MAX_CORRELATION;
|
|
lvl = lvlOut;
|
|
searchZone = (GGPPtr->trackSearch / (1<<lvl))/2;
|
|
if (searchZone < 1) searchZone = 1;
|
|
searchRect.left = GGPPtr[lvl].src[ggpIdx].coords.x - searchZone;
|
|
searchRect.right = GGPPtr[lvl].src[ggpIdx].coords.x + searchZone;
|
|
searchRect.top = GGPPtr[lvl].src[ggpIdx].coords.y - searchZone;
|
|
searchRect.bottom = GGPPtr[lvl].src[ggpIdx].coords.y + searchZone;
|
|
|
|
if (searchRect.left < BORDER_LIMIT) searchRect.left = BORDER_LIMIT;
|
|
if (searchRect.top < BORDER_LIMIT) searchRect.top = BORDER_LIMIT;
|
|
if (searchRect.right > (M6[lvl].SX-BORDER_LIMIT)) searchRect.right = M6[lvl].SX-BORDER_LIMIT;
|
|
if (searchRect.bottom > (M6[lvl].SY-BORDER_LIMIT)) searchRect.bottom = M6[lvl].SY-BORDER_LIMIT;
|
|
|
|
GGPPtr[lvl].src[ggpIdx].weight = FERN_MAX_LIMIT;
|
|
for (int y=searchRect.top; y<=searchRect.bottom; y++)
|
|
{
|
|
for (int x=searchRect.left; x<=searchRect.right; x++)
|
|
{
|
|
GetFern(&M6[lvl], x, y, FERN_IN);
|
|
MaskEqu = FernListEq(FERN_IN, GGPPtr[lvl].src[ggpIdx].FernMask);
|
|
if (MaskEqu < GGPPtr[lvl].src[ggpIdx].weight)
|
|
{
|
|
GGPPtr[lvl].src[ggpIdx].weight = MaskEqu;
|
|
GGPPtr[lvl].src[ggpIdx].coordsNext.x = x;
|
|
GGPPtr[lvl].src[ggpIdx].coordsNext.y = y;
|
|
GGPPtr[lvl].src[ggpIdx].tag |= 0x2;
|
|
}
|
|
}
|
|
}
|
|
while (lvl--)
|
|
{
|
|
if (lvl > 0)
|
|
GGPPtr[lvl].src[ggpIdx].weight = FERN_MAX_LIMIT;
|
|
else
|
|
GGPPtr[lvl].src[ggpIdx].weight = FERN_MAX_CORRELATION;
|
|
for (int y=GGPPtr[lvl+1].src[ggpIdx].coordsNext.y*2-INTER_LEVEL_SEARCH;
|
|
y<=GGPPtr[lvl+1].src[ggpIdx].coordsNext.y*2+INTER_LEVEL_SEARCH; y++)
|
|
{
|
|
if (y<BORDER_LIMIT || y>(M6[lvl].SY-BORDER_LIMIT)) continue;
|
|
for (int x=GGPPtr[lvl+1].src[ggpIdx].coordsNext.x*2-INTER_LEVEL_SEARCH;
|
|
x<=GGPPtr[lvl+1].src[ggpIdx].coordsNext.x*2+INTER_LEVEL_SEARCH; x++)
|
|
{
|
|
if (x<BORDER_LIMIT || x>(M6[lvl].SX-BORDER_LIMIT)) continue;
|
|
GetFern(&M6[lvl], x, y, FERN_IN);
|
|
MaskEqu = FernListEq(FERN_IN, GGPPtr[lvl].src[ggpIdx].FernMask);
|
|
if (MaskEqu < GGPPtr[lvl].src[ggpIdx].weight)
|
|
{
|
|
GGPPtr[lvl].src[ggpIdx].weight = MaskEqu;
|
|
GGPPtr[lvl].src[ggpIdx].coordsNext.x = x;
|
|
GGPPtr[lvl].src[ggpIdx].coordsNext.y = y;
|
|
GGPPtr[lvl].src[ggpIdx].tag |= 0x2;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// Step 2 : use map to search images
|
|
inline void SetHalfMap(MAP *src, MAP *dst)
|
|
{
|
|
dst->PITCH =
|
|
dst->SX = src->SX>>1;
|
|
dst->SY = src->SY>>1;
|
|
dst->SetBase(src->GetBase() + src->PITCH*src->SY);
|
|
}
|
|
|
|
|
|
int ZoneCompute(MAP *imageMap, int index, int *ptX, int *ptY, int forceReset, int newImage, int nbTrkPoint, int trkZone, int trkSearch)
|
|
{
|
|
MAP img[NB_LEVELS];
|
|
MAP M6[NB_LEVELS];
|
|
int newX, newY;
|
|
int ptXIn, ptYIn;
|
|
|
|
float FACTOR = 1;
|
|
float FACTOR_INV = 1;
|
|
|
|
if (index >= MAX_TRACK_POINTS)
|
|
return 0;
|
|
|
|
START_RASTER(Feature_Test1)
|
|
|
|
img[0] = *imageMap;
|
|
|
|
ptXIn = *ptX;
|
|
ptYIn = *ptY;
|
|
if (imageMap->SX > MAX_IMG_SIZE)
|
|
{
|
|
FACTOR = (float)MAX_IMG_SIZE / imageMap->SX;
|
|
FACTOR_INV = 1*FACTOR_INV;
|
|
ptXIn = (int)(FACTOR*ptXIn);
|
|
ptYIn = (int)(FACTOR*ptYIn);
|
|
trkZone = (int)(FACTOR*trkZone);
|
|
trkSearch = (int)(FACTOR*trkSearch);
|
|
}
|
|
|
|
if (newImage)
|
|
{
|
|
s32 *M6BasePtr;
|
|
s32 *imgBasePtr;
|
|
RECT zoneBounds[NB_LEVELS];
|
|
|
|
if (newImage == 2)
|
|
{
|
|
SetRect(&zoneBounds[0], ptXIn - (trkSearch + trkZone)/2,
|
|
ptYIn - (trkSearch + trkZone)/2,
|
|
ptXIn + (trkSearch + trkZone)/2,
|
|
ptYIn + (trkSearch + trkZone)/2);
|
|
if (zoneBounds[0].left < 0) zoneBounds[0].left = 0;
|
|
if (zoneBounds[0].top < 0) zoneBounds[0].top = 0;
|
|
if (zoneBounds[0].right > imageMap->SX) zoneBounds[0].right = imageMap->SX;
|
|
if (zoneBounds[0].bottom> imageMap->SY) zoneBounds[0].bottom= imageMap->SY;
|
|
}
|
|
|
|
if (FACTOR != 1)
|
|
{
|
|
img[0].SX = (int)(FACTOR*img[0].SX);
|
|
img[0].PITCH = img[0].SX;
|
|
img[0].SY =(int)(FACTOR*img[0].SY);
|
|
img[0].SetBase(ImgBaseSave);
|
|
COPY_OX_RGBA(imageMap, &img[0]);
|
|
}
|
|
imgBasePtr = img[0].GetBase();
|
|
img[0].SetBase(ImgBaseSave);
|
|
for (int i=1; i<NB_LEVELS; i++)
|
|
{
|
|
SetHalfMap(&img[i-1], &img[i]);
|
|
zoneBounds[i].left = zoneBounds[i-1].left /2;
|
|
zoneBounds[i].right = zoneBounds[i-1].right /2;
|
|
zoneBounds[i].top = zoneBounds[i-1].top /2;
|
|
zoneBounds[i].bottom= zoneBounds[i-1].bottom /2;
|
|
}
|
|
img[0].SetBase(imgBasePtr);
|
|
for (int i=1; i<NB_LEVELS; i++)
|
|
{
|
|
COPY_OX_RGBA(&img[i-1], &img[i]);
|
|
}
|
|
|
|
M6BasePtr = M6BaseSave;
|
|
for (int i=0; i<NB_LEVELS; i++)
|
|
{
|
|
if (newImage == 2)
|
|
MGet(&img[i], &M6[i], &zoneBounds[i]);
|
|
else
|
|
MGet(&img[i], &M6[i]);
|
|
|
|
memcpy(M6BasePtr, M6[i].GetBase(), M6[i].PITCH*M6[i].SY*4);
|
|
M6BasePtr += M6[i].PITCH*M6[i].SY;
|
|
}
|
|
}
|
|
M6[0] = img[0]; M6[0].SetBase(M6BaseSave);
|
|
for (int i=1; i<NB_LEVELS; i++)
|
|
{
|
|
SetHalfMap(&M6[i-1], &M6[i]);
|
|
}
|
|
|
|
GGPoint *GGPPtr;
|
|
GGPoint *GGPPtr0 = GGP[index];
|
|
|
|
if (forceReset)
|
|
{
|
|
if (GGPInit)
|
|
{
|
|
for (int idx=0; idx<MAX_TRACK_POINTS; idx++)
|
|
memset(GGP[idx], 0, sizeof(GGP[idx]));
|
|
GGPInit = false;
|
|
}
|
|
|
|
for (int i=0; i<NB_LEVELS; i++)
|
|
{
|
|
GGPPtr = GGPPtr0 + i;
|
|
|
|
if (GGPPtr->size < M6[i].SX * M6[i].SY)
|
|
{
|
|
if (GGPPtr->src)
|
|
CC_free(GGPPtr->src);
|
|
|
|
GGPPtr->size = M6[i].SX * M6[i].SY;
|
|
GGPPtr->src = (ZoneInfo *)CC_malloc(GGPPtr->size*sizeof(ZoneInfo));
|
|
}
|
|
|
|
GGPPtr->trackSearch = trkSearch;
|
|
if (GGPPtr->trackPoints == NULL || GGPPtr->nbTrackPoints != nbTrkPoint)
|
|
{
|
|
int isInit = 1;
|
|
|
|
if (GGPPtr->trackPoints)
|
|
CC_free(GGPPtr->trackPoints);
|
|
GGPPtr->nbTrackPoints = nbTrkPoint;
|
|
GGPPtr->trackPoints = (POINT *)CC_malloc(GGPPtr->nbTrackPoints*sizeof(POINT));
|
|
PointsInit(&isInit, GGPPtr->trackPoints, GGPPtr->nbTrackPoints, trkZone, trkZone);
|
|
}
|
|
|
|
for (GGPPtr->count = 0; GGPPtr->count < GGPPtr->nbTrackPoints; GGPPtr->count++)
|
|
{
|
|
GGPPtr->src[GGPPtr->count].coords.x = ptXIn + GGPPtr->trackPoints[GGPPtr->count].x/(1<<i);
|
|
GGPPtr->src[GGPPtr->count].coords.y = ptYIn + GGPPtr->trackPoints[GGPPtr->count].y/(1<<i);
|
|
|
|
if (GGPPtr->src[GGPPtr->count].coords.x < BORDER_LIMIT ||
|
|
GGPPtr->src[GGPPtr->count].coords.y < BORDER_LIMIT ||
|
|
(M6[i].SX - GGPPtr->src[GGPPtr->count].coords.x < BORDER_LIMIT) ||
|
|
(M6[i].SY - GGPPtr->src[GGPPtr->count].coords.y < BORDER_LIMIT))
|
|
{
|
|
GGPPtr->src[GGPPtr->count].tag = 0;
|
|
} else
|
|
{
|
|
GGPPtr->src[GGPPtr->count].tag = 1;
|
|
GetFern(&M6[i], GGPPtr->src[GGPPtr->count].coords.x,
|
|
GGPPtr->src[GGPPtr->count].coords.y,
|
|
GGPPtr->src[GGPPtr->count].FernMask);
|
|
}
|
|
}
|
|
GGPPtr->P.x = ptXIn;
|
|
GGPPtr->P.y = ptYIn;
|
|
|
|
ptXIn = ptXIn>>1;
|
|
ptYIn = ptYIn>>1;
|
|
}
|
|
STOP_RASTER(Feature_Test1)
|
|
return 0;
|
|
}
|
|
|
|
// search image process
|
|
STOP_RASTER(Feature_Test1);
|
|
|
|
POINT FinalV = {0,0};
|
|
int searchRes;
|
|
int res = false;
|
|
|
|
/* Calcul complet ----------
|
|
START_RASTER(Feature_Test1);
|
|
for (int i=0; i<NB_LEVELS; i++)
|
|
{
|
|
for (int ct=0; ct<GGPPtr0[i].count; ct++)
|
|
{
|
|
GGPPtr0[i].src[ct].weight = FERN_MAX_CORRELATION;
|
|
GGPPtr0[i].src[ct].coordsNext.x =
|
|
GGPPtr0[i].src[ct].coordsNext.y = 0;
|
|
GGPPtr0[i].src[ct].tag &= 0x1;
|
|
}
|
|
}
|
|
|
|
SearchBestPoint(GGP[index], M6, 0);
|
|
|
|
FinalV.x = FinalV.y = 0;
|
|
searchRes = SearchNewCenter(&FinalV, GGPPtr0, 5);
|
|
|
|
if (searchRes > 0)
|
|
{
|
|
|
|
|
|
*ptX = (GGPPtr0->P.x + FinalV.x)*FACTOR_INV;
|
|
*ptY = (GGPPtr0->P.y + FinalV.y)*FACTOR_INV;
|
|
|
|
POINT P;
|
|
P.x = *ptX;
|
|
P.y = *ptY;
|
|
|
|
#ifdef MYDEBUG
|
|
for (int ct=0; ct<GGPPtr0->count; ct++)
|
|
{
|
|
if ((GGPPtr0->src[ct].tag & 0x3) == 0x3)
|
|
{
|
|
DrawLine(imageMap, GGPPtr0->src[ct].coords.x*FACTOR_INV,
|
|
GGPPtr0->src[ct].coords.y*FACTOR_INV,
|
|
GGPPtr0->src[ct].coordsNext.x*FACTOR_INV,
|
|
GGPPtr0->src[ct].coordsNext.y*FACTOR_INV, 0xFF0000);
|
|
}
|
|
}
|
|
DrawCross(imageMap, P, 0xFF0000,3);
|
|
#endif
|
|
res = true;
|
|
}
|
|
|
|
STOP_RASTER(Feature_Test1);
|
|
*/
|
|
|
|
|
|
START_RASTER(Feature_Test2);
|
|
for (int i=0; i<NB_LEVELS; i++)
|
|
{
|
|
for (int ct=0; ct<GGPPtr0[i].count; ct++)
|
|
{
|
|
GGPPtr0[i].src[ct].weight = FERN_MAX_CORRELATION;
|
|
GGPPtr0[i].src[ct].coordsNext.x =
|
|
GGPPtr0[i].src[ct].coordsNext.y = 0;
|
|
GGPPtr0[i].src[ct].tag &= 0x1;
|
|
}
|
|
}
|
|
|
|
SearchBestPoint(GGP[index], M6, -1);
|
|
|
|
FinalV.x = FinalV.y = 0;
|
|
searchRes = SearchNewCenter(&FinalV, GGPPtr0);
|
|
|
|
if (searchRes > 0)
|
|
{
|
|
|
|
|
|
*ptX = (GGPPtr0->P.x + FinalV.x)*FACTOR_INV;
|
|
*ptY = (GGPPtr0->P.y + FinalV.y)*FACTOR_INV;
|
|
|
|
POINT P;
|
|
P.x = *ptX;
|
|
P.y = *ptY;
|
|
|
|
#ifdef MYDEBUG
|
|
for (int ct=0; ct<GGPPtr0->count; ct++)
|
|
{
|
|
if ((GGPPtr0->src[ct].tag & 0x3) == 0x3)
|
|
{
|
|
DrawLine(imageMap, GGPPtr0->src[ct].coords.x*FACTOR_INV,
|
|
GGPPtr0->src[ct].coords.y*FACTOR_INV,
|
|
GGPPtr0->src[ct].coordsNext.x*FACTOR_INV,
|
|
GGPPtr0->src[ct].coordsNext.y*FACTOR_INV, 0xFF00);
|
|
}
|
|
}
|
|
DrawCross(imageMap, P, 0xFF0000,3);
|
|
#endif
|
|
res = true;
|
|
}
|
|
|
|
STOP_RASTER(Feature_Test2);
|
|
|
|
|
|
void DrawLine(MAP *MapDST,float X1,float Y1,float X2,float Y2,u32 Color);
|
|
#ifdef MYDEBUG
|
|
DrawLine(imageMap, *ptX - (trkZone*FACTOR_INV)/2, *ptY - (trkZone*FACTOR_INV)/2,
|
|
*ptX - (trkZone*FACTOR_INV)/2, *ptY + (trkZone*FACTOR_INV)/2, 0xFF00);
|
|
DrawLine(imageMap, *ptX + (trkZone*FACTOR_INV)/2, *ptY + (trkZone*FACTOR_INV)/2,
|
|
*ptX + (trkZone*FACTOR_INV)/2, *ptY - (trkZone*FACTOR_INV)/2, 0xFF00);
|
|
DrawLine(imageMap, *ptX + (trkZone*FACTOR_INV)/2, *ptY - (trkZone*FACTOR_INV)/2,
|
|
*ptX - (trkZone*FACTOR_INV)/2, *ptY - (trkZone*FACTOR_INV)/2, 0xFF00);
|
|
DrawLine(imageMap, *ptX - (trkZone*FACTOR_INV)/2, *ptY + (trkZone*FACTOR_INV)/2,
|
|
*ptX + (trkZone*FACTOR_INV)/2, *ptY + (trkZone*FACTOR_INV)/2, 0xFF00);
|
|
|
|
#endif
|
|
return res;
|
|
}
|