318 lines
No EOL
8.8 KiB
C++
318 lines
No EOL
8.8 KiB
C++
#include "ShotTracker.h"
|
|
#include "math.h"
|
|
#include "string.h"
|
|
#include "stdio.h"
|
|
#include "stdlib.h"
|
|
#include "memory.h"
|
|
|
|
|
|
void SHOTS_CLASS::InitcosTable()
|
|
{
|
|
if (IsCosInit) return;
|
|
IsCosInit = 1;
|
|
for (int i = 0 ; i < FAST_COS_DISCREET+1 ; i ++)
|
|
{
|
|
FASTCOS[i] = cosf((float)i * 3.1415927f * 2.0f / (float)FAST_COS_DISCREET);
|
|
FASTSIN[i] = sinf((float)i * 3.1415927f * 2.0f / (float)FAST_COS_DISCREET);
|
|
}
|
|
|
|
}
|
|
|
|
|
|
Vector SHOTS_Project_2_Screen(Vector &V,s32 MAPSX,s32 MAPSY, float focale)
|
|
{
|
|
Vector Ret;
|
|
float Ooz;
|
|
if (V.z > 0.001f)
|
|
{
|
|
Ooz = 1.0f / V.z;
|
|
Ret.x = 0.5f*(focale * V.x * Ooz * MAPSX + MAPSX);
|
|
Ret.y = 0.5f*(focale * V.y * Ooz * MAPSX + MAPSY);
|
|
Ret.z = Ooz;
|
|
} else Ret.z = -1.0f;
|
|
//Ret = V;
|
|
return Ret;
|
|
}
|
|
|
|
|
|
|
|
void DrawEllipse(MAP *MapDST,s32 X,s32 Y,Vector *pBig,float SmallFactor,u32 Color)
|
|
{
|
|
SHOTS_CLASS::InitcosTable();
|
|
#define DISC_DISC 16
|
|
int Alpha = 0;
|
|
Vector Small;
|
|
s32 BX,BY;
|
|
s32 BX2,BY2;
|
|
Small.x = -pBig->y * SmallFactor;
|
|
Small.y = pBig->x * SmallFactor;
|
|
BX2 = (int)(SHOTS_CLASS::FASTCOS[Alpha] * pBig->x);
|
|
BY2 = (int)(SHOTS_CLASS::FASTCOS[Alpha] * pBig->y);
|
|
BX2 += (int)(SHOTS_CLASS::FASTSIN[Alpha] * Small.x);
|
|
BY2 += (int)(SHOTS_CLASS::FASTSIN[Alpha] * Small.y);
|
|
u32 Ep = FAST_COS_DISCREET / DISC_DISC;
|
|
while (Ep--)
|
|
{
|
|
Alpha += DISC_DISC;
|
|
BX = (int)(SHOTS_CLASS::FASTCOS[Alpha] * pBig->x);
|
|
BY = (int)(SHOTS_CLASS::FASTCOS[Alpha] * pBig->y);
|
|
BX += (int)(SHOTS_CLASS::FASTSIN[Alpha] * Small.x);
|
|
BY += (int)(SHOTS_CLASS::FASTSIN[Alpha] * Small.y);
|
|
DrawLine(MapDST,(float)(X + BX),(float)(Y + BY),(float)(X + BX2),(float)(Y + BY2),Color);
|
|
BX2 = BX;
|
|
BY2 = BY;
|
|
}
|
|
}
|
|
|
|
|
|
void DrawMatrix(MAP *MapSRC,MATRIX *M, float focale)
|
|
{
|
|
Vector P0,PI,PJ,PK;
|
|
P0 = SHOTS_Project_2_Screen(M->T,MapSRC->SX,MapSRC->SY,focale);
|
|
PI = SHOTS_Project_2_Screen(M->T + M->I,MapSRC->SX,MapSRC->SY,focale);
|
|
PJ = SHOTS_Project_2_Screen(M->T + M->J,MapSRC->SX,MapSRC->SY,focale);
|
|
PK = SHOTS_Project_2_Screen(M->T + M->K,MapSRC->SX,MapSRC->SY,focale);
|
|
DrawLine_Clip(MapSRC,P0.x, P0.y + 10, P0.x, P0.y - 10, 0x808080);//*/
|
|
DrawLine_Clip(MapSRC,P0.x + 10, P0.y, P0.x - 10, P0.y, 0x808080);//*/
|
|
DrawLine_Clip(MapSRC,P0.x, P0.y, PI.x, PI.y, 0xff0000);//*/
|
|
DrawLine_Clip(MapSRC,P0.x, P0.y, PJ.x, PJ.y, 0xff00);//*/
|
|
DrawLine_Clip(MapSRC,P0.x, P0.y, PK.x, PK.y, 0xff);//*/
|
|
}
|
|
void Draw3DCircle(MAP *MapSRC,MATRIX *M,float Radius,u32 Color, float focale)
|
|
{
|
|
Vector P0p,P1p,P0,P1;
|
|
SHOTS_CLASS::InitcosTable();
|
|
#define CIRCLE_DISC 32
|
|
int Alpha = 0;
|
|
u32 Ep = FAST_COS_DISCREET / CIRCLE_DISC;
|
|
P0 = M->J * SHOTS_CLASS::FASTSIN[Alpha] * Radius;
|
|
P0 += M->K * SHOTS_CLASS::FASTCOS[Alpha] * Radius;
|
|
P0 += M->T;
|
|
P0p = SHOTS_Project_2_Screen(P0,MapSRC->SX,MapSRC->SY,focale);
|
|
while (Ep--)
|
|
{
|
|
P1 = P0;
|
|
P1p= P0p;
|
|
Alpha += CIRCLE_DISC;
|
|
P0 = M->J * SHOTS_CLASS::FASTSIN[Alpha] * Radius;
|
|
P0 += M->K * SHOTS_CLASS::FASTCOS[Alpha] * Radius;
|
|
P0 += M->T;
|
|
P0p = SHOTS_Project_2_Screen(P0,MapSRC->SX,MapSRC->SY,focale);
|
|
DrawLine(MapSRC,P0p.x,P0p.y,P1p.x,P1p.y,Color);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
u32 SHOTS_CLASS::SHOT_ComputeMatrix(GravC *pShot , MAP8 *pSrc , u32 ComputeCode)
|
|
{
|
|
MATRIX K;
|
|
K.Identity();
|
|
//pShot->BigAxis.z = pShot->BigAxis.z * 0.5f + 0.5f ;
|
|
/* Compute T */
|
|
K.T.x = (pShot->Coord.x - (pSrc->SX/2.0f)) / pSrc->SX;
|
|
K.T.y = (pShot->Coord.y - (pSrc->SY/2.0f)) / pSrc->SX;
|
|
K.T.z = SHOTS_FOCALE/2.0f;
|
|
|
|
K.T.Normalize();
|
|
K.T *= 0.5f * SHOTS_FOCALE * ((float)pSrc->SX) / pShot->BigAxis.NormXY();
|
|
|
|
/* Compute I */
|
|
Vector Normale,TN;
|
|
Normale.x = -pShot->BigAxis.y;
|
|
Normale.y = pShot->BigAxis.x;
|
|
Normale.z = 0.0f;
|
|
pShot->PerspectiveDir.z = 0.0f;
|
|
if (Normale * pShot->PerspectiveDirSmoothed < 0.0f) Normale *= -1.0f;
|
|
|
|
// Anil
|
|
float F,Z;
|
|
Z = pShot->BigAxis.z;
|
|
if (Z > 0.9999999f) Z = 0.9999999f;
|
|
if (Z < 0.0000001f) Z = 0.0000001f;
|
|
|
|
TN = K.T;
|
|
TN.Normalize();
|
|
Normale = Normale - TN * (Normale * TN);
|
|
Normale.Normalize();
|
|
TN *= -Z;
|
|
F = 1.0f - Z * Z;// * pShot->BigAxis.z);
|
|
F = sqrtf(F);
|
|
Normale *= F;
|
|
K.I = TN - Normale;
|
|
|
|
|
|
/* Compute J & K */
|
|
K.I.Normalize();
|
|
K.J = K.I ^ Vector(0,0,1);
|
|
if (K.J.SqrNorm() > 0.25f*0.25f)
|
|
{
|
|
K.K = K.I ^ K.J;
|
|
} else
|
|
{
|
|
K.K = Vector(0,1,0) ^ K.I;
|
|
K.J = K.K ^ K.I;
|
|
}
|
|
K.J.Normalize();
|
|
K.K.Normalize();//*/
|
|
|
|
if (K.CheckNAN())
|
|
return 0;
|
|
|
|
// Research and validation of the code contained (or not) in the recognized circle.
|
|
if (ComputeCode)
|
|
{
|
|
// Line code extraction (white circle)
|
|
Get3DCode(pSrc,&K,CODERADIUSCOEF,pShot->CODE);
|
|
|
|
float CODE[RAW_CODESAMPLEL];
|
|
|
|
// Pixels shift depending on the white anchor
|
|
u32 ShiftV;
|
|
ShiftV = GetCodeOrigin(pShot->CODE);
|
|
|
|
memcpy(CODE,pShot->CODE,sizeof(pShot->CODE));
|
|
|
|
// Code pixels shift before reading
|
|
for (u32 i = 0 ; i < RAW_CODESAMPLEL - ShiftV ; i++)
|
|
pShot->CODE[i] = CODE[i+ShiftV];
|
|
|
|
for (u32 i = RAW_CODESAMPLEL - ShiftV ; i < RAW_CODESAMPLEL; i++)
|
|
pShot->CODE[i] = CODE[i-(RAW_CODESAMPLEL - ShiftV)];
|
|
|
|
// Code reading
|
|
pShot->CodeNumber = GetCode(pShot->CODE);
|
|
|
|
//if (pShot->CodeNumber == -1)
|
|
// return 0;
|
|
|
|
|
|
// Reverse code reading in case of invalidated code
|
|
if (pShot->CodeNumber == ~0)
|
|
{
|
|
float tmp;
|
|
// pShot->CODE inversion
|
|
for (int i = 0; i < RAW_CODESAMPLEL; i++)
|
|
pShot->CODE[i] = CODE[RAW_CODESAMPLEL-1-i];
|
|
// CODE inversion
|
|
for (int i = 0; i < RAW_CODESAMPLEL; i++)
|
|
CODE[i] = pShot->CODE[i];
|
|
// Shift computation
|
|
ShiftV = GetCodeOrigin(pShot->CODE);
|
|
// Code line pixels rotation
|
|
for (u32 i = 0 ; i < RAW_CODESAMPLEL - ShiftV ; i++)
|
|
pShot->CODE[i] = CODE[i+ShiftV];
|
|
for (u32 i = RAW_CODESAMPLEL - ShiftV ; i < RAW_CODESAMPLEL; i++)
|
|
pShot->CODE[i] = CODE[i-(RAW_CODESAMPLEL - ShiftV)];
|
|
// Code reading
|
|
pShot->CodeNumber = GetCode(pShot->CODE);
|
|
// Axis rotation to be near of the white anchor
|
|
K.RotateAround_I(-(float)ShiftV * 3.1415927f * 2.0f / (float)RAW_CODESAMPLEL);
|
|
}
|
|
// Validated code
|
|
else
|
|
|
|
// Axis rotation to be near of the white anchor
|
|
K.RotateAround_I((float)ShiftV * 3.1415927f * 2.0f / (float)RAW_CODESAMPLEL);
|
|
|
|
pShot->MatrixSmoothed = K;
|
|
}
|
|
pShot->Matrix = K;
|
|
return 1;
|
|
}
|
|
|
|
void SHOTS_CLASS::SHOT_ComputeSmooth(GravC *OutpL)
|
|
{
|
|
Quat Angle;
|
|
float SinAlpha;
|
|
MATRIX InVertPrev;
|
|
Vector OdlT,NewT;
|
|
OdlT = OutpL->MatrixSmoothed.T;
|
|
NewT = OutpL->Matrix.T;
|
|
OutpL->Matrix.Inverse(InVertPrev);
|
|
InVertPrev *= OutpL->MatrixSmoothed;
|
|
Angle.ComputeFromMatrix(&InVertPrev);
|
|
SinAlpha = (*((Vector *)&Angle)).SqrNorm();
|
|
#define COEF 0.10f
|
|
// Blend Angles
|
|
if (SinAlpha < COEF*COEF)
|
|
{
|
|
// blend
|
|
SinAlpha = sqrtf(SinAlpha)/ COEF;
|
|
SinAlpha = sqrtf(SinAlpha);
|
|
OutpL->MatrixSmoothed.Blend(OutpL->Matrix,SinAlpha);
|
|
OutpL->MatrixSmoothed.T = OutpL->Matrix.T;
|
|
|
|
} else
|
|
// Copy
|
|
OutpL->MatrixSmoothed = OutpL->Matrix;//*/
|
|
|
|
// Blend Translation
|
|
//OutpL->MatrixSmoothed.T = OutpL->Matrix.T * 0.1 + OdlT * 0.9;
|
|
|
|
|
|
Vector ToleranceVector,ErrorVector;
|
|
ToleranceVector.x = 0.001f * /*Pixel*/ NewT.z * SHOTS_FOCALE;
|
|
ToleranceVector.y = ToleranceVector.x;
|
|
ErrorVector = NewT - OdlT;
|
|
if ((fabs(ErrorVector.x) > ToleranceVector.x) ||
|
|
(fabs(ErrorVector.y) > ToleranceVector.y))
|
|
/* Out Of Tolerance -> Copy */
|
|
OutpL->MatrixSmoothed.T = NewT;
|
|
else
|
|
/* else -> Blend */
|
|
OutpL->MatrixSmoothed.T = NewT * 0.25f + OdlT * 0.75f;
|
|
|
|
|
|
|
|
if (OutpL->MatrixSmoothed.I.z < -0.9f)
|
|
OutpL->PerspectiveDirSmoothed = OutpL->PerspectiveDir;
|
|
else
|
|
OutpL->PerspectiveDirSmoothed = (OutpL->PerspectiveDirSmoothed * 0.925f) + (OutpL->PerspectiveDir * 0.075f);
|
|
}
|
|
|
|
void SHOTS_CLASS::SHOT_DrawDebug(GravC *pShot,MAP *pSrc,u32 DrawCodeBMP , u32 DrawCodeTEXT , u32 Color)
|
|
{
|
|
if (DrawCodeBMP)
|
|
{
|
|
float LocdalCode2[RAW_CODESAMPLEL];
|
|
memcpy(LocdalCode2,pShot->CODE,sizeof(pShot->CODE));
|
|
for (int i = 0 ; i < RAW_CODESAMPLEL ; i ++)
|
|
{
|
|
s32 Color = (u32)((LocdalCode2[i]*2.0f+0.5f)*256.0f);
|
|
if (Color > 255) Color = 255;
|
|
if (Color < 0) Color = 0;
|
|
Color |= Color << 8;
|
|
Color |= Color << 8;
|
|
if ((i&((1<<RAW_CODESAMPLEL_Po2)-1)) == 0) Color = Color;
|
|
for (int j = 0 ; j < 20 ; j ++)
|
|
{
|
|
s32 X = (s32)(pShot->Coord.x + i);
|
|
s32 Y = (s32)(pShot->Coord.y + j);
|
|
if (((X>0)&&(Y>0)) &&
|
|
((X<(int)pSrc->SX)&&(Y<<pSrc->SY)))
|
|
{
|
|
pSrc->SetValue(X + Y * pSrc->PITCH, Color);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
Draw3DCircle(pSrc,&pShot->Matrix,1,Color, SHOTS_FOCALE);
|
|
Draw3DCircle(pSrc,&pShot->Matrix,CODERADIUSCOEF,0xffffff, SHOTS_FOCALE);
|
|
//DrawEllipse(pSrc,pShot->Coord2D.x,pShot->Coord2D.y,&pShot->BigAxis,pShot->BigAxis.z,0x808080);
|
|
DrawMatrix(pSrc,&pShot->Matrix, SHOTS_FOCALE);
|
|
DrawMatrix(pSrc,&pShot->MatrixSmoothed, SHOTS_FOCALE);
|
|
/*
|
|
if (DrawCodeTEXT)
|
|
{
|
|
void DRawTEXT_Soft_B(unsigned char *pTEXT,u32 *pBase,u32 X, u32 Y, s32 Pitch , u32 Height , u32 Color);
|
|
|
|
char T[1024];
|
|
sprintf(T,"%02d",pShot->CodeNumber);
|
|
DRawTEXT_Soft_B((u8*)T,(u32*)pSrc->GetBase(),pShot->Coord.x - strlen(T) * 4 , pShot->Coord.y - 8, pSrc->PITCH, pSrc->SY, Color);
|
|
sprintf(T,"E=%3.2f",pShot->ContourError);
|
|
DRawTEXT_Soft_B((u8*)T,(u32*)pSrc->GetBase(),pShot->Coord.x - strlen(T) * 4 , pShot->Coord.y - 8 + 16, pSrc->PITCH, pSrc->SY, Color);
|
|
}
|
|
*/
|
|
} |