537 lines
No EOL
12 KiB
C++
537 lines
No EOL
12 KiB
C++
#include "ShotTracker.h"
|
||
#include "math.h"
|
||
#include "stdlib.h"
|
||
|
||
#define RadiusMinAcc 1.0f
|
||
|
||
#define NEIBGO(DX,DY) \
|
||
if (*(Pmap+DX+DY*PITCH) & VAnd)\
|
||
{\
|
||
p->x += DX;\
|
||
p->y += DY;\
|
||
*DP += 1.0f;\
|
||
GravityCenter(Pmap+DX+DY*PITCH,PITCH,p,GC,DP);\
|
||
p->x -= DX;\
|
||
p->y -= DY;\
|
||
}
|
||
|
||
#define NEIBGOD(DX,DY) \
|
||
if (*(Pmap+DX+DY*PITCH) & VAnd)\
|
||
{\
|
||
p->x += DX;\
|
||
p->y += DY;\
|
||
*DP += 1.414f;\
|
||
GravityCenter(Pmap+DX+DY*PITCH,PITCH,p,GC,DP);\
|
||
p->x -= DX;\
|
||
p->y -= DY;\
|
||
}
|
||
|
||
void GravityCenter(u8 *Pmap,s32 PITCH,Vector *p,Vector *GC,float *DP)
|
||
{
|
||
u32 VAnd = *Pmap & (2+4);
|
||
*Pmap ^= 2+4;
|
||
*GC += *p;
|
||
NEIBGO(1,0);
|
||
NEIBGO(-1,0);
|
||
NEIBGO(0,1);
|
||
NEIBGO(0,-1);
|
||
NEIBGOD(1,1);
|
||
NEIBGOD(-1,1);
|
||
NEIBGOD(1,-1);
|
||
NEIBGOD(-1,-1);//*/
|
||
|
||
}
|
||
|
||
#define BA_M(DX,DY,FUNC) \
|
||
if (*(Pmap+DX+DY*PITCH) & 4)\
|
||
{\
|
||
p->x += DX;\
|
||
p->y += DY;\
|
||
FUNC(Pmap+DX+DY*PITCH,PITCH,p,GC,Vote,CovMat,pRMAX,pRMIN);\
|
||
p->x -= DX;\
|
||
p->y -= DY;\
|
||
}
|
||
|
||
|
||
typedef struct VSCaleEntry_
|
||
{
|
||
s32 Min,Max;
|
||
} VSCaleEntry;
|
||
VSCaleEntry VTAB[2048];
|
||
s32 HMin,HMax;
|
||
|
||
|
||
void Copvariance(u8 *Pmap,s32 PITCH,Vector *p,Vector *GC,Vector *Vote,Vector *CovMat,float *pRMAX,float *pRMIN)
|
||
{
|
||
*Pmap ^= 4+8;
|
||
Vector V;
|
||
V = *p - *GC;
|
||
if ((s32)p->y < (s32)HMin)
|
||
{
|
||
HMin = (s32)p->y;
|
||
VTAB[(s32)p->y].Max = VTAB[(s32)p->y].Min = (s32)p->x;
|
||
} else
|
||
if ((s32)p->y > (s32)HMax)
|
||
{
|
||
HMax = (s32)p->y;
|
||
VTAB[(s32)p->y].Max = VTAB[(s32)p->y].Min = (s32)p->x;
|
||
} else
|
||
{
|
||
if ((s32)p->x < VTAB[(s32)p->y].Min) VTAB[(s32)p->y].Min = (s32)p->x;
|
||
if ((s32)p->x > VTAB[(s32)p->y].Max) VTAB[(s32)p->y].Max = (s32)p->x;
|
||
}
|
||
CovMat->x += V.x * V.x;
|
||
CovMat->y += V.y * V.y;
|
||
CovMat->z += V.x * V.y;
|
||
V.z = V.SqrNormXY();
|
||
if (V.z > *pRMAX)
|
||
{
|
||
*pRMAX = V.z;
|
||
}
|
||
if (V.z < *pRMIN)
|
||
{
|
||
*pRMIN = V.z;
|
||
}
|
||
if (V.z > GC->z)
|
||
{
|
||
V.z = 1.0f;
|
||
if (V.x * Vote->x + V.y * Vote->y < 0.0f)
|
||
{
|
||
V.x = -V.x;
|
||
V.y = -V.y;
|
||
}
|
||
*Vote += V;
|
||
}
|
||
BA_M(1,0,Copvariance);
|
||
BA_M(-1,0,Copvariance);
|
||
BA_M(0,1,Copvariance);
|
||
BA_M(0,-1,Copvariance);
|
||
BA_M(1,1,Copvariance);
|
||
BA_M(-1,1,Copvariance);
|
||
BA_M(1,-1,Copvariance);
|
||
BA_M(-1,-1,Copvariance);//*/
|
||
}
|
||
|
||
#if 1
|
||
#define DEBUGSource(a,b,c,d,e,f)
|
||
#else
|
||
#define BA_MD(DX,DY,FUNC) \
|
||
if (*(Pmap+DX+DY*PITCH) & AND)\
|
||
{\
|
||
if (PmapD) FUNC(AND,Color,Pmap+DX+DY*PITCH,PITCH,PmapD+DX+DY*PITCH,PITCHD);\
|
||
else FUNC(AND,Color,Pmap+DX+DY*PITCH,PITCH,NULL,PITCHD);\
|
||
}
|
||
void DEBUGSource(u32 AND,u32 Color,u8 *Pmap,s32 PITCH,s32 *PmapD,s32 PITCHD)
|
||
{
|
||
*Pmap ^= 8+16;
|
||
if (PmapD) *PmapD = Color;
|
||
BA_MD(1,0,DEBUGSource);
|
||
BA_MD(-1,0,DEBUGSource);
|
||
BA_MD(0,1,DEBUGSource);
|
||
BA_MD(0,-1,DEBUGSource);
|
||
BA_MD(1,1,DEBUGSource);
|
||
BA_MD(-1,1,DEBUGSource);
|
||
BA_MD(1,-1,DEBUGSource);
|
||
BA_MD(-1,-1,DEBUGSource);//*/
|
||
}
|
||
#endif
|
||
|
||
|
||
//#define RadiusCompatible(a,b) 1
|
||
|
||
u32 RadiusCompatible(float R1,float R2)
|
||
{
|
||
if (R1 > R2)
|
||
{
|
||
float RX = R1;
|
||
R1 = R2;
|
||
R2 = RX;
|
||
}
|
||
if (R2 > 2.0f)
|
||
{
|
||
R1 /= R2;
|
||
if ((R1 > 0.1f) && (R1 < 0.7f))
|
||
return 1;
|
||
}
|
||
return 0;
|
||
}
|
||
//*/
|
||
|
||
static int __cdecl Grav_Compare(const void *elem1, const void *elem2 )
|
||
{
|
||
if (((GravC *)elem2)->Coord.x < ((GravC *)elem1)->Coord.x)
|
||
return -1;
|
||
else
|
||
return 1;
|
||
}
|
||
|
||
|
||
|
||
u32 SHOTS_CLASS::SHOTS_DETECT(MAP *pSrc,/*OUT*/GravC *OutpL,u8 *pTempBuffer)
|
||
{
|
||
MAP8 MHS;
|
||
u32 DetectedNumber = 0;
|
||
GravC *OutpLBase = OutpL;
|
||
MAP_2_MAP8(pSrc,&MHS,pTempBuffer);
|
||
s32 *PPs;
|
||
u8 *PPd,*PPl,*PPll,*PPDeriv;
|
||
MAP8_BLUR8_H_Inplace(&MHS,3);
|
||
MAP8_BLUR8_V_Inplace(&MHS,3);
|
||
/* 1 cut in 2 */
|
||
PPs = pSrc->GetBase() + pSrc->PITCH + 1;
|
||
PPd = MHS.GetBase() + MHS.PITCH + 1;
|
||
PPl = MHS.GetBase() + pSrc->PITCH * pSrc->SY;
|
||
PPDeriv = MHS.GetBase()+1;
|
||
//Lim -= Lim>>2;
|
||
while (PPd < PPl)
|
||
{
|
||
PPll = PPd + pSrc->SX;
|
||
while(PPd < PPll)
|
||
{
|
||
u32 VA;
|
||
u32 VB;
|
||
VB = (u32 )*(PPd);
|
||
*PPd = 1;
|
||
|
||
VA = (*PPs & 0xff00)>>8;
|
||
if (VA < VB) *PPd &= ~1;
|
||
|
||
VA = *(PPd - 1) ^ *(PPd);
|
||
VA |= *(PPDeriv) ^ *(PPd);
|
||
VA <<= 1;
|
||
if (VA & 2)
|
||
{
|
||
s32 dERIVATIV,A,B;
|
||
A = (((*(PPs) & 0xff00)>>8) - ((*(PPs-pSrc->PITCH-1) & 0xff00)>>8));
|
||
B = (((*(PPs-pSrc->PITCH) & 0xff00)>>8) - ((*(PPs-1) & 0xff00)>>8));
|
||
dERIVATIV = A * A + B * B;
|
||
|
||
*PPDeriv |= VA & (2+32);
|
||
|
||
if (dERIVATIV < 12*12)
|
||
*PPDeriv = 0;
|
||
} else
|
||
*PPDeriv = 0;
|
||
|
||
|
||
PPd ++;
|
||
PPs ++;
|
||
PPDeriv++;
|
||
}
|
||
PPs += pSrc->PITCH - pSrc->SX;
|
||
}
|
||
|
||
/* Little cleanup */
|
||
PPd = MHS.GetBase() + 1;
|
||
PPl = MHS.GetBase() + pSrc->PITCH * pSrc->SY - MHS.PITCH;
|
||
while (PPd < PPl)
|
||
{
|
||
*(PPd) &= ((*(PPd-1) & *(PPd+MHS.PITCH)) ^2 ) & 2;
|
||
PPd ++;
|
||
}
|
||
|
||
|
||
SET_BORDER8(&MHS,2,0);
|
||
/*
|
||
|
||
DARKER(pSrc);
|
||
|
||
PPs = pSrc->GetBase();
|
||
PPd = MHS.GetBase();
|
||
PPl = PPd + pSrc->PITCH * pSrc->SY;
|
||
while (PPd < PPl)
|
||
{
|
||
if (*PPd & 2) *PPs = 0xff00;
|
||
if (*PPd & 32) *PPs |= 0xff;
|
||
PPd ++;
|
||
PPs ++;
|
||
}
|
||
return;
|
||
//*/
|
||
|
||
|
||
for (u32 Y = 0 ; Y < MHS.SY;Y++)
|
||
{
|
||
u8 *PBase = MHS.GetBase() + Y * MHS.PITCH;
|
||
for (u32 X = 0 ; X < MHS.SX;X++,PBase++)
|
||
{
|
||
if (*PBase & 2)
|
||
{
|
||
float Radius,SampleNum ,OoSampleNum ;
|
||
Vector BaseX = Vector(((float)X)+0.5f,((float)Y)+0.5f,1.0f);
|
||
Vector BaseRES = Vector(0,0,0);
|
||
Radius = 0;
|
||
GravityCenter(PBase,MHS.PITCH,&BaseX,&BaseRES,&Radius);
|
||
if (BaseRES.z > 5.0f)
|
||
{
|
||
Vector Vote = Vector(0,0,0);
|
||
Vector Cov = Vector(0,0,0);
|
||
SampleNum = BaseRES.z;
|
||
OoSampleNum = 1.0f / SampleNum;
|
||
Radius = Radius / (2.0f * 3.1415927f);
|
||
BaseRES *= OoSampleNum;
|
||
BaseRES.z = Radius * Radius;
|
||
float RadiusMax = 0.0f;
|
||
float RadiusMin = 10000000.0f;
|
||
float Surface = 0.0f;
|
||
|
||
HMin = 100000;
|
||
HMax = -100000;
|
||
|
||
Copvariance(PBase,MHS.PITCH,&BaseX,&BaseRES,&Vote,&Cov,&RadiusMax,&RadiusMin);
|
||
if (RadiusMin>=RadiusMinAcc)
|
||
{
|
||
float SurfaceC,Coef;
|
||
while (HMin < HMax)
|
||
{
|
||
Surface += VTAB[HMin].Max - VTAB[HMin].Min;
|
||
HMin++;
|
||
}
|
||
|
||
RadiusMax = sqrtf(RadiusMax);
|
||
RadiusMin = sqrtf(RadiusMin);
|
||
Surface = abs(Surface);
|
||
SurfaceC = 3.141592765f * RadiusMax * RadiusMin;
|
||
if (Surface > SurfaceC)
|
||
{
|
||
DEBUGSource(8,0xff,PBase,MHS.PITCH,pSrc->GetBase() + X + pSrc->PITCH * Y,pSrc->PITCH);
|
||
DEBUGSource(16,0,PBase,MHS.PITCH,NULL,0);
|
||
SurfaceC = 3.141592765f * (RadiusMax+1.0f) * (RadiusMin+1.0f);
|
||
Coef = SurfaceC / Surface;
|
||
} else {
|
||
DEBUGSource(8,0xff,PBase,MHS.PITCH,pSrc->GetBase() + X + pSrc->PITCH * Y,pSrc->PITCH);
|
||
DEBUGSource(16,0,PBase,MHS.PITCH,NULL,0);
|
||
SurfaceC = 3.141592765f * (RadiusMax-1.0f) * (RadiusMin-1.0f);
|
||
Coef = Surface / SurfaceC ;
|
||
}
|
||
|
||
if (Coef > 1.0f)
|
||
{
|
||
DEBUGSource(8,0xff00,PBase,MHS.PITCH,pSrc->GetBase() + X + pSrc->PITCH * Y,pSrc->PITCH);
|
||
DEBUGSource(16,0,PBase,MHS.PITCH,NULL,0);
|
||
|
||
|
||
/*
|
||
---------EIGENVALUES--------------------------------------------------------------------------
|
||
|
||
x y
|
||
x y
|
||
x x x<> z DET => x<>y<EFBFBD> - (z<>)
|
||
y y z y<>
|
||
|
||
|
||
DET |<7C>-x<> z | = 0 => (<28>-x<>)(<28>-y<>) - z<> = 0
|
||
|z <09>-y<>|
|
||
<09><> - <20>x<EFBFBD> - <20>y<EFBFBD> + (x<>y<EFBFBD> - z<>) = 0
|
||
<09><> - <20>(x<>+y<>) + (x<>y<EFBFBD> - z<>) = 0
|
||
|
||
DELTA = (x<>+y<>)<29> - 4*(x<>y<EFBFBD> - z<>)
|
||
<09>1 = ((x<>+y<>) - root(DELTA)) / 2
|
||
<09>2 = ((x<>+y<>) + root(DELTA)) / 2
|
||
|
||
---------EIGENVECTORS--------------------------------------------------------------------------
|
||
(<28>1(I) - CovMat)V1 = 0
|
||
|
||
|<7C>1-x<> z ||V1x| = 0
|
||
|z <09>1-y<>||V1y|
|
||
|
||
=> (<28>1-x<>)V1x + (z)V1y = 0
|
||
=> V1y = V1x * -(<28>1-x<>)/z
|
||
|
||
-----------------------------------------------------------------------------------------------
|
||
|
||
---------DIM 4 CROSS PRODUCT-------------------------------------------------------------------
|
||
|
||
|
||
xy'z" + x'y"z + x"yz' + xy"z' - x'yz" - x"y'z = 1
|
||
|
||
z(x'y" - x"y') + y(x"z' - x'z") + x(y'z" - y"z') = 1
|
||
|
||
x(y') + y(-x') = 1
|
||
|
||
|
||
|
||
|
||
zz' + yy' + xx'
|
||
|
||
|a b c|
|
||
DET |d e f| = aei - afh - bdi + cdh + bfg - ceg = 0
|
||
|g h i|
|
||
|
||
|a b c|
|
||
DET |b e f| = aei - af<61> - ib<69> - ec<65> + cbf + bfc = 0
|
||
|c f i|
|
||
|
||
(<28>-a)(<28>-e)(<28>-i) - (<28>-a)f<> - (<28>-i)b<> - (<28>-e)c<> + cbf + bfc
|
||
(<28>-a)(<28>-e)(<28>-i) - <20>f<EFBFBD>-af<61> - <20>b<EFBFBD>-ib<69> - <20>c<EFBFBD>-ec<65> = K
|
||
(<28>-a)(<28>-e)(<28>-i) - <20>f<EFBFBD> - <20>b<EFBFBD> - <20>c<EFBFBD> = K
|
||
(<28>-a)(<28>-e)(<28>-i) - <20>(f<> + b<> + c<>) = K2
|
||
(<28>-a)(<28>-e)(<28>-i) = <20>K3 - K2
|
||
|
||
|
||
|
||
=> ?
|
||
|
||
-----------------------------------------------------------------------------------------------
|
||
|
||
*/
|
||
Cov *= OoSampleNum;
|
||
/* reduction */
|
||
|
||
float DElta = (Cov.x + Cov.y)*(Cov.x + Cov.y) - 4.0f*(Cov.x * Cov.y - Cov.z * Cov.z);
|
||
OutpL->Mu1 = OutpL->Mu2 = 0;
|
||
OutpL->VMu1 = Vector(0,0,0);
|
||
if (DElta > 0.0f)
|
||
{
|
||
OutpL->Mu1 = ((((Cov.x+Cov.y) - sqrt(DElta)) / 2.0f));
|
||
OutpL->Mu2 = ((((Cov.x+Cov.y) + sqrt(DElta)) / 2.0f));
|
||
} else
|
||
if (DElta == 0.0f)
|
||
{
|
||
OutpL->Mu1 = (Cov.x+Cov.y);
|
||
OutpL->Mu2 = (Cov.x+Cov.y);
|
||
}
|
||
if (OutpL->Mu2 > OutpL->Mu1)
|
||
{
|
||
float Swap;
|
||
Swap = OutpL->Mu2;
|
||
OutpL->Mu2 = OutpL->Mu1;
|
||
OutpL->Mu1 = Swap;
|
||
}
|
||
if (Cov.z)
|
||
{
|
||
if (abs((OutpL->Mu1-Cov.x)/Cov.z) < abs((OutpL->Mu1-Cov.y)/Cov.z))
|
||
{
|
||
OutpL->VMu1.x = RadiusMax;
|
||
OutpL->VMu1.y = RadiusMax * (OutpL->Mu1-Cov.x)/Cov.z;
|
||
} else
|
||
{
|
||
OutpL->VMu1.y = RadiusMax;
|
||
OutpL->VMu1.x = RadiusMax * (OutpL->Mu1-Cov.y)/Cov.z;
|
||
}
|
||
OutpL->VMu1.Normalize();
|
||
OutpL->VMu1 *= RadiusMax;
|
||
}
|
||
|
||
|
||
if (Vote.z)
|
||
{
|
||
Vote *= 1.0f / Vote.z;
|
||
Vote.z = 0.0f;
|
||
if (Vote.SqrNormXY())
|
||
{
|
||
Vote.Normalize();
|
||
Vote *= RadiusMax;
|
||
} else
|
||
{
|
||
Vote.x = RadiusMax;
|
||
Vote.y = 0.0f;
|
||
}
|
||
} else
|
||
{
|
||
Vote.x = RadiusMax;
|
||
Vote.y = 0.0f;
|
||
}
|
||
{
|
||
BaseRES.z = 1.0f;
|
||
OutpL->Coord = BaseRES;
|
||
OutpL->Radius = RadiusMax;
|
||
OutpL->BigAxis = OutpL->VMu1;
|
||
OutpL->BigAxis.z = RadiusMin*1.05f / RadiusMax;
|
||
if (DetectedNumber < MAX_DETECTED_SHOT)
|
||
{
|
||
DetectedNumber++;
|
||
OutpL++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
qsort( OutpLBase, DetectedNumber , sizeof(GravC), &Grav_Compare);
|
||
/* then merg them */
|
||
GravC *pGSrc,*pGDst,*pGLast;
|
||
pGDst = pGSrc = OutpLBase;
|
||
pGLast = pGDst + DetectedNumber;
|
||
while (pGSrc < pGLast)
|
||
{
|
||
if (pGSrc->Coord.z)
|
||
{
|
||
pGDst = pGSrc + 1;
|
||
while (pGDst < pGLast)
|
||
{
|
||
if (pGDst->Coord.x - pGSrc->Coord.x > 5)
|
||
pGDst = pGLast;
|
||
else
|
||
if ((pGDst->Coord.z) && RadiusCompatible(pGDst->Radius,pGSrc->Radius))
|
||
{
|
||
Vector Dist,BA;
|
||
float Lenght;
|
||
BA = pGDst->BigAxis;
|
||
BA.Normalize();
|
||
Dist = (pGDst->Coord - pGSrc->Coord);
|
||
BA *= Dist * BA;
|
||
Lenght = Dist.NormXY();
|
||
if ((BA.SqrNormXY() < 50.0f) &&
|
||
(
|
||
((pGSrc->Radius * pGSrc->BigAxis.z) > Lenght * 0.5f) &&
|
||
((pGDst->Radius * pGDst->BigAxis.z) > Lenght * 0.5f)
|
||
) )//*/
|
||
{
|
||
/* Merge */
|
||
pGSrc->PerspectiveDir = pGSrc->Coord - pGDst->Coord;
|
||
pGSrc->Coord.z += pGDst->Coord.z;
|
||
pGDst->Coord.z = 0;
|
||
if (pGDst->Radius > pGSrc->Radius)
|
||
{
|
||
pGSrc->Radius = pGDst->Radius;
|
||
pGSrc->BigAxis = pGDst->BigAxis;
|
||
pGSrc->VMu1 = pGDst->VMu1;
|
||
pGSrc->PerspectiveDir *= -1.0f;
|
||
pGSrc->Coord2D = pGDst->Coord;
|
||
} else
|
||
{
|
||
pGSrc->Coord2D = pGSrc->Coord;
|
||
pGSrc->Coord.x = pGDst->Coord.x;
|
||
pGSrc->Coord.y = pGDst->Coord.y;
|
||
}
|
||
|
||
pGDst->Coord.z = 0.0f;
|
||
pGSrc->PerspectiveDir.z = 0.0f;
|
||
pGSrc->PerspectiveDirSmoothed = pGSrc->PerspectiveDir;
|
||
pGSrc->TrackedTime = 0;
|
||
}
|
||
}
|
||
pGDst++;
|
||
}
|
||
}
|
||
pGSrc++;
|
||
}
|
||
|
||
MAP_2_MAP8(pSrc,&MHS,pTempBuffer);
|
||
GravC *PBase,*PConst,*PBEnd;
|
||
PConst = PBase = OutpLBase;
|
||
PBEnd = PBase + DetectedNumber;
|
||
DetectedNumber = 0;
|
||
while (PBase < PBEnd)
|
||
{
|
||
if (PBase->Coord.z >= 1.9f)
|
||
{
|
||
// Affichage des ellipses reconnues
|
||
//DrawEllipse(pSrc,PBase->Coord2D.x,PBase->Coord2D.y,&PBase->BigAxis,PBase->BigAxis.z,0xFF00FF);
|
||
|
||
// Appel pour le calcul du code.
|
||
if (SHOT_ComputeMatrix(PBase,&MHS))
|
||
{
|
||
DetectedNumber++;
|
||
*(PConst++) = *(PBase);
|
||
PBase->ContourError = 1.0f;
|
||
}
|
||
}
|
||
PBase++;
|
||
}
|
||
|
||
return DetectedNumber;
|
||
} |