dreyndev/JD2022-TU1
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions
JD2022-TU1/main/extern/Camcam/Fast2D/Fast2DButton.cpp

771 lines
26 KiB
C++
Raw Blame History

#include "Fast2D.h"
#include <math.h>
#include <stdio.h>
#include "memory.h"
//#pragma optimize("",off)
s32 Copied = 0;
/*************************************************************************************************************/
/*************************************************************************************************************/
/********* BEGIN RANSAC ****************************************************************************************/
/*************************************************************************************************************/
/*************************************************************************************************************/
u32 CFast2D::FB_init = 1;
Fast2DFeatureBox CFast2D::FB_Table[MAX_FEATBOX];
void DRawTEXT_Soft_B(u8 *pTEXT,u32 *pBase,u32 X, u32 Y, s32 Pitch , u32 Height , u32 Color);
u32 CFast2D::EvaluateNumber(u8 *Valid , u32 ulNumber)
{
u32 Result = 0,Counter = ulNumber;
while (Counter--){ if (*(Valid++) == 1) Result++;}
return Result;
}
/* Compute Error from estimation matrix by projecting Lastpos and compare it with Newpos */
float CFast2D::EvaluateError(u8 *Valid , u32 ulNumber, const Vector *Lastpos , const Vector *Newpos , MATRIX *Matrix23 , float Threshold)
{
float Error = 0.0f , Pond = 0.0f;
u32 Counter = ulNumber;
while (Counter--)
{
if (*(Valid))
{
Vector Result;
Result = *Matrix23 * *Lastpos;
float ErrorLocal = (*Newpos - Result).NormXY();
//Newpos->z = 1.0f / (ErrorLocal+1.0f);
if (ErrorLocal < Threshold)
{
if (*(Valid) == 1)
{
Error += ErrorLocal;
Pond++;
}//*/
*(Valid) = 1;
} else
*(Valid) = 2;
}
Valid++;
Newpos++;
Lastpos++;
}
if (Pond)
return Error/(float)Pond;
else
return 0.0f;
}
// Compute gravity center of the set by pondere it with Z ( = Matching quality )
u32 CFast2D::ComputeGCXY(u8 *Valid , u32 ulNumber , const Vector *Positions , Vector *GCenter )
{
*GCenter = Vector(0,0,0);
u32 Counter = ulNumber;
while (Counter--)
{
if (*(Valid++) == 1)
{
GCenter->x += Positions->x;
GCenter->y += Positions->y;
GCenter->z ++;
}
Positions++;
}
if (GCenter->z)
{
GCenter->x /= GCenter->z;
GCenter->y /= GCenter->z;
return 1;
} else
return 0;
}
/*
Compute scale factor from GravityCenter 1 & 2
*/
float CFast2D::ComputeGCScale(u8 *Valid , u32 ulNumber , const Vector *Lastpos , const Vector *Newpos , Vector *LastGC, Vector *NewGC , float *Scale , float *Angle)
{
float R1 = 0.0f, R2 = 0.0f,SinusAlpha = 0.0f , Pond = 0.0f , CosAlpha = 0.0f;
u32 Counter1 = ulNumber,RES = 0;
while (Counter1--)
{
if (*(Valid++) == 1)
{
float LastNorm = (*Lastpos - *LastGC).NormXY();
float NewNorm = (*Newpos - *NewGC).NormXY();
if (LastNorm * NewNorm > 2.0f*2.0f)
{
R1 += LastNorm ;
R2 += NewNorm ;
Vector V1,V2;
V1 = (*Lastpos - *LastGC);
V2 = (*Newpos - *NewGC);
V1.z = V2.z = 0.0f;
V1 *= NewNorm / V1.NormXY();
V2 *= 1.0f / V2.NormXY();
SinusAlpha += Newpos->z * (V1 ^ V2).z;
CosAlpha += Newpos->z * (V1 * V2);
Pond += Newpos->z * NewNorm/** NewNorm * LastNorm*/;
RES++;
}
}
Newpos++;
Lastpos++;
}
Valid-=ulNumber;
*Scale = 1.0f;
*Angle = 0.0f;
if (R1 * R2 != 0.0f)
{
//*Scale = R2 / R1 ;
if (Pond)
{
*Angle = asinf(SinusAlpha / Pond);
if (CosAlpha < 0)
*Angle = 3.1415927f - *Angle;
if (*Angle > 3.1415927f / 4) *Angle = 3.1415927f / 4;
if (*Angle < -3.1415927f / 4) *Angle = -3.1415927f / 4;
}
/* Compute Ratio Y */
}
return Pond;
}
void CFast2D::ComputeGCXYScale(u8 *Valid , u32 ulNumber , const Vector *Lastpos , const Vector *Newpos , Vector *YAxis,float *XScale,float *YScale)
{
float YPreviousSQR = 0,YPrevious = 0;
float YCurrentSQR = 0,YCurrent = 0;
float XPreviousSQR = 0,XPrevious = 0;
float XCurrentSQR = 0,XCurrent = 0;
float Pond = 0.0f ;
u32 Counter1 = ulNumber,RES = 0;
while (Counter1--)
{
if (*(Valid++) == 1)
{
float P = 1.0f;
float DPNew = (Newpos->x * YAxis->x + Newpos->y * YAxis->y) ;
YCurrentSQR += DPNew * DPNew * P ;
YCurrent += DPNew * P;
float DPOld = (Lastpos->x * YAxis->x + Lastpos->y * YAxis->y) ;
YPreviousSQR += DPOld * DPOld* P ;
YPrevious += DPOld* P ;
DPNew = (Newpos->x * YAxis->y - Newpos->y * YAxis->x) ;
XCurrentSQR += DPNew * DPNew* P ;
XCurrent += DPNew* P ;
DPOld = (Lastpos->x * YAxis->y - Lastpos->y * YAxis->x) ;
XPreviousSQR += DPOld * DPOld* P ;
XPrevious += DPOld* P ;
Pond += P ;
RES++;
}
Newpos++;
Lastpos++;
}
float OoP = 1.0f/Pond;
YPrevious = sqrtf(OoP * YPreviousSQR - OoP * OoP * YPrevious * YPrevious );
YCurrent = sqrtf(OoP * YCurrentSQR - OoP * OoP * YCurrent * YCurrent );
XPrevious = sqrtf(OoP * XPreviousSQR - OoP * OoP * XPrevious * XPrevious );
XCurrent = sqrtf(OoP * XCurrentSQR - OoP * OoP * XCurrent * XCurrent );
*YScale = YCurrent / YPrevious ;
*XScale = XCurrent / XPrevious ;
#define LIMITSCALE 0.25
if (*YScale < 0.25f) *YScale = 0.25f;
if (*YScale > 4.f) *YScale = 4.f;
if (*XScale < 0.25f) *XScale = 0.25f;
if (*XScale > 4.f) *XScale = 4.f;
}
/*
Try to find the best projection matrix as possible.
*/
float CFast2D::FeatureBoxRansac(u8 *Valid , u32 ulNumber , const Vector *LastPosition , const Vector *NewPosition , Vector *SpeedScaleDest , float *Rotation,Fast2DFeatureBox *pFB ,float LowLimit)
{
MATRIX Estim,Estim2,Covariance,InvCovariance;
Vector OldGC,NewGC,Speed,NewT;
float ErrorEst,Sclae,Rot,ErrorFactor;
u32 Converge = 10;
Sclae = 1.0f;
Rot = 0.0f;
ErrorFactor = 1.0f;
u32 FullMatrixMode = pFB->FixeMode * 3;
#define DOOMA LastPosition
#define DOOMB NewPosition
while (Converge--)
{
if (EvaluateNumber(Valid,ulNumber) < ulNumber * 0.75f) FullMatrixMode = 0;
/* 1 Evaluate a Speed */
ComputeGCXY(Valid ,ulNumber, DOOMA , &OldGC );
ComputeGCXY(Valid ,ulNumber, DOOMB , &NewGC );
if (FullMatrixMode)
{
/* Full matrix evaluation */
memset(&Covariance,0,sizeof(Covariance));
float RealNum = 0.0f;
Vector I = Vector(0,0,0);
Vector J = Vector(0,0,0);
Vector Res = Vector(0,0,0);
for (u32 i = 0; i < ulNumber ; i++)
{
if (Valid[i] == 1)
{
Vector Last,New;
Last = DOOMA[i] - OldGC;
New = DOOMB[i] - NewGC;
Covariance.I.x += Last.x * Last.x;
Covariance.I.y += Last.x * Last.y;
Covariance.J.y += Last.y * Last.y;
I.x += Last.x * New.x;
I.y += Last.y * New.x;
J.x += Last.x * New.y;
J.y += Last.y * New.y;
RealNum+=1.0f;
}
}
RealNum = 1.0f / RealNum;
Covariance.I *= RealNum;
Covariance.J *= RealNum;
Covariance.J.x = Covariance.I.y;
I *= RealNum;
J *= RealNum;
Covariance.K.z = 1;
Covariance.Inverse(InvCovariance);
I = InvCovariance * I;
J = InvCovariance * J;
Estim.Identity();
Estim2.Identity();
Estim2.T = OldGC * -1;
I -= Vector(1,0,0);
J -= Vector(0,1,0);
float LocalN = I.NormXY() + J.NormXY();
#define Smooth 0.25f
if (LocalN < Smooth)
{
LocalN = 15*LocalN / Smooth;
LocalN += 1;
LocalN /= 16;
I = I * LocalN ;
J = J * LocalN ;
}
I += Vector(1,0,0);
J += Vector(0,1,0);
Estim.I.x = I.x;
Estim.J.x = I.y;
Estim.I.y = J.x;
Estim.J.y = J.y;
Estim.T = NewGC;
Estim.T.z = 0;
Estim2 *= Estim;
Estim =Estim2 ;
} else {
ComputeGCScale(Valid , ulNumber,DOOMA, DOOMB, &OldGC , &NewGC , &Sclae, &Rot);
ComputeGCXYScale(Valid , ulNumber , DOOMA, DOOMB, &Vector(sinf(Rot),cosf(Rot),0),&pFB->XScale,&pFB->YScale);
if (pFB->ComputeRotation == 0) Rot = 0.0f;
if (pFB->ComputeScale == 0) pFB->XScale = pFB->YScale = 1.0f;
if (!pFB->FixeMode)
{
pFB->XScale = (pFB->YScale + pFB->XScale) / 2.0f;
pFB->YScale = pFB->XScale ;
pFB->XScale = pFB->YScale = 1.0f;
}
Estim2.Identity();
Estim2.T = OldGC * -1;
Estim.Identity();
Estim.I.x = pFB->XScale;
Estim.J.y = pFB->YScale;
Estim.RotateAround_K(Rot);
Estim.T = NewGC ;
Estim.T.z = 0.0f;//*/
Estim2 *= Estim;
Estim =Estim2 ;
}
/* 4 Decimate & Reinsert */
ErrorEst = EvaluateError(Valid , ulNumber , DOOMA, DOOMB, &Estim);
EvaluateError(Valid , ulNumber , DOOMA, DOOMB, &Estim , ErrorEst * ErrorFactor + LowLimit );//*/
ErrorFactor *= 1.0f/2.0f;
}
Speed = NewGC - OldGC;
SpeedScaleDest->x = Speed.x;
SpeedScaleDest->y = Speed.y;
SpeedScaleDest->z = (pFB->XScale + pFB->YScale) / 2;
*Rotation = Rot;//*/
pFB->LastTransforMatrix = Estim;
return (float) EvaluateNumber(Valid,ulNumber);
}
/*************************************************************************************************************/
/*************************************************************************************************************/
/********* END RANSAC ****************************************************************************************/
/*************************************************************************************************************/
/*************************************************************************************************************/
void CFast2D::FeatureBoxUpdateOne(u32 Index)
{
if (FB_init) return ;
{
FB_Table[Index].ulNumberOfParticules = 0;
s32 Counter = FB_Table[Index].DXSize * FB_Table[Index].DYSize;
while (Counter--)
{
s32 cX,cY;
cX = (float)(Counter % FB_Table[Index].DXSize) * (float)FB_Table[Index].W / (float)FB_Table[Index].DXSize + FB_Table[Index].X - (float)FB_Table[Index].W/2 + (float)FB_Table[Index].W / ((float)FB_Table[Index].DXSize * 2.0f);
cY = (float)(Counter / FB_Table[Index].DXSize) * (float)FB_Table[Index].H / (float)FB_Table[Index].DYSize + FB_Table[Index].Y - (float)FB_Table[Index].H/2 + (float)FB_Table[Index].H / ((float)FB_Table[Index].DYSize * 2.0f);
FB_Table[Index].FirstPos[FB_Table[Index].ulNumberOfParticules].x = cX;
FB_Table[Index].FirstPos[FB_Table[Index].ulNumberOfParticules].y = cY;
FB_Table[Index].FirstPos[FB_Table[Index].ulNumberOfParticules].z = 1.0f;
if (FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules] == -1)
FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules] = New(cX , cY);
else
{
Set(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules],&FB_Table[Index].FirstPos[FB_Table[Index].ulNumberOfParticules]);
}
UpdateData(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules]);
/* Max Optimize */
u32 FalsePoint = 0;
/* float Max = GetNorm(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules], 0);
// int CounterIter = ((int)(0.5f * (float)FB_Table[Index].W / (float)FB_Table[Index].DXSize))<<2;
Vector FinalPos = FB_Table[Index].FirstPos[FB_Table[Index].ulNumberOfParticules];
#define RadiusSearch 6*1
for(int X = -RadiusSearch + 1 ; X < RadiusSearch ; X++)
for(int Y = -RadiusSearch + 1 ; Y < RadiusSearch ; Y++)
{
Vector Srch = FB_Table[Index].FirstPos[FB_Table[Index].ulNumberOfParticules];
Srch.x += X;
Srch.y += Y;
Set(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules],&Srch);
UpdateData(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules]);
if (Max >= GetNorm(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules], 0))
{
FinalPos = Srch;
Max = GetNorm(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules], 0);
FalsePoint = 0;
}
}
FB_Table[Index].FirstPos[FB_Table[Index].ulNumberOfParticules] = FinalPos;
Set(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules],&FB_Table[Index].FirstPos[FB_Table[Index].ulNumberOfParticules]);
UpdateData(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules]);//*/
ComputeTrack(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules],2);
if ((FalsePoint) || (GetError(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules]) <= 0.6))
{
Destroy(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules]);
FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules] = -1;
} else
{
FB_Table[Index].ParticlesPonderation[FB_Table[Index].ulNumberOfParticules] = 1.0f / GetNorm(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules], 0);
Set(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules],&FB_Table[Index].FirstPos[FB_Table[Index].ulNumberOfParticules]);
FB_Table[Index].ulNumberOfParticules ++;
}
}
float ParticlesPonderationSumm = 0;
for (int i = 0 ; i < FB_Table[Index].ulNumberOfParticules ; i ++)
ParticlesPonderationSumm += FB_Table[Index].ParticlesPonderation[i];
ParticlesPonderationSumm = 1.0f / ParticlesPonderationSumm;
for (int i = 0 ; i < FB_Table[Index].ulNumberOfParticules ; i ++)
FB_Table[Index].ParticlesPonderation[i] *= ParticlesPonderationSumm;
}
}
u32 CFast2D::DestroyFeatureBox(u32 Index)
{
if (Index > MAX_FEATBOX) return 0;
s32 Counter = MAX_QFB;
while (Counter--) Destroy(FB_Table[Index].TrackerID[Counter]);
return 1;
}
u32 CFast2D::CreateFeatureBox(u32 Index, s32 X,s32 Y,s32 W, s32 H, float Rotation)
{
if (FB_init)
{
memset(FB_Table,0,sizeof(FB_Table));
FB_init = 0;
}
if (Index > MAX_FEATBOX) return 0;
if (FB_Table[Index].IsInit) DestroyFeatureBox(Index);
memset(FB_Table[Index].TrackerID,-1,sizeof(FB_Table[Index].TrackerID));
FB_Table[Index].IsInit = 0;//*/
FB_Table[Index].X = X + abs(W) / 2;
FB_Table[Index].Y = Y + abs(H) / 2;
FB_Table[Index].SearchX = FB_Table[Index].SearchY = 0.0f;
FB_Table[Index].W = abs(W) ;
FB_Table[Index].H = abs(H) ;
FB_Table[Index].DXSize = W / DISC_PIXEL;
FB_Table[Index].DYSize = H / DISC_PIXEL;
FB_Table[Index].BigSearchBase = 1;
if (FB_Table[Index].DXSize < 3) FB_Table[Index].DXSize = 3;
if (FB_Table[Index].DYSize < 3) FB_Table[Index].DYSize = 3;
if (FB_Table[Index].DXSize > MAX_QFB_DX) FB_Table[Index].DXSize = MAX_QFB_DX;
if (FB_Table[Index].DYSize > MAX_QFB_DY) FB_Table[Index].DYSize = MAX_QFB_DY;
if (!FB_Table[Index].IsInit)
{
s32 Counter = FB_Table[Index].DXSize * FB_Table[Index].DYSize;
while (Counter--) FB_Table[Index].TrackerID[Counter] = -1;
}
FeatureBoxUpdateOne(Index);
FB_Table[Index].IsInit = 1;
FB_Table[Index].ComputeScale = 1;
FB_Table[Index].ComputeRotation = 1;
FB_Table[Index].LearningMode = 0;
FB_Table[Index].LastTransforMatrix.Identity();
FB_Table[Index].PertinenceLimiteDown = PertinenceLimiteDownDefault;
FB_Table[Index].DontMoveMode = 0;
FB_Table[Index].PausedMode = 0;
FB_Table[Index].MINIMALERROR = MINIMALERRORDEFAULT;
FB_Table[Index].TOLERANCE = 0;
return 1;
}
void CFast2D::FeatureBoxSetParam(u32 Index,char *String , float Value)
{
u32 P = ((u32)String[0]<<0) | ((u32)String[1]<<8) | ((u32)String[2]<<16) | ((u32)String[3]<<24);
if (Index > MAX_FEATBOX) return;
if (!FB_Table[Index].IsInit) return;
switch(P)
{
case 'TURN':case 'NRUT':
FB_Table[Index].ComputeRotation = 0;
if (Value != 0.0f)
FB_Table[Index].ComputeRotation = 1;
break;
case 'SCAL':case 'LACS':
FB_Table[Index].ComputeScale = 0;
if (Value != 0.0f)
FB_Table[Index].ComputeScale = 1;
break;
case 'INIT':case 'TINI':
FB_Table[Index].FixeMode = 0;
if (Value != 0.0f)
{
FB_Table[Index].FixeMode = 1;
}
break;
case 'JUMP':case 'PMUJ':
FB_Table[Index].BigSearchBase
= 1;
if (Value != 0.0f)
{
FB_Table[Index].BigSearchBase = (int)Value;
}
break;
case 'BLND':case 'DNLB':
FB_Table[Index].BigSearchBase = 1;
if (Value != 0.0f)
{
FB_Table[Index].BigSearchBase = (int)Value;
}
break;
case 'REST':case 'TSER':
FB_Table[Index].DontMoveMode = 0;
if (Value != 0.0f)
{
FB_Table[Index].DontMoveMode = 1;
}
break;
case 'PELD':case 'DLEP':
FB_Table[Index].PertinenceLimiteDown = Value;
break;
case 'EROR':case 'RORE':
FB_Table[Index].MINIMALERROR = Value;
break;
case 'LEAR':case 'RAEL':
FB_Table[Index].LearningMode = Value;
break;
case 'TOLE':case 'ELOT':
FB_Table[Index].TOLERANCE = Value;
break;
case 'STOP':case 'POTS':
FB_Table[Index].PausedMode = 0;
if (Value != 0.0f) FB_Table[Index].PausedMode = 1;
break;
}
}
u32 CFast2D::FeatureBoxGetXYS(u32 Index,Vector *Result)
{
if (Index > MAX_FEATBOX) return 0;
*Result = FB_Table[Index].LastMovesXYS;
return 1;
}
u32 CFast2D::FeatureBoxGetSXSY(u32 Index,Vector *Result)
{
if (Index > MAX_FEATBOX) return 0;
Result->x = FB_Table[Index].XScale;
Result->y = FB_Table[Index].YScale;
Result->z = FB_Table[Index].LastMovesXYS.z;
return 1;
}
u32 CFast2D::FeatureBoxGetRot(u32 Index,float *Result)
{
if (Index > MAX_FEATBOX) return 0;
*Result = FB_Table[Index].Rotation;
return 1;
}
float CFast2D::FeatureBoxGetPertinence(u32 Index)
{
if (Index > MAX_FEATBOX) return 0.0f;
return FB_Table[Index].Pertinence;
}
MATRIX *CFast2D::FeatureBoxGetMatrix(u32 Index)
{
if (Index > MAX_FEATBOX) return NULL;
return &FB_Table[Index].LastTransforMatrix;
}
u32 CFast2D::FeatureBoxFrame(MAP *DbugMap,u32 Only4Clear)
{
Vector C,NewPos[MAX_QFB],SpeedScale;
float Rotation;
u8 Valid[MAX_QFB];
if (FB_init) return 0;
for (u32 FeatBoxIndex = 0; FeatBoxIndex < MAX_FEATBOX; FeatBoxIndex++)
{
u32 DrawIt = 0;
MATRIX NewM ;
if (!FB_Table[FeatBoxIndex].IsInit) continue;
if (FB_Table[FeatBoxIndex].PausedMode)
{
FB_Table[FeatBoxIndex].Pertinence = 0;
FB_Table[FeatBoxIndex].LastMovesXYS = Vector(0,0,1);
FB_Table[FeatBoxIndex].Rotation = 0.0f;
FB_Table[FeatBoxIndex].XScale = FB_Table[FeatBoxIndex].YScale = 1.0f;
FB_Table[FeatBoxIndex].LastTransforMatrix.Identity();
continue;
}
if (!Only4Clear)
{
u32 UlNumber = FB_Table[FeatBoxIndex].ulNumberOfParticules;
u32 CounterDest = 0;
if (FB_Table[FeatBoxIndex].LearningMode) FB_Table[FeatBoxIndex].Pertinence = 0;
float FinalFactor = 0;
FB_Table[FeatBoxIndex].LastMovesXYS = Vector(0,0,1);
FB_Table[FeatBoxIndex].Rotation = 0.0f;
FB_Table[FeatBoxIndex].XScale = FB_Table[FeatBoxIndex].YScale = 1.0f;
u32 BigSearchMode = 0;
if (FB_Table[FeatBoxIndex].Pertinence < 10.0) BigSearchMode = 2;
float OldPertinence = FB_Table[FeatBoxIndex].Pertinence;
FB_Table[FeatBoxIndex].Pertinence = 0.0f;
memset(Valid , 0 , sizeof(Valid ));
if ((FeatBoxIndex & 1) && (FB_Table[FeatBoxIndex-1].Pertinence > FB_Table[FeatBoxIndex-1].PertinenceLimiteDown * 0.6666f))
goto ByPass;
for (u32 Counter = 0 ; Counter < UlNumber;Counter++)
{
//if ((OldPertinence == 0.0) && ((Counter & 1) == 0)) continue;
u32 TrackedID = FB_Table[FeatBoxIndex].TrackerID[Counter];
if (Get(TrackedID, &C))
{
s32 BigSearch = 0;
C.z = 0;
do
{
Set(TrackedID, &C);
ComputeTrack(TrackedID,BigSearch++);
} while ((GetError(TrackedID) < FB_Table[FeatBoxIndex].MINIMALERROR) && (BigSearch < FB_Table[FeatBoxIndex].BigSearchBase + BigSearchMode));
if (GetError(TrackedID) < FB_Table[FeatBoxIndex].MINIMALERROR)
{
Set(TrackedID, &C);
}
}
}
for (u32 Counter = 0 ; Counter < UlNumber;Counter++)
{
NewPos[Counter].z = 0;
if (Get(FB_Table[FeatBoxIndex].TrackerID[Counter], &C) && (C.z > FB_Table[FeatBoxIndex].MINIMALERROR) )
{
NewPos[Counter] = C;
Valid[Counter] = 1;
}
}
// LAUNCH SIMILI RANSAC ******************************************************************
s32 MustRedo = 1;
s32 MustRedoCounter = 3;
do
{
float Tolerance = FB_Table[FeatBoxIndex].FixeMode ? TOLERANSAC_FIXE: TOLERANSAC_NOTFIXE;
if (FB_Table[FeatBoxIndex].TOLERANCE != 0) Tolerance = FB_Table[FeatBoxIndex].TOLERANCE;
FeatureBoxRansac(Valid,UlNumber, FB_Table[FeatBoxIndex].FirstPos, NewPos , &SpeedScale , &Rotation , &FB_Table[FeatBoxIndex], Tolerance) ;
MustRedo = 0;
for (u32 Counter = 0 ; Counter < UlNumber;Counter++)
{
if (Valid[Counter] == 2)
{
Vector Result , FirstPos = FB_Table[FeatBoxIndex].FirstPos[Counter];
Result = FB_Table[FeatBoxIndex].LastTransforMatrix * FirstPos;
u32 TrackedID = FB_Table[FeatBoxIndex].TrackerID[Counter];
Set(TrackedID, &Result);
ComputeTrack(TrackedID,0);
if (GetError(TrackedID) > FB_Table[FeatBoxIndex].MINIMALERROR )
{
Get(FB_Table[FeatBoxIndex].TrackerID[Counter], &Result);
NewPos[Counter] = Result;
Valid[Counter] = 1;
MustRedo = 1;
}
}
}
MustRedoCounter--;
} while (MustRedo && MustRedoCounter);
FB_Table[FeatBoxIndex].Pertinence = floor(100.0f * EvaluateNumber(Valid , UlNumber ) / (float)UlNumber );
for (u32 Counter = 0 ; Counter < UlNumber;Counter++)
{
if (Valid[Counter] == 1)
{
float Factor = NewPos[Counter].z * 1.25f;//FB_Table[FeatBoxIndex].ParticlesPonderation[Counter];
if (Factor > 1.0f) Factor = 1.0f;
Factor = FB_Table[FeatBoxIndex].ParticlesPonderation[Counter] * NewPos[Counter].z * 1.25f;
FinalFactor += Factor;
CounterDest++;
}
}
if (CounterDest>3)
{
FB_Table[FeatBoxIndex].LastMovesXYS = SpeedScale;
FB_Table[FeatBoxIndex].Rotation = Rotation;
}
if (CounterDest > 4)
{
//FinalFactor /= (float)CounterDest;
FB_Table[FeatBoxIndex].Pertinence *= FinalFactor;
FB_Table[FeatBoxIndex].Pertinence = (float)(int)FB_Table[FeatBoxIndex].Pertinence;
/*if (!FB_Table[FeatBoxIndex].LearningMode)
FB_Table[FeatBoxIndex].Pertinence = OldPertinence * PertinenceBlend + FB_Table[FeatBoxIndex].Pertinence * (1.0f - PertinenceBlend);*/
// FB_Table[FeatBoxIndex].Pertinence = FB_Table[FeatBoxIndex].Pertinence ;
}
else
FB_Table[FeatBoxIndex].Pertinence = 0;
if (FB_Table[FeatBoxIndex].Pertinence < FB_Table[FeatBoxIndex].PertinenceLimiteDown / 2)
{
FB_Table[FeatBoxIndex].LastMovesXYS.x = FB_Table[FeatBoxIndex].LastMovesXYS.y = 0.0f;
FB_Table[FeatBoxIndex].LastMovesXYS.z = 1.0f;
FB_Table[FeatBoxIndex].Rotation = 0.0f;
FB_Table[FeatBoxIndex].XScale = FB_Table[FeatBoxIndex].YScale = 1.0f;
FB_Table[FeatBoxIndex].LastTransforMatrix.Identity();
}
ByPass:
NewM = FB_Table[FeatBoxIndex].LastTransforMatrix;
if (FB_Table[FeatBoxIndex].LearningMode || FB_Table[FeatBoxIndex].DontMoveMode) NewM.Identity();
if ((FeatBoxIndex & 1) && (FB_Table[FeatBoxIndex -1].Pertinence > FB_Table[FeatBoxIndex -1].PertinenceLimiteDown * 0.6666f))
{
// Help From Static
FB_Table[FeatBoxIndex].LastTransforMatrix = FB_Table[FeatBoxIndex-1].LastTransforMatrix;
NewM = FB_Table[FeatBoxIndex].LastTransforMatrix;
FB_Table[FeatBoxIndex].LastMovesXYS = FB_Table[FeatBoxIndex-1].LastMovesXYS;
FB_Table[FeatBoxIndex].Rotation = FB_Table[FeatBoxIndex-1].Rotation;
FB_Table[FeatBoxIndex].XScale = FB_Table[FeatBoxIndex-1].YScale;
FB_Table[FeatBoxIndex].YScale = FB_Table[FeatBoxIndex-1].YScale;
FB_Table[FeatBoxIndex].Pertinence = 100;
DrawIt = 1;
}
u32 Done = 0;
for (u32 Counter = 0 ; Counter < UlNumber;Counter++)
{
Vector Result , FirstPos ;
FirstPos = FB_Table[FeatBoxIndex].FirstPos[Counter];
Result = NewM * FirstPos;
Result.z = NewPos[Counter].z;
if (Valid[Counter] != 1) Result.z = 0;
Set(FB_Table[FeatBoxIndex].TrackerID[Counter], &Result);
//if (Valid[Counter] != 1) UpdateData(FB_Table[FeatBoxIndex].TrackerID[Counter]);
if (!FB_Table[FeatBoxIndex].FixeMode)
{
float L = (FirstPos -NewPos[Counter]).SqrNormXY();
if (L > 1)
UpdateData(FB_Table[FeatBoxIndex].TrackerID[Counter]);
}
if (DbugMap)
{
if (FB_Table[FeatBoxIndex].ParticlesPonderation[Counter] > 0.01)
CopyDataRGBA(DbugMap,
TRACKSTACK[FB_Table[FeatBoxIndex].TrackerID[Counter]].InitMap ,
TRACKSTACK[FB_Table[FeatBoxIndex].TrackerID[Counter]].X ,
TRACKSTACK[FB_Table[FeatBoxIndex].TrackerID[Counter]].Y , F2DT8Size ,
TRACKSTACK[FB_Table[FeatBoxIndex].TrackerID[Counter]].Norm[0]);
}
}
}
if (DbugMap)
{
RECT r;
r.right = FB_Table[FeatBoxIndex].LastMovesXYS.x + FB_Table[FeatBoxIndex].X - FB_Table[FeatBoxIndex].XScale * FB_Table[FeatBoxIndex].W / 2;
r.top = FB_Table[FeatBoxIndex].LastMovesXYS.y + FB_Table[FeatBoxIndex].Y - FB_Table[FeatBoxIndex].YScale * FB_Table[FeatBoxIndex].H / 2;
r.left = r.right + FB_Table[FeatBoxIndex].XScale * FB_Table[FeatBoxIndex].W ;
r.bottom = r.top + FB_Table[FeatBoxIndex].YScale * FB_Table[FeatBoxIndex].H ;
if (FB_Table[FeatBoxIndex].Pertinence>FB_Table[FeatBoxIndex].PertinenceLimiteDown)
{
Vector Center ;
NewM = FB_Table[FeatBoxIndex].LastTransforMatrix;
Center.x = FB_Table[FeatBoxIndex].X ;
Center.y = FB_Table[FeatBoxIndex].Y ;
Center = NewM * Center;
Vector XAXIs = Vector(FB_Table[FeatBoxIndex].W*0.5,0,0);
Vector YAXIs = Vector(0,FB_Table[FeatBoxIndex].H*0.5,0);
NewM.T = Vector(0,0,0);
XAXIs = NewM * XAXIs;
YAXIs = NewM * YAXIs;
void DrawLine_Clip(MAP *MapDST,float X1,float Y1,float X2,float Y2,u32 Color);
for (int epx = -1 ;epx < 2 ; epx ++)
for (int epy = -1 ;epy < 2 ; epy ++)
{
DrawLine_Clip(DbugMap,
epx+(Center + XAXIs + YAXIs).x,epy+(Center + XAXIs + YAXIs).y,
epx+(Center - XAXIs + YAXIs).x,epy+(Center - XAXIs + YAXIs).y,
FB_Table[FeatBoxIndex].FixeMode?0xff0000:(Copied ? 0xffff00:0xff00ff));
DrawLine(DbugMap,
epx+(Center + XAXIs - YAXIs).x,epy+(Center + XAXIs - YAXIs).y,
epx+(Center - XAXIs - YAXIs).x,epy+(Center - XAXIs - YAXIs).y,
FB_Table[FeatBoxIndex].FixeMode?0xff0000:(Copied ? 0xffff00:0xff00ff));
DrawLine(DbugMap,
epx+(Center + XAXIs - YAXIs).x,epy+(Center + XAXIs - YAXIs).y,
epx+(Center + XAXIs + YAXIs).x,epy+(Center + XAXIs + YAXIs).y,
FB_Table[FeatBoxIndex].FixeMode?0xff0000:(Copied ? 0xffff00:0xff00ff));
DrawLine(DbugMap,
epx+(Center - XAXIs + YAXIs).x,epy+(Center - XAXIs + YAXIs).y,
epx+(Center - XAXIs - YAXIs).x,epy+(Center - XAXIs - YAXIs).y,
FB_Table[FeatBoxIndex].FixeMode?0xff0000:(Copied ? 0xffff00:0xff00ff));
}
//DrawQuad_R(DbugMap,r.right,r.top,r.left,r.bottom,FB_Table[FeatBoxIndex].FixeMode?0xff0000:(Copied ? 0xffff00:0xff00ff),FB_Table[FeatBoxIndex].FixeMode?4:3,FB_Table[FeatBoxIndex].Rotation);
}
char DEBUGTEXT[2048];
void DRawTEXT_Soft_B(u8 *pTEXT,u32 *pBase,u32 X, u32 Y, s32 Pitch , u32 Height , u32 Color);
sprintf(DEBUGTEXT,"%d",(u32)(FB_Table[FeatBoxIndex].YScale * 100));//,SPm.SX * SPm.SY ,CHKDBG*100 / (SPm.SX * SPm.SY));
// DRawTEXT_Soft_B((u8*)DEBUGTEXT , (u32*)DbugMap->GetBase() , 16 , 32 + 16 * FeatBoxIndex, DbugMap->PITCH , DbugMap->SY , 0xff00ff );
r.right = FB_Table[FeatBoxIndex].X - 5;
r.top = FB_Table[FeatBoxIndex].Y - 5;
r.left = FB_Table[FeatBoxIndex].X + 5;
r.bottom = FB_Table[FeatBoxIndex].Y + 5;
DrawQuad_R(DbugMap,r.right,r.top,r.left,r.bottom,0xffff00,3,0);
}
}
return 0;
}
Reference in a new issue View git blame Copy permalink