321 lines
No EOL
8.1 KiB
C++
321 lines
No EOL
8.1 KiB
C++
#include "TRACK.h"
|
|
#include "Rrasters.h"
|
|
|
|
float IG_GetError(WORKINGSPACE *WSPC,MATRIX *M,u32 Group2Test,float MAXErr = 100000000.0f)
|
|
{
|
|
float Error,NE;
|
|
TRACKEDPOINT *pTPCurrent, *pTPLast, *pTPrevious;
|
|
pTPCurrent = WSPC->Current_WSP->TPS;
|
|
pTPLast = pTPCurrent + WSPC->Current_WSP->nTps;
|
|
pTPrevious = WSPC->Previous_WSP->TPS;
|
|
NE = Error = 0.0f;
|
|
if (MAXErr < 5.0f) MAXErr = 5.0f;
|
|
while (pTPCurrent < pTPLast)
|
|
{
|
|
if (pTPCurrent->GroupIndex & Group2Test)
|
|
{
|
|
Vector V,LastV;
|
|
V.x = pTPCurrent->X;
|
|
V.y = pTPCurrent->Y;
|
|
V.z = 0;
|
|
LastV.x = (pTPrevious+pTPCurrent->PreviousIndex)->X;
|
|
LastV.y = (pTPrevious+pTPCurrent->PreviousIndex)->Y;
|
|
LastV.z = 0;
|
|
|
|
V = *M * V;
|
|
V -= LastV;
|
|
float LE = V.SqrNormXY();
|
|
if (LE > MAXErr)
|
|
pTPCurrent->GroupIndex &= ~Group2Test;
|
|
else
|
|
{
|
|
Error += V.SqrNormXY();
|
|
NE++;
|
|
}
|
|
}
|
|
pTPCurrent++;
|
|
}
|
|
if (NE)
|
|
return Error / NE;
|
|
else
|
|
return 10000000.0f;
|
|
}
|
|
|
|
void IG_ComputeTranslationScale(WORKINGSPACE *WSPC,MATRIX *M,u32 Group2Test)
|
|
{
|
|
Vector TGlob;
|
|
TRACKEDPOINT *pTPCurrent, *pTPLast, *pTPrevious;
|
|
pTPCurrent = WSPC->Current_WSP->TPS;
|
|
pTPLast = pTPCurrent + WSPC->Current_WSP->nTps;
|
|
pTPrevious = WSPC->Previous_WSP->TPS;
|
|
/* Compute General translation */
|
|
TGlob = Vector (0,0,0);
|
|
while (pTPCurrent < pTPLast)
|
|
{
|
|
if (pTPCurrent->GroupIndex & Group2Test)
|
|
{
|
|
TGlob.x += pTPCurrent->SpeedX;
|
|
TGlob.y += pTPCurrent->SpeedY;
|
|
TGlob.z ++;
|
|
}
|
|
pTPCurrent++;
|
|
}
|
|
if (TGlob.z)
|
|
{
|
|
TGlob.z = 1.0f / TGlob.z;
|
|
TGlob.x *= TGlob.z;
|
|
TGlob.y *= TGlob.z;
|
|
|
|
/* Compute General Scale */
|
|
|
|
}
|
|
M->T.x = -TGlob.x;
|
|
M->T.y = -TGlob.y;
|
|
M->T.z = 0.0f;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
void IG_DisplayGroups(WORKINGSPACE *WSPC)
|
|
{
|
|
for (int i = 0 ; i < 32 ; i ++)
|
|
{
|
|
/* Compute BV */
|
|
Vector Max,Min;
|
|
u32 N;
|
|
Max = Vector(-1000000.0f,-1000000.0f,-1000000.0f);
|
|
Min = Vector(1000000.0f,1000000.0f,1000000.0f);
|
|
TRACKEDPOINT *pTPCurrent, *pTPLast, *pTPrevious;
|
|
pTPCurrent = WSPC->Current_WSP->TPS;
|
|
pTPLast = pTPCurrent + WSPC->Current_WSP->nTps;
|
|
pTPrevious = WSPC->Previous_WSP->TPS;
|
|
/* Compute General translation */
|
|
N= 0;
|
|
while (pTPCurrent < pTPLast)
|
|
{
|
|
if ((pTPCurrent->GroupIndex) == (1<<i))
|
|
{
|
|
Vector V,LastV;
|
|
V.x = pTPCurrent->X * (float)WSPC->RENDERMAP->SX / (float)WSPC->DRAWMAP->SX;
|
|
V.y = pTPCurrent->Y * (float)WSPC->RENDERMAP->SY / (float)WSPC->DRAWMAP->SY;
|
|
V.z = 0;
|
|
Max.Max(V);
|
|
Min.Min(V);
|
|
N++;
|
|
}
|
|
pTPCurrent++;
|
|
}
|
|
if (N > 10)
|
|
{
|
|
u32 Color = 0;
|
|
if (i&1) Color |= 0xff;
|
|
if (i&2) Color |= 0xff00;
|
|
if (i&4) Color |= 0xff0000;
|
|
DrawQuad_R(WSPC->RENDERMAP,Min.x,Min.y,Max.x,Max.y,Color,2,0.0f);
|
|
}
|
|
}
|
|
}
|
|
|
|
void IG_Posit(WORKINGSPACE *WSPC)
|
|
{
|
|
MATRIX M;
|
|
float Err;
|
|
for (int i=0;i<32;i++)
|
|
{
|
|
M.Identity();
|
|
IG_ComputeTranslationScale(WSPC,&M,1<<i);
|
|
Err = IG_GetError(WSPC,&M,1<<i);
|
|
IG_GetError(WSPC,&M,1<<i,Err/2);
|
|
IG_ComputeTranslationScale(WSPC,&M,1<<i);
|
|
Err = IG_GetError(WSPC,&M,1<<i);
|
|
IG_GetError(WSPC,&M,1<<i,Err/2);
|
|
TRACKEDPOINT *pTPCurrent, *pTPLast, *pTPrevious;
|
|
pTPCurrent = WSPC->Current_WSP->TPS;
|
|
pTPLast = pTPCurrent + WSPC->Current_WSP->nTps;
|
|
while (pTPCurrent < pTPLast)
|
|
{
|
|
if (pTPCurrent->GroupIndex & (1<<i))
|
|
pTPCurrent->GroupIndex = (1<<i);
|
|
pTPCurrent++;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
/* Try to match and find group of uniform affine transform */
|
|
void INTERP_GROUP(WORKINGSPACE *WSPC)
|
|
{
|
|
TRACKEDPOINT *pTPCurrent, *pTPLast, *pTPrevious;
|
|
u32 SPEED_MAP[64*64];
|
|
memset(SPEED_MAP,0,sizeof(SPEED_MAP));
|
|
float AX,AY,AD;
|
|
AX = AY = AD = 0.0f;
|
|
|
|
pTPCurrent = WSPC->Current_WSP->TPS;
|
|
pTPLast = pTPCurrent + WSPC->Current_WSP->nTps;
|
|
pTPrevious = WSPC->Previous_WSP->TPS;
|
|
while (pTPCurrent < pTPLast)
|
|
{
|
|
if (pTPCurrent->PreviousIndex != 0xffffffff)
|
|
{
|
|
pTPCurrent->GroupIndex = 0;
|
|
pTPCurrent->GroupIndex = 0;
|
|
s32 DX = (pTPCurrent->X - pTPrevious[pTPCurrent->PreviousIndex].X) * 3;
|
|
s32 DY = (pTPCurrent->Y - pTPrevious[pTPCurrent->PreviousIndex].Y) * 3;
|
|
#define S_BLENDER 0.95f
|
|
pTPCurrent->SpeedX = pTPrevious[pTPCurrent->PreviousIndex].SpeedX * S_BLENDER + (float)DX * (1.0f - S_BLENDER);
|
|
pTPCurrent->SpeedY = pTPrevious[pTPCurrent->PreviousIndex].SpeedY * S_BLENDER + (float)DY * (1.0f - S_BLENDER);
|
|
if (pTPCurrent->TrackingTime > 5)
|
|
{
|
|
#define ADRADIUS 14
|
|
if (((pTPCurrent->X > WSPC->DRAWMAP->SX/2 - ADRADIUS) &&(pTPCurrent->X < WSPC->DRAWMAP->SX/2 + ADRADIUS))&&
|
|
((pTPCurrent->Y > WSPC->DRAWMAP->SY/2 - ADRADIUS) &&(pTPCurrent->Y < WSPC->DRAWMAP->SY/2 + ADRADIUS)))
|
|
{
|
|
float Pond = (pTPCurrent->SpeedX * pTPCurrent->SpeedX + pTPCurrent->SpeedY * pTPCurrent->SpeedY);
|
|
Pond = 1.0f;
|
|
|
|
AX+=(s32)pTPCurrent->SpeedX * Pond;
|
|
AY+=(s32)pTPCurrent->SpeedY * Pond;
|
|
AD += Pond;
|
|
}
|
|
float ZSqrL = pTPCurrent->SpeedX * pTPCurrent->SpeedX + pTPCurrent->SpeedY * pTPCurrent->SpeedY;
|
|
if (ZSqrL > 14.0F * 14.0F)
|
|
{
|
|
float Renorm;
|
|
Renorm = 14.0f / sqrtf(ZSqrL);
|
|
pTPCurrent->SpeedX *= Renorm;
|
|
pTPCurrent->SpeedY *= Renorm;
|
|
}
|
|
//if (ZSqrL > 3.0f)
|
|
{
|
|
SPEED_MAP[(s32)pTPCurrent->SpeedX + (s32)pTPCurrent->SpeedY * 64 + 64*32 + 32]++;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
/* Inherit Previous Group */
|
|
pTPCurrent->GroupIndex = -1;//(pTPLast + pTPCurrent->PreviousIndex)->GroupIndex;
|
|
pTPCurrent->SpeedX = pTPCurrent->SpeedY = 0.0f;
|
|
}
|
|
pTPCurrent++;
|
|
}
|
|
|
|
/* Find 4 Most significant points */
|
|
u32 MaxV[4],MaxIX[4],MaxIY[4];
|
|
for (u32 M = 0 ; M < 4 ; M ++)
|
|
{
|
|
MaxV[M] = 0;
|
|
MaxIX[M] = MaxIY[M] = 8;
|
|
for (u32 X = 2 ; X < 62 ; X ++)
|
|
{
|
|
for (u32 Y = 2 ; Y < 62 ; Y ++)
|
|
{
|
|
u32 Color = SPEED_MAP[X + (Y<<6)];
|
|
if ((!(Color >> 24)) && Color)
|
|
{
|
|
u32 LocalT = 0;
|
|
LocalT += SPEED_MAP[(X-1) + ((Y-1)<<6)] & 0xffffff;
|
|
LocalT += SPEED_MAP[(X-0) + ((Y-1)<<6)] & 0xffffff;
|
|
LocalT += SPEED_MAP[(X+1) + ((Y-1)<<6)] & 0xffffff;
|
|
|
|
LocalT += SPEED_MAP[(X-1) + ((Y-0)<<6)] & 0xffffff;
|
|
LocalT += SPEED_MAP[(X-0) + ((Y-0)<<6)] & 0xffffff;
|
|
LocalT += SPEED_MAP[(X+1) + ((Y-0)<<6)] & 0xffffff;
|
|
|
|
LocalT += SPEED_MAP[(X-1) + ((Y+1)<<6)] & 0xffffff;
|
|
LocalT += SPEED_MAP[(X-0) + ((Y+1)<<6)] & 0xffffff;
|
|
LocalT += SPEED_MAP[(X+1) + ((Y+1)<<6)] & 0xffffff;
|
|
|
|
if (LocalT > 2)
|
|
{
|
|
if (LocalT > MaxV[M])
|
|
{
|
|
MaxV[M] = LocalT;
|
|
MaxIX[M] = X;
|
|
MaxIY[M] = Y;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#define R 3
|
|
|
|
if (MaxIX[M] != 8)
|
|
for (u32 X = MaxIX[M]-R ; X < MaxIX[M]+R ; X ++)
|
|
{
|
|
for (u32 Y = MaxIY[M]-R ; Y < MaxIY[M]+R ; Y ++)
|
|
{
|
|
SPEED_MAP[X + (Y<<6)] = 0x1000000 | M;
|
|
SPEED_MAP[MaxIX[M] + (MaxIY[M]<<6)] = 0x1000000 | M;
|
|
}
|
|
}//*/
|
|
}
|
|
|
|
|
|
|
|
for (u32 X = 0 ; X < 128 ; X ++)
|
|
{
|
|
for (u32 Y = 0 ; Y < 128 ; Y ++)
|
|
{
|
|
u32 Color = SPEED_MAP[(X>>1) + 64*(Y>>1)];
|
|
if (Color & 0xff000000)
|
|
{
|
|
Color = 0xff0000 ;
|
|
} else
|
|
{
|
|
Color <<= 4;
|
|
if (Color > 255) Color = 255;
|
|
Color |= Color<<8;
|
|
Color |= Color<<8;
|
|
}
|
|
*(WSPC->RENDERMAP->BASE + X + Y * WSPC->RENDERMAP->PITCH) = Color;
|
|
}
|
|
}
|
|
|
|
pTPCurrent = WSPC->Current_WSP->TPS;
|
|
pTPLast = pTPCurrent + WSPC->Current_WSP->nTps;
|
|
pTPrevious = WSPC->Previous_WSP->TPS;
|
|
|
|
while (pTPCurrent < pTPLast)
|
|
{
|
|
if (pTPCurrent->PreviousIndex != 0xffffffff)
|
|
{
|
|
if (SPEED_MAP[(s32)pTPCurrent->SpeedX + (s32)pTPCurrent->SpeedY * 64 + 64*32 + 32] & 0xff000000)
|
|
{
|
|
pTPCurrent->GroupIndex = 1<<(SPEED_MAP[(s32)pTPCurrent->SpeedX + (s32)pTPCurrent->SpeedY * 64 + 64*32 + 32]&0xffff);
|
|
}
|
|
//else pTPCurrent->GroupIndex = (pTPrevious + pTPCurrent->PreviousIndex)->GroupIndex;
|
|
}
|
|
else
|
|
{
|
|
/* Inherit Previous Group */
|
|
pTPCurrent->GroupIndex = 0;//(pTPLast + pTPCurrent->PreviousIndex)->GroupIndex;
|
|
}
|
|
pTPCurrent++;
|
|
}
|
|
if (!AD) AD = 100000.0f;
|
|
if (AD)
|
|
{
|
|
|
|
AX/=AD * ADRADIUS * ADRADIUS / 8.0f;
|
|
AY/=AD * ADRADIUS * ADRADIUS / 8.0f;
|
|
AX/=(float)WSPC->DRAWMAP->SX / (float)WSPC->RENDERMAP->SX;
|
|
AY/=(float)WSPC->DRAWMAP->SX / (float)WSPC->RENDERMAP->SX;
|
|
|
|
//if (AX * AX + AY * AY > 0.0625f / 2.0f)
|
|
DrawPointFull(WSPC->RENDERMAP,AX * 64.0f + WSPC->RENDERMAP->SX/2,AY * 64.0f + WSPC->RENDERMAP->SY/2,6,0xffff);
|
|
AD = ADRADIUS * (float)WSPC->RENDERMAP->SX / (float)WSPC->DRAWMAP->SX;
|
|
DrawQuad_R(WSPC->RENDERMAP,WSPC->RENDERMAP->SX/2 - AD,WSPC->RENDERMAP->SY/2 - AD,WSPC->RENDERMAP->SX/2 + AD,WSPC->RENDERMAP->SY/2 + AD,0xff00,2,0.0f);
|
|
|
|
}
|
|
//*/
|
|
|
|
|
|
|
|
IG_Posit(WSPC);
|
|
IG_DisplayGroups(WSPC);
|
|
} |