302 lines
No EOL
7.7 KiB
C++
302 lines
No EOL
7.7 KiB
C++
#include "ShotTracker.h"
|
||
#include "memory.h"
|
||
#include "math.h"
|
||
|
||
//#pragma optimize( "", off )
|
||
#define OverSamplingPo2 2
|
||
#define BIT_PREC 0.001f
|
||
|
||
Vector SHOTS_Project_2_Screen(Vector &V,s32 MAPSX,s32 MAPSY, float focale);
|
||
|
||
void SHOTS_CLASS::Get3DCode(MAP8 *Map,MATRIX *M,float Radius,float *pCode)
|
||
{
|
||
Vector P0p,P1p,P0,P1;
|
||
InitcosTable();
|
||
float Alpha,AD;
|
||
memset(pCode,0,RAW_CODESAMPLEL*4);
|
||
|
||
u32 RadialOverSample = 1;
|
||
while (RadialOverSample--)
|
||
{
|
||
Alpha = 1024.0f;
|
||
AD = 1024.0f / (float)RAW_CODESAMPLEL;
|
||
AD *= 1.0f/(float)(1<<OverSamplingPo2);
|
||
u32 Ep = RAW_CODESAMPLEL * 4;
|
||
P0 = M->J * FASTSIN[(u32)Alpha] * Radius;
|
||
P0 += M->K * FASTCOS[(u32)Alpha] * Radius;
|
||
P0 += M->T;
|
||
P0p = SHOTS_Project_2_Screen(P0,Map->SX,Map->SY,SHOTS_FOCALE);
|
||
for (u32 Ep = 0 ; Ep < RAW_CODESAMPLEL << OverSamplingPo2 ; Ep++)
|
||
{
|
||
P1 = P0;
|
||
P1p= P0p;
|
||
Alpha -= AD;
|
||
P0 = M->J * FASTSIN[(u32)Alpha] * Radius;
|
||
P0 += M->K * FASTCOS[(u32)Alpha] * Radius;
|
||
P0 += M->T;
|
||
P0p = SHOTS_Project_2_Screen(P0,Map->SX,Map->SY,SHOTS_FOCALE);
|
||
if ((P0p.x > 0.0) && (P0p.y > 0.0) &&
|
||
(P0p.x < Map->SX) && (P0p.y < Map->SY))
|
||
{
|
||
float V;
|
||
ExtractPoint(Map,P0p.x,P0p.y,&V);
|
||
pCode[Ep>>OverSamplingPo2] += V * 1.0f / (float)(1<<OverSamplingPo2);
|
||
}
|
||
}
|
||
Radius *= 0.95f;
|
||
}
|
||
|
||
|
||
|
||
|
||
/* Then Normalize localy (1) */
|
||
///*
|
||
/*
|
||
#define LOCALYBLUR (8<<RAW_CODESAMPLEL_Po2)
|
||
{
|
||
float BITTABLEblur[RAW_CODESAMPLEL+4];
|
||
float AV = 0.0f;
|
||
float NORM = 0.0f;
|
||
float *pCodeLast,*pCodeCurrent,*pCodeP,*pCodeN,*pCodeW;
|
||
pCodeLast = pCode + RAW_CODESAMPLEL;
|
||
pCodeCurrent = pCode;
|
||
pCodeP = pCodeLast - (LOCALYBLUR>>1);
|
||
pCodeN = pCode + (LOCALYBLUR>>1);
|
||
while (pCodeP != pCodeN)
|
||
{
|
||
AV += *(pCodeP++);
|
||
if (pCodeP == pCodeLast) pCodeP = pCode;
|
||
}
|
||
pCodeP = pCodeLast - (LOCALYBLUR>>1);
|
||
pCodeW = BITTABLEblur;
|
||
while (pCodeCurrent < pCodeLast)
|
||
{
|
||
AV -= *(pCodeP++);
|
||
AV += *(pCodeN++);
|
||
*(pCodeW++) = AV * (1.0f/(float)LOCALYBLUR);
|
||
if (pCodeP == pCodeLast) pCodeP = pCode;
|
||
if (pCodeN == pCodeLast) pCodeN = pCode;
|
||
pCodeCurrent++;
|
||
}
|
||
pCodeW = BITTABLEblur;
|
||
pCodeCurrent = pCode;
|
||
while (pCodeCurrent < pCodeLast)
|
||
{
|
||
*(pCodeCurrent++) -= *(pCodeW++);
|
||
}
|
||
}
|
||
//*/
|
||
|
||
|
||
/* Then Normalize (2) */
|
||
{
|
||
float AV = 0.0f;
|
||
float NORM = 0.0f;
|
||
float *pCodeLast;
|
||
pCodeLast = pCode + RAW_CODESAMPLEL;
|
||
while (pCode < pCodeLast) AV += *(pCode++);
|
||
pCode -= RAW_CODESAMPLEL;
|
||
AV /= (float)RAW_CODESAMPLEL;
|
||
while (pCode < pCodeLast)
|
||
{
|
||
*(pCode) -= AV;
|
||
NORM += *(pCode)**(pCode);
|
||
pCode++;
|
||
}
|
||
pCode -= RAW_CODESAMPLEL;
|
||
NORM = 1.0f / sqrtf(NORM);
|
||
while (pCode < pCodeLast) *(pCode++) *= NORM;
|
||
}
|
||
|
||
}
|
||
void SetCode(u32 *pCodeG_100,u32 CodeV)
|
||
{
|
||
memset(pCodeG_100,0,sizeof(u32) * RAW_CODELENGHT);
|
||
for (int i = 0 ; i < RAW_CODELENGHT ; i ++)
|
||
{
|
||
// Creation of the 8 last code values (ANCHOR) : here, 1 Black, 4 White, 3 Black
|
||
if (i >= RAW_CODELENGHT - ANCHOR_SIZE)
|
||
{
|
||
u32 H;
|
||
H = i - (RAW_CODELENGHT - ANCHOR_SIZE);
|
||
// Modif dtm
|
||
//if (H < ANCHOR_BSIZE)
|
||
if (H < ANCHOR_BSIZE-1)
|
||
pCodeG_100[i] = 0;
|
||
else
|
||
// Modif dtm
|
||
//if (H >= (ANCHOR_SIZE) - (ANCHOR_BSIZE))
|
||
if (H >= (ANCHOR_SIZE) - (ANCHOR_BSIZE)-1)
|
||
pCodeG_100[i] = 0;
|
||
else
|
||
pCodeG_100[i] = 1;
|
||
}
|
||
// Creation of the (RAW_CODELENGHT-8) code main values.
|
||
else
|
||
{
|
||
if (i < RAW_CODEBITNUM * 2)
|
||
{
|
||
pCodeG_100[i] = ((CodeV >> (i>>1)) ^ i) & 1;
|
||
} else
|
||
if (i < RAW_CODELENGHT - ANCHOR_SIZE)
|
||
{
|
||
u32 CheckSum,Shift;
|
||
CheckSum = (CodeV ^ (CodeV >> (RAW_CODEBITNUM>>1)));
|
||
CheckSum ^= 0xAAAA;
|
||
CheckSum &= ((1<<(RAW_CODEBITNUM>>1))-1);
|
||
Shift = i-RAW_CODEBITNUM * 2;
|
||
pCodeG_100[i] = (((CheckSum >> (Shift>>1)) ^ Shift) & 1);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
/*
|
||
Inoput = RAW CODE
|
||
output = index of the origin
|
||
*/
|
||
|
||
u32 SHOTS_CLASS::GetCodeOrigin(float *pCode)
|
||
{
|
||
float *pCodeParser,*pCodeLast;
|
||
u32 CurrnetAlpha,ReturnValue;
|
||
float Accum = 0.0f;
|
||
float SqrAccum = 0.0f;
|
||
float EcartType = 0.0f;
|
||
float EMin = -100000000000000000000.0f;
|
||
float OoN = 1.0f / (float)RAW_WhiteLenghtPlus;
|
||
pCodeLast = pCode + RAW_CODESAMPLEL;
|
||
CurrnetAlpha = ReturnValue = 0;
|
||
pCodeParser = pCodeLast - RAW_WhiteLenghtPlus;
|
||
/*
|
||
Eq = (Vi-VM)(Vi-VM) + (Vi-VM)(Vi-VM) + ....
|
||
Eq = Vi<56> + VM<56> - 2 * VM * Vi + ....
|
||
Eq = e{Vi<56>} + N*VM<56> - N * 2 * VM * Vi + ....
|
||
=> VM = e{Vi} / N
|
||
Eq = e{Vi<56>} + N*e{Vi}<7D> / N<> - (2 * (e{Vi} / N)) e{Vi}
|
||
Eq = e{Vi<56>} + e{Vi}<7D> / N - 2 * e{Vi}<7D> / N
|
||
Eq = e{Vi<56>} - e{Vi}<7D> / N
|
||
|
||
|
||
Eq = e{Vi<56>} + e{Vi}<7D> * 1/N<> - e{Vi} * 2 * VM
|
||
Eq = e{Vi<56>} + e{Vi}<7D> * 1/N<> - e{Vi} * e{Vi} * 2/N
|
||
Eq = e{Vi<56>} + e{Vi}<7D> * 1/N<> - e{Vi}<7D> * 2/N
|
||
Eq = e{Vi<56>} - e{Vi}<7D> / N
|
||
|
||
Norm = 1.0f / sqrt(Eq);
|
||
|
||
VV = ((Vi-VM) * Coef[i] / Norm);
|
||
*/
|
||
|
||
while (pCodeParser < pCodeLast)
|
||
{
|
||
Accum += *(pCodeParser);
|
||
SqrAccum += *(pCodeParser) * *(pCodeParser);
|
||
pCodeParser++;
|
||
}
|
||
pCodeParser = pCodeLast - RAW_WhiteLenghtPlus;
|
||
while (pCodeParser < pCodeLast)
|
||
{
|
||
Accum += *pCode - *pCodeParser;
|
||
SqrAccum += *pCode * *pCode - *pCodeParser * *pCodeParser;
|
||
EcartType = SqrAccum - Accum * Accum * OoN;
|
||
|
||
pCode++;
|
||
pCodeParser++;
|
||
CurrnetAlpha++;
|
||
if (Accum >EMin)
|
||
//if (EcartType < EMin)
|
||
{
|
||
EMin = Accum;//EcartType;
|
||
ReturnValue = CurrnetAlpha;
|
||
}
|
||
}
|
||
pCodeParser = pCode - RAW_WhiteLenghtPlus;
|
||
while (pCode < pCodeLast)
|
||
{
|
||
Accum += *pCode - *pCodeParser;
|
||
SqrAccum += *pCode * *pCode - *pCodeParser * *pCodeParser;
|
||
EcartType = SqrAccum - Accum * Accum * OoN;
|
||
pCode++;
|
||
pCodeParser++;
|
||
CurrnetAlpha++;
|
||
if (Accum >EMin)
|
||
//if (EcartType < EMin)
|
||
{
|
||
EMin = Accum;//EcartType;
|
||
ReturnValue = CurrnetAlpha;
|
||
}
|
||
}
|
||
return ReturnValue;
|
||
}
|
||
|
||
// Read code
|
||
u32 SHOTS_CLASS::GetCode(float *pCODE)
|
||
{
|
||
// Extract Code
|
||
u32 CodeBBd,Checksum ,BitOne;
|
||
BitOne = Checksum = CodeBBd = 0;
|
||
float *p = pCODE;
|
||
float BITTABLE[RAW_CODELENGHT+4];
|
||
|
||
// Computes the sums of CODE values 8 by 8 and writes the results in BITTABLE
|
||
for (u32 i = 0; i < RAW_CODELENGHT+4; i++) // RAW_CODELENGHT = 20 / 26
|
||
{
|
||
BITTABLE[i] = 0.0f;
|
||
for (u32 j = 0; j < (1<<RAW_CODESAMPLEL_Po2)-0; j++) // RAW_CODESAMPLEL_Po2 = 3
|
||
BITTABLE[i] += *(p++);
|
||
}
|
||
p = BITTABLE;
|
||
u32 BitParser = ANCHOR_BSIZE - 1; // ANCHOR_BSIZE = 2
|
||
|
||
// Tests the content of the first packet of 8 pixels : It has to be black.
|
||
while (BitParser--)
|
||
if (*(p++) > -BIT_PREC) // BIT_PREC = 0.001f
|
||
return -1;
|
||
|
||
// Computes the Checksum of the following RAW_CODEBITNUM pixels packets :
|
||
// Determines the value of the RAW_CODEBITNUM/2 Checksum bits depending on the encountered pattern
|
||
// (BlackWhite -> 1, WhiteBlack -> 0)
|
||
// If pattern WhiteWhite or BlackBlack -> error.
|
||
for (BitParser = 0; BitParser < RAW_CODEBITNUM>>1; BitParser++,p+=2) // RAW_CODEBITNUM = 4 / 6
|
||
{
|
||
Checksum <<= 1;
|
||
|
||
if ( (*p<-BIT_PREC) && (*(p+1) > BIT_PREC) )
|
||
Checksum |= 1;
|
||
else
|
||
if ( ! ( (*p>-BIT_PREC) && (*(p+1) < BIT_PREC) ))
|
||
return -1;
|
||
}
|
||
|
||
// Computes the code on the RAW_CODEBITNUM*2 following packets of 8 pixels :
|
||
// Determines the values of the RAW_CODEBITNUM code bits depending on the encountered pattern
|
||
// (BlackWhite -> 1, WhiteBlack -> 0)
|
||
// If pattern WhiteWhite or BlackBlack -> error.
|
||
|
||
for (BitParser = 0; BitParser < RAW_CODEBITNUM ; BitParser++,p+=2) // RAW_CODEBITNUM = 4 / 6
|
||
{
|
||
CodeBBd <<= 1;
|
||
if ( (*p<-BIT_PREC) && (*(p+1) > BIT_PREC) )
|
||
CodeBBd |= 1;
|
||
else
|
||
if ( ! ( (*p>-BIT_PREC) && (*(p+1) < BIT_PREC) ))
|
||
return -1;
|
||
}
|
||
|
||
// Tests the 3 following packets of 8 pixels : They must be Black.
|
||
BitParser = ANCHOR_BSIZE + 1; // ANCHOR_BSIZE = 2
|
||
|
||
while (BitParser--) if (*(p++) > -BIT_PREC) return -1;
|
||
|
||
// Tests the following 4 packets of 8 pixels : They must be White.
|
||
BitParser = ANCHOR_WSIZE; // ANCHOR_WSIZE = 4
|
||
while (BitParser--) if (*(p++) < BIT_PREC) return -1;
|
||
|
||
Checksum ^= 0xAAAA;
|
||
Checksum &= (1<<(RAW_CODEBITNUM>>1)) -1 ;
|
||
if ((Checksum != ((CodeBBd ^ (CodeBBd >> (RAW_CODEBITNUM>>1))) & ((1<<(RAW_CODEBITNUM>>1))-1)))) return -1;
|
||
|
||
return CodeBBd;
|
||
} |