437 lines
12 KiB
C++
437 lines
12 KiB
C++
#include "ShotTracker.h"
|
|
#include "math.h"
|
|
#include "stdlib.h"
|
|
#include "memory.h"
|
|
|
|
#define MORE_PREC 1
|
|
#define STEP_ST_FACTOR 1.5f
|
|
//#define SHOT_DRAW_DEBUG
|
|
|
|
//#pragma optimize( "", off )
|
|
|
|
float GetLineError(MAP8 *MapDST,float X1,float Y1,float X2,float Y2,float *Coefs,u32 Number);
|
|
|
|
#ifdef SHOT_DRAW_DEBUG
|
|
#define DBGTEST
|
|
MAP *MapDSTDEBUG = 0;
|
|
#endif
|
|
|
|
float MATCHEllipse2(MAP8 *MapDST,float X,float Y,Vector *pBig,float SmallFactor,float K = 2)
|
|
{
|
|
SHOTS_CLASS::InitcosTable();
|
|
float RetValu;
|
|
#define DISC_NUMBER 16
|
|
#define DISC_DISC (FAST_COS_DISCREET / DISC_NUMBER)
|
|
|
|
int Alpha = 0;
|
|
Vector Small;
|
|
Vector Big2,Small2;
|
|
float BX,BY;
|
|
float BX2,BY2;
|
|
Small.x = -pBig->y * fabs(SmallFactor);
|
|
Small.y = pBig->x * fabs(SmallFactor);
|
|
float Local;
|
|
Local = pBig->NormXY();
|
|
K = Local * 0.125f;
|
|
if (K < 4.0f) K = 4.0f;
|
|
Big2 = *pBig * ((Local + K) / Local);
|
|
Local *= fabs(SmallFactor);
|
|
Small2 = Small * ((Local + K) / Local);
|
|
u32 Ep = FAST_COS_DISCREET / DISC_DISC;
|
|
RetValu = 0.0f;
|
|
u32 NTT = 0;
|
|
while (Ep--)
|
|
{
|
|
Alpha += DISC_DISC;
|
|
BX = (SHOTS_CLASS::FASTCOS[Alpha] * pBig->x) + X;
|
|
BY = (SHOTS_CLASS::FASTCOS[Alpha] * pBig->y) + Y;
|
|
BX += (SHOTS_CLASS::FASTSIN[Alpha] * Small.x);
|
|
BY += (SHOTS_CLASS::FASTSIN[Alpha] * Small.y);
|
|
BX2 = (SHOTS_CLASS::FASTCOS[Alpha] * Big2.x) + X;
|
|
BY2 = (SHOTS_CLASS::FASTCOS[Alpha] * Big2.y) + Y;
|
|
BX2 += (SHOTS_CLASS::FASTSIN[Alpha] * Small2.x);
|
|
BY2 += (SHOTS_CLASS::FASTSIN[Alpha] * Small2.y);
|
|
if ((BX2 > 0) && (BX2 < MapDST->SX) && (BY2 > 0) && (BY2 < MapDST->SY))
|
|
{
|
|
#define Norm8 0.35355339059327376220042218105242f
|
|
float Coefs[] = {-Norm8,-Norm8,-Norm8,-Norm8,Norm8,Norm8,Norm8,Norm8};
|
|
float L = GetLineError(MapDST,BX,BY,BX2,BY2,Coefs,8);
|
|
NTT++;
|
|
if (SmallFactor > 0)
|
|
{
|
|
if (L>0.0) RetValu += L;
|
|
}
|
|
else
|
|
{
|
|
if (L<0.0) RetValu -= L;
|
|
}
|
|
|
|
#ifdef DBGTEST
|
|
if (MapDSTDEBUG && (MapDSTDEBUG->SX == MapDST->SX))
|
|
{
|
|
*(MapDSTDEBUG->GetBase() + (s32)BX + (s32)BY * MapDSTDEBUG->PITCH) |= 0xff;
|
|
*(MapDSTDEBUG->GetBase() + (s32)BX2 + (s32)BY2 * MapDSTDEBUG->PITCH) |= 0xff0000;
|
|
}//*/
|
|
#endif
|
|
}
|
|
}
|
|
if (NTT < DISC_NUMBER>>1) return 0;
|
|
return RetValu / (float)NTT;
|
|
}
|
|
|
|
float MATCHEllipse(MAP8 *MapDST,float X,float Y,Vector *pBig,float SmallFactor,float K = 2)
|
|
{
|
|
return MATCHEllipse2(MapDST,X,Y,pBig,SmallFactor);
|
|
Vector BA2;
|
|
BA2 = *pBig * SmallRaidusFactor;
|
|
return MATCHEllipse2(MapDST,X,Y,pBig,SmallFactor) + MATCHEllipse2(MapDST,X,Y,&BA2,-SmallFactor);
|
|
}
|
|
|
|
float SHOT_TrackSHOT_T(MAP8 *pSrc,Vector *pCenter,Vector *pBigAxis)
|
|
{
|
|
/* Make it Converge */
|
|
float CurrentET[4],CurrentE;
|
|
|
|
u32 Conv = 2 + MORE_PREC;
|
|
float K = 2.f ;//(float)(1<<Conv)/0.75f;
|
|
|
|
while (Conv--)
|
|
{
|
|
u32 i;
|
|
CurrentE = MATCHEllipse(pSrc,pCenter->x,pCenter->y,pBigAxis,pBigAxis->z);
|
|
CurrentET[0] = MATCHEllipse(pSrc,pCenter->x+K,pCenter->y,pBigAxis,pBigAxis->z);
|
|
CurrentET[1] = MATCHEllipse(pSrc,pCenter->x-K,pCenter->y,pBigAxis,pBigAxis->z);
|
|
CurrentET[2] = MATCHEllipse(pSrc,pCenter->x,pCenter->y+K,pBigAxis,pBigAxis->z);
|
|
CurrentET[3] = MATCHEllipse(pSrc,pCenter->x,pCenter->y-K,pBigAxis,pBigAxis->z);
|
|
i = -1;
|
|
if (CurrentET[0] > CurrentE) {CurrentE = CurrentET[0];i=0;};
|
|
if (CurrentET[1] > CurrentE) {CurrentE = CurrentET[1];i=1;};
|
|
if (CurrentET[2] > CurrentE) {CurrentE = CurrentET[2];i=2;};
|
|
if (CurrentET[3] > CurrentE) {CurrentE = CurrentET[3];i=3;};
|
|
if (i != -1)
|
|
{
|
|
switch (i)
|
|
{
|
|
case 0:pCenter->x+=K;break;
|
|
case 1:pCenter->x-=K;break;
|
|
case 2:pCenter->y+=K;break;
|
|
case 3:pCenter->y-=K;break;
|
|
}
|
|
}
|
|
|
|
K *= 0.4f;
|
|
if (!K) K = 1;
|
|
}
|
|
return CurrentE;
|
|
}
|
|
|
|
float SHOT_TrackSHOT_T_UnAffect(MAP8 *pSrc,Vector *pCenter,Vector *pBigAxis)
|
|
{
|
|
/* Make it Converge */
|
|
float Ret;
|
|
Vector SaveT;
|
|
SaveT = *pCenter;
|
|
Ret = SHOT_TrackSHOT_T(pSrc,pCenter,pBigAxis);
|
|
*pCenter = SaveT;
|
|
return Ret;
|
|
}
|
|
float SHOT_TrackSHOT_ST(MAP8 *pSrc,Vector *pCenter,Vector *pBigAxis)
|
|
{
|
|
float CurrentET[6],CurrentE;
|
|
Vector BA[6];
|
|
Vector CenterSave;
|
|
/* Make it Converge */
|
|
u32 Conv = 4 + MORE_PREC;
|
|
float K = 1.5f * STEP_ST_FACTOR;
|
|
while (Conv--)
|
|
{
|
|
u32 i;
|
|
CurrentE = SHOT_TrackSHOT_T(pSrc,pCenter,pBigAxis);
|
|
|
|
BA[0] = BA[1] = BA[2] = BA[3] = BA[4] = BA[5] = *pBigAxis ;
|
|
BA[0].x += K*2.0f;
|
|
BA[1].x -= K*2.0f;
|
|
BA[2].y += K*2.0f;
|
|
BA[3].y -= K*2.0f;
|
|
|
|
BA[4] = BA[5] = *pBigAxis ;
|
|
|
|
float N = pBigAxis->NormXY() * pBigAxis->z;
|
|
BA[4].z *= (N + K) / N;
|
|
BA[5].z *= (N - K) / N;
|
|
if (BA[4].z > 1.0f) BA[4].z = 1.0f;
|
|
if (BA[5].z > 1.0f) BA[5].z = 1.0f;
|
|
|
|
CurrentET[0] = SHOT_TrackSHOT_T_UnAffect(pSrc,pCenter,&BA[0]);
|
|
CurrentET[1] = SHOT_TrackSHOT_T_UnAffect(pSrc,pCenter,&BA[1]);
|
|
CurrentET[2] = SHOT_TrackSHOT_T_UnAffect(pSrc,pCenter,&BA[2]);
|
|
CurrentET[3] = SHOT_TrackSHOT_T_UnAffect(pSrc,pCenter,&BA[3]);
|
|
CurrentET[4] = SHOT_TrackSHOT_T_UnAffect(pSrc,pCenter,&BA[4]);
|
|
CurrentET[5] = SHOT_TrackSHOT_T_UnAffect(pSrc,pCenter,&BA[5]);//*/
|
|
|
|
i = -1;
|
|
if (CurrentET[0] > CurrentE) {CurrentE = CurrentET[0];i=0;};
|
|
if (CurrentET[1] > CurrentE) {CurrentE = CurrentET[1];i=1;};
|
|
if (CurrentET[2] > CurrentE) {CurrentE = CurrentET[2];i=2;};
|
|
if (CurrentET[3] > CurrentE) {CurrentE = CurrentET[3];i=3;};
|
|
if (CurrentET[4] > CurrentE) {CurrentE = CurrentET[4];i=4;};
|
|
if (CurrentET[5] > CurrentE) {CurrentE = CurrentET[5];i=5;};//*/
|
|
|
|
if (i != -1)
|
|
{
|
|
*pBigAxis = BA[i];
|
|
if (BA[i].z > 1.0f) BA[i].z = 1.0f;
|
|
SHOT_TrackSHOT_T(pSrc,pCenter,pBigAxis);
|
|
}
|
|
|
|
K *= 0.3333f;
|
|
if (!K) K = 1;
|
|
}
|
|
return CurrentE;
|
|
}
|
|
|
|
float SHOT_TrackSHOT_ST_UnAffect(MAP8 *pSrc,Vector *pCenter,Vector *pBigAxis)
|
|
{
|
|
/* Make it Converge */
|
|
float ret;
|
|
Vector SaveT,SaveBA;
|
|
SaveT = *pCenter;
|
|
SaveBA = *pBigAxis;
|
|
ret = SHOT_TrackSHOT_ST(pSrc,pCenter,pBigAxis);
|
|
*pCenter = SaveT;
|
|
*pBigAxis= SaveBA;
|
|
return ret;
|
|
}
|
|
|
|
float SHOT_TrackSHOT(MAP8 *pSrc,MAP *pSrcR,Vector *pCenter,Vector *pCenter3D,Vector *pBigAxis,u32 TOnly)
|
|
{
|
|
float CurrentE;
|
|
Vector BA[4];
|
|
Vector BA3D;
|
|
Vector CenterSave;
|
|
/* Make it Converge */
|
|
//pBigAxis->z = 1.0f;
|
|
if (TOnly)
|
|
CurrentE = SHOT_TrackSHOT_T(pSrc,pCenter,pBigAxis);
|
|
else
|
|
CurrentE = SHOT_TrackSHOT_ST(pSrc,pCenter,pBigAxis);
|
|
|
|
|
|
if (pCenter3D)
|
|
{
|
|
BA3D = *pBigAxis * SmallRaidusFactor;
|
|
BA3D.z = -pBigAxis->z;
|
|
SHOT_TrackSHOT_T(pSrc,pCenter3D,&BA3D);
|
|
}
|
|
|
|
|
|
if (pSrcR)
|
|
{
|
|
#ifdef DBGTEST
|
|
MapDSTDEBUG = pSrcR;//*/
|
|
//DARKER(pSrcR);//*/
|
|
Vector BA2;
|
|
DrawEllipse(pSrcR,pCenter->x,pCenter->y,pBigAxis,pBigAxis->z,0xff80);
|
|
DrawEllipse(pSrcR,pCenter3D->x,pCenter3D->y,&BA3D,pBigAxis->z,0xff80);
|
|
char TEXT[2048];
|
|
sprintf(TEXT,"%2.2f",pBigAxis->NormXY());
|
|
DRawTEXT_Soft_B((u8*)TEXT,(u32*)pSrcR->GetBase(),16,16,pSrcR->PITCH,pSrcR->SY,0xff00);
|
|
#endif
|
|
}
|
|
return CurrentE;
|
|
}
|
|
|
|
|
|
u32 SHOTS_CLASS::SHOTS_TRACK(MAP8 *pSrc,MAP8 *pSrcO2,MAP8 *pSrcO4,/*IN - OUT*/GravC *OutpL,MAP *pSrcR)
|
|
{
|
|
OutpL->Coord *= 0.125f;
|
|
OutpL->Coord2D *= 0.125f;
|
|
OutpL->BigAxis.x *= 0.125f;
|
|
OutpL->BigAxis.y *= 0.125f;
|
|
//Tracking Mode
|
|
// SHOT_TrackSHOT(&MHSO8,0,&OutpL->Coord2D, &OutpL->BigAxis,1);
|
|
OutpL->Coord *= 2.0f;
|
|
OutpL->Coord2D *= 2.0f;
|
|
OutpL->BigAxis.x *= 2.0f;
|
|
OutpL->BigAxis.y *= 2.0f;
|
|
SHOT_TrackSHOT(pSrcO4,0,&OutpL->Coord2D, &OutpL->Coord, &OutpL->BigAxis,1);
|
|
OutpL->Coord = OutpL->Coord2D;
|
|
OutpL->Coord *= 2.0f;
|
|
OutpL->Coord2D *= 2.0f;
|
|
OutpL->BigAxis.x *= 2.0f;
|
|
OutpL->BigAxis.y *= 2.0f;
|
|
SHOT_TrackSHOT(pSrcO2,0,&OutpL->Coord2D, &OutpL->Coord, &OutpL->BigAxis,0);
|
|
// OutpL->Coord = OutpL->Coord2D;
|
|
OutpL->Coord *= 2.0f;
|
|
OutpL->Coord2D *= 2.0f;
|
|
OutpL->BigAxis.x *= 2.0f;
|
|
OutpL->BigAxis.y *= 2.0f;
|
|
OutpL->ContourError = SHOT_TrackSHOT(pSrc,pSrcR,&OutpL->Coord2D, &OutpL->Coord, &OutpL->BigAxis,0);
|
|
if ((OutpL->BigAxis.NormXY() < 4.0f) ||
|
|
(OutpL->BigAxis.z < 0.1f))
|
|
return 0;
|
|
|
|
OutpL->PerspectiveDir *= 0.90f;
|
|
OutpL->PerspectiveDir = (OutpL->Coord2D - OutpL->Coord) * 0.1f;
|
|
OutpL->PerspectiveDir.y -= FACTOR_ForceVDir;
|
|
{
|
|
MATRIX OriginalMatrix;
|
|
MATRIX TranMat;
|
|
Quat TransQuat;
|
|
OriginalMatrix = OutpL->Matrix;
|
|
|
|
// 1:Recompute MATRIX
|
|
if (!SHOT_ComputeMatrix(OutpL,pSrc,0))
|
|
{
|
|
OutpL->Matrix.I = OriginalMatrix.I;
|
|
OutpL->Matrix.J = OriginalMatrix.J;
|
|
OutpL->Matrix.K = OriginalMatrix.K;
|
|
} else
|
|
{
|
|
Vector DP,T;
|
|
T = OutpL->Matrix.T;
|
|
DP = OutpL->Matrix.I + OriginalMatrix.I;
|
|
DP.Normalize();
|
|
*(Vector *)&TransQuat = DP ^OutpL->Matrix.I;
|
|
TransQuat.NormalizeW();
|
|
TransQuat.TransforToMatrix(&TranMat);
|
|
OutpL->Matrix = OriginalMatrix;
|
|
OutpL->Matrix *= TranMat;
|
|
OutpL->Matrix.T = T;
|
|
}
|
|
|
|
// 2:Compute Real 3D Center
|
|
{
|
|
float Factor;
|
|
Vector P1,P2,P1p,P2p,PC,PCS;
|
|
P2.x = -OutpL->BigAxis.y * OutpL->BigAxis.z;
|
|
P2.y = OutpL->BigAxis.x * OutpL->BigAxis.z;
|
|
P1.x = OutpL->Coord2D.x + P2.x;
|
|
P1.y = OutpL->Coord2D.y + P2.y;
|
|
P2.x = OutpL->Coord2D.x - P2.x;
|
|
P2.y = OutpL->Coord2D.y - P2.y;
|
|
#ifdef SHOT_DRAW_DEBUG
|
|
if (pSrcR)
|
|
{
|
|
DrawQuadFull(pSrcR,P1.x-4,P1.y-4,P1.x+4,P1.y+4,0xff00);
|
|
DrawQuadFull(pSrcR,P2.x-4,P2.y-4,P2.x+4,P2.y+4,0xff00);
|
|
}
|
|
#endif
|
|
Factor = sqrtf(1.0f - OutpL->Matrix.I.z * OutpL->Matrix.I.z);
|
|
P1.z = OutpL->Matrix.T.z- Factor * 1.0f;
|
|
P2.z = OutpL->Matrix.T.z+ Factor * 1.0f;
|
|
Factor = 0.5f / P1.z + 0.5f / P2.z;
|
|
P1p = P1 * (1.0f / P1.z);
|
|
P2p = P2 * (1.0f / P2.z);
|
|
PC = (P1p + P2p) * 0.5f;
|
|
PC *= 1.0f / Factor;
|
|
P1p = P1 * (1.0f / P2.z);
|
|
P2p = P2 * (1.0f / P1.z);
|
|
PCS = (P1p + P2p) * 0.5f;
|
|
PCS *= 1.0f / Factor;
|
|
Vector BA[2];
|
|
float BRA[2];
|
|
BA[0] = PC;
|
|
BA[1] = PCS;
|
|
Vector BA3D;
|
|
BA3D = OutpL->BigAxis * SmallRaidusFactor;
|
|
BA3D.z = -OutpL->BigAxis.z;
|
|
|
|
BRA[0] = SHOT_TrackSHOT(pSrc,0,&BA[0], 0, &BA3D,1);
|
|
BRA[1] = SHOT_TrackSHOT(pSrc,0,&BA[1], 0, &BA3D,1);
|
|
if (BRA[0] > BRA[1])
|
|
OutpL->Coord = BA[0];
|
|
else
|
|
OutpL->Coord = BA[1];
|
|
#ifdef SHOT_DRAW_DEBUG
|
|
if (pSrcR)
|
|
{
|
|
Vector BA3D = OutpL->BigAxis * SmallRaidusFactor;
|
|
DrawQuadFull(pSrcR, PC.x-4, PC.y-4, PC.x+4, PC.y+4,0xff00);
|
|
DrawQuadFull(pSrcR,PCS.x-4,PCS.y-4,PCS.x+4,PCS.y+4,0xff00);
|
|
DrawQuadFull(pSrcR,OutpL->Coord.x-4,OutpL->Coord.y-4,OutpL->Coord.x+4,OutpL->Coord.y+4,0xff8080);
|
|
DrawEllipse(pSrcR,OutpL->Coord.x,OutpL->Coord.y,&BA3D,OutpL->BigAxis.z,0xff80);
|
|
}
|
|
#endif
|
|
if (!SHOT_ComputeMatrix(OutpL,pSrc,0))
|
|
{
|
|
OutpL->Matrix.I = OriginalMatrix.I;
|
|
OutpL->Matrix.J = OriginalMatrix.J;
|
|
OutpL->Matrix.K = OriginalMatrix.K;
|
|
} else
|
|
{
|
|
Vector DP,T;
|
|
T = OutpL->Matrix.T;
|
|
DP = OutpL->Matrix.I + OriginalMatrix.I;
|
|
DP.Normalize();
|
|
*(Vector *)&TransQuat = DP ^OutpL->Matrix.I;
|
|
TransQuat.NormalizeW();
|
|
TransQuat.TransforToMatrix(&TranMat);
|
|
OutpL->Matrix = OriginalMatrix;
|
|
OutpL->Matrix *= TranMat;
|
|
OutpL->Matrix.T = T;
|
|
}
|
|
|
|
// SHOT_ComputeMatrix(OutpL,pSrc,0);
|
|
}//*/
|
|
|
|
// 2:Recompute MATRIX I Angle
|
|
{
|
|
MATRIX Smat;
|
|
Smat = OutpL->Matrix;
|
|
Get3DCode(pSrc,&OutpL->Matrix,CODERADIUSCOEF,OutpL->CODE);
|
|
float CODE[RAW_CODESAMPLEL];
|
|
u32 ShiftV;
|
|
ShiftV = GetCodeOrigin(OutpL->CODE);
|
|
memcpy(CODE,OutpL->CODE,sizeof(OutpL->CODE));
|
|
for (u32 i = 0 ; i < RAW_CODESAMPLEL - ShiftV ; i++)
|
|
{
|
|
OutpL->CODE[i] = CODE[i+ShiftV];
|
|
}
|
|
for (u32 i = RAW_CODESAMPLEL - ShiftV ; i < RAW_CODESAMPLEL; i++)
|
|
{
|
|
OutpL->CODE[i] = CODE[i-(RAW_CODESAMPLEL - ShiftV)];
|
|
}
|
|
u32 NewCN;
|
|
NewCN = GetCode(OutpL->CODE);
|
|
// Extract Code
|
|
if ((OutpL->ContourError > 0.9f) || (OutpL->CodeNumber == NewCN))
|
|
{
|
|
if (NewCN != -1)
|
|
OutpL->Matrix.RotateAround_I(((float)ShiftV) * 3.1415927f * 2.0f / (float)RAW_CODESAMPLEL);
|
|
} else
|
|
OutpL->CodeNumber = NewCN;
|
|
//if (pShot->CodeNumber == -1) return 0;
|
|
//if (OutpL->CodeNumber == -1) OutpL->Matrix = Smat;
|
|
}//*/
|
|
/* s32 Prec = 4;
|
|
while (Prec)
|
|
{
|
|
float Error = -1000000000000000.0f;
|
|
MATRIX FM;
|
|
FM = OutpL->Matrix;
|
|
for (s32 C = -2 * Prec; C < 2 * Prec; C += Prec)
|
|
{
|
|
float LocdalCode[RAW_CODESAMPLEL],LocalEr;
|
|
MATRIX M2;
|
|
M2 = OutpL->Matrix;
|
|
M2.RotateAround_I((float)C * 3.1415927f / 1024.0f);
|
|
Get3DCode(pSrc,&M2,CODERADIUSCOEF,(float*)LocdalCode);
|
|
LocalEr = 0.0f;
|
|
for (int j = 0 ; j < RAW_CODESAMPLEL ; j ++)
|
|
LocalEr += LocdalCode[j] * OutpL->CODE[j];
|
|
if (LocalEr > Error)
|
|
{
|
|
FM = M2;
|
|
Error = LocalEr;
|
|
}
|
|
}
|
|
OutpL->Matrix = FM;
|
|
Prec >>= 1;
|
|
}//*/
|
|
|
|
// 3:SmoothMatrix
|
|
SHOT_ComputeSmooth(OutpL);
|
|
}
|
|
return 1;
|
|
}
|