424 lines
No EOL
13 KiB
C++
424 lines
No EOL
13 KiB
C++
#if 1
|
|
#include <stdio.h>
|
|
|
|
#include "math.h"
|
|
#include "memory.h"
|
|
#include "CCMath.h"
|
|
#include "CCmap.h"
|
|
|
|
|
|
//#pragma optimize("",off)
|
|
#define PLANE_TileSize 18
|
|
#define PRIMESENSFOV 58.512135f
|
|
#define PRIMESENSDISTORTION_Y 1.0f
|
|
#define EcartType_Tolerance 2.5f
|
|
#define DY_Z_Tolerance -1.0f
|
|
#define XOZ 655360.0f
|
|
|
|
#define Focale (1.f / tanf(PRIMESENSFOV / 2.0f * 3.1415927f / 180.0f))
|
|
|
|
//#define DRAWGREENTILES
|
|
#define DRAWGROUND
|
|
|
|
Vector PRIMESENS_GroundZ;
|
|
float PRIMESENS_GroundDistance = 0.0f;
|
|
|
|
double MEGADX = 0;
|
|
double MEGADY = 0;
|
|
Vector MEGADZ = Vector(0,0,0);
|
|
|
|
|
|
u32 IsNan(float F);
|
|
void MAP_PPRIMESENS_ReLightWithZ(u16 *ZOoMap , MAP *pColorMap , MAP *pShadowMAP , Vector *GCenter , float DX, float DY);
|
|
|
|
void PRIMESENS_3D_to_2D_Coordinate(MAP *ZMap, Vector *p3D , Vector *p2D)
|
|
{
|
|
p2D->z = p3D->z;
|
|
p2D->x = ((float)ZMap->SX/2.0f) + p3D->x * (((float)ZMap->SX/2.0f) * Focale) / p3D->z ;
|
|
p2D->y = ((float)ZMap->SY/2.0f) + p3D->y * (((float)ZMap->SX/2.0f) * Focale) / p3D->z ;
|
|
}
|
|
void PRIMESENS_2D_to_3D_Coordinate(MAP *ZMap, Vector *p2D , Vector *p3D)
|
|
{
|
|
p3D->z = p2D->z;
|
|
p3D->x = (p2D->x - ((float)ZMap->SX/2.0f)) * p3D->z / (((float)ZMap->SX/2.0f) * Focale);
|
|
p3D->y = (p2D->y - ((float)ZMap->SY/2.0f)) * p3D->z / (((float)ZMap->SX/2.0f) * Focale);
|
|
}
|
|
|
|
|
|
float ComputeDXDYAndEcarType(MAP *ZMap,int X,int Y, float &DX, float &DY , Vector &GCenter , float *Ponderation /* PLANE_TileSize * PLANE_TileSize */, float Threshold,u32 &Counter)
|
|
{
|
|
float Pond= 0;
|
|
s32 * pBASE = ZMap->GetBase();
|
|
/* 1 Comppute DX - DY */
|
|
Counter = 0;
|
|
GCenter = Vector(0,0,0);
|
|
|
|
for (int XT = 0 ; XT < PLANE_TileSize ; XT++ )
|
|
{
|
|
for (int YT = 0 ; YT < PLANE_TileSize ; YT++ )
|
|
{
|
|
s32 *pBase = pBASE + X + XT + (Y + YT) * ZMap ->PITCH;
|
|
if (*(pBase))
|
|
{
|
|
GCenter += Vector(XT , YT , *(pBase)) * Ponderation[XT + YT * PLANE_TileSize] ;
|
|
Pond += Ponderation[XT + YT * PLANE_TileSize];
|
|
if (Ponderation[XT + YT * PLANE_TileSize])
|
|
Counter++;
|
|
}
|
|
}
|
|
}
|
|
if (Pond )
|
|
{
|
|
|
|
GCenter *= 1.0f / (float)Pond;
|
|
DX = DY = 0.0f;
|
|
Pond = 0.0f;
|
|
for (int XT = 0 ; XT < PLANE_TileSize ; XT++ )
|
|
{
|
|
for (int YT = 0 ; YT < PLANE_TileSize ; YT++ )
|
|
{
|
|
s32 *pBase = pBASE + X + XT + (Y + YT) * ZMap ->PITCH;
|
|
if (*(pBase))
|
|
{
|
|
float Factor,Distance;
|
|
Distance = ((float)XT - GCenter.x) * ((float)XT - GCenter.x) + ((float)YT - GCenter.y) * ((float)YT - GCenter.y);
|
|
if (Distance)
|
|
{
|
|
//Distance = sqrt(Distance) ;
|
|
DX += ((float)XT - GCenter.x) * ((float)*(pBase) - GCenter.z) * Ponderation[XT + YT * PLANE_TileSize];
|
|
DY += ((float)YT - GCenter.y) * ((float)*(pBase) - GCenter.z) * Ponderation[XT + YT * PLANE_TileSize];
|
|
Pond += Distance * Ponderation[XT + YT * PLANE_TileSize];//= abs(((float)*(pBase) - GCenter.z));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
DX /= 0.5f * (float)Pond;
|
|
DY /= 0.5f * (float)Pond;//*/
|
|
Pond = 0;
|
|
float SqrValue = 0.0f , Value = 0.0f;
|
|
for (int XT = 0 ; XT < PLANE_TileSize ; XT++ )
|
|
for (int YT = 0 ; YT < PLANE_TileSize ; YT++ )
|
|
{
|
|
s32 *pBase = pBASE + X + XT + (Y + YT) * ZMap ->PITCH;
|
|
if (*(pBase))
|
|
{
|
|
float Distance = *(pBase) - ((((float)XT - GCenter.x) * DX + ((float)YT - GCenter.y) * DY + GCenter.z));
|
|
SqrValue += Distance * Distance * Ponderation[XT + YT * PLANE_TileSize];
|
|
Value += Distance * Ponderation[XT + YT * PLANE_TileSize];
|
|
Pond+=Ponderation[XT + YT * PLANE_TileSize];
|
|
if (Distance > Threshold)
|
|
Ponderation[XT + YT * PLANE_TileSize] = 0;
|
|
else
|
|
Ponderation[XT + YT * PLANE_TileSize] = 1;
|
|
}
|
|
}
|
|
SqrValue /= Pond;
|
|
Value /= Pond;
|
|
return sqrtf(SqrValue - Value * Value);;
|
|
} else return 10000000.0f;
|
|
}
|
|
u16 ZMAXMAP[640*480];
|
|
void MAP_ComputePlanes(MAP *ZMap,u16 *PrimsensZMapSave)
|
|
{
|
|
static u32 BasicTimeStamp = 0;
|
|
u32 TIMESTAMPAND = (BasicTimeStamp < 16) ? 15 : (BasicTimeStamp < 64) ? 63 : (BasicTimeStamp < 1000) ? 127 : 511;
|
|
/* for (int X = 0 ; X < ZMap->SX - PLANE_TileSize; X+=PLANE_TileSize )
|
|
for (int Y = 0 ; Y < ZMap->SY - PLANE_TileSize ; Y+=PLANE_TileSize )*/
|
|
if ((BasicTimeStamp & TIMESTAMPAND) == 0) memset(ZMAXMAP,0,sizeof(ZMAXMAP));
|
|
u32 CCounter = ZMap->SX * ZMap->SY;
|
|
u16 *PrimsensZMap = PrimsensZMapSave;
|
|
u16 *PrimsensZMapDst = ZMAXMAP;
|
|
while (CCounter--)
|
|
{
|
|
if (*PrimsensZMap && (*PrimsensZMap > *PrimsensZMapDst))
|
|
*PrimsensZMapDst = *PrimsensZMap;
|
|
PrimsensZMap++;
|
|
PrimsensZMapDst++;
|
|
}
|
|
BasicTimeStamp++;
|
|
if ((BasicTimeStamp & TIMESTAMPAND) == TIMESTAMPAND) return;
|
|
PrimsensZMap = ZMAXMAP;
|
|
float Ponderation[PLANE_TileSize * PLANE_TileSize];
|
|
Vector GroundVectorStack[8192];
|
|
Vector GroundVectorStackDXDY[8192];
|
|
u32 ulNumberOfGVS = 0;
|
|
s32 * pBASE = ZMap->GetBase();
|
|
{
|
|
s32 *pBase;
|
|
pBase = pBASE + ZMap->SX * ZMap->SY;
|
|
for (s32 Y = ZMap->SY-1 ;Y>=0 ; Y--)
|
|
for (s32 X = ZMap->SX-1 ;X>=0 ; X--)
|
|
{
|
|
pBase--;
|
|
if (*PrimsensZMap)
|
|
*(pBase) = XOZ / (s32)*PrimsensZMap;
|
|
else
|
|
*(pBase) = 0;
|
|
PrimsensZMap++;
|
|
}
|
|
PrimsensZMap -= ZMap->SX * ZMap->SY;
|
|
}
|
|
|
|
for (int X = 0 ; X < ZMap->SX - PLANE_TileSize; X+=PLANE_TileSize )
|
|
for (int Y = 0 ; Y < ZMap->SY - PLANE_TileSize ; Y+=PLANE_TileSize)
|
|
{
|
|
float EcartType = 10000000000.0;
|
|
|
|
float DX = 0 , DY = 0 ;
|
|
u32 Counter = PLANE_TileSize * PLANE_TileSize;
|
|
while (Counter--) Ponderation[Counter] = 1.f;
|
|
|
|
Vector GCenter = Vector(0,0,0);
|
|
for (int ITER = 0 ; ITER < 1 ; ITER++)
|
|
{
|
|
EcartType = ComputeDXDYAndEcarType(ZMap,X,Y, DX, DY , GCenter ,Ponderation,EcartType * 2 + EcartType_Tolerance,Counter);
|
|
}
|
|
if ((EcartType < EcartType_Tolerance) && (DY < DY_Z_Tolerance) && (Counter > (PLANE_TileSize * PLANE_TileSize)>>3) )
|
|
{
|
|
GroundVectorStackDXDY[ulNumberOfGVS].x = DX;
|
|
GroundVectorStackDXDY[ulNumberOfGVS].y = DY;
|
|
GroundVectorStack[ulNumberOfGVS++] = Vector (GCenter.x + X ,GCenter.y + Y,GCenter.z );
|
|
#ifdef DRAWGREENTILES
|
|
for (int XT = 0 ; XT < PLANE_TileSize ; XT++ )
|
|
for (int YT = 0 ; YT < PLANE_TileSize ; YT++ )
|
|
{
|
|
s32 *pBase = pBASE + X + XT + (Y + YT) * ZMap ->PITCH;
|
|
//if (*(pBase))
|
|
{
|
|
*(pBase) = 0xff00ff00;
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
}
|
|
|
|
if (ulNumberOfGVS < 3)
|
|
{
|
|
//MAP_PPRIMESENS_ReLightWithZ(PrimsensZMap, ZMap, NULL, &MEGADZ, MEGADX , MEGADY);
|
|
return;
|
|
}
|
|
/* Then treat GVS to Extract Ground Matrix */
|
|
Vector GravityCenter = Vector (0,0,0);
|
|
static float LastAverage = 10000.0f;
|
|
float NewAverage = 0.0f;
|
|
float DX = 0 , DY = 0 ;
|
|
{
|
|
float Pond;
|
|
Pond = 0.0f;
|
|
for (int K = 0 ; K < ulNumberOfGVS ; K ++)
|
|
{
|
|
float Factor = (sqrt(((MEGADX - GroundVectorStackDXDY[K].x)*(MEGADX - GroundVectorStackDXDY[K].x)
|
|
+ (MEGADY - GroundVectorStackDXDY[K].y)*(MEGADY - GroundVectorStackDXDY[K].y)) + 0.1f));
|
|
NewAverage += Factor;
|
|
/* if (Factor < LastAverage * 2)
|
|
Factor = LastAverage * 2;*/
|
|
Factor = 1.0f / Factor;
|
|
/* float Distance = *(pBase) - ((((float)X - GravityCenter.x) * DX + ((float)Y - GravityCenter.y) * DY + GravityCenter.z));
|
|
if (fabs(Distance) < EcartType_Tolerance * 4.0f) NewNumber++;*/
|
|
|
|
GravityCenter += GroundVectorStack[K] * Factor;
|
|
DX += Factor * GroundVectorStackDXDY[K].x;
|
|
DY += Factor * GroundVectorStackDXDY[K].y;
|
|
Pond+=Factor;//*/
|
|
/*
|
|
float Factor,Distance;
|
|
Distance = (GroundVectorStack[K].x - GravityCenter.x) * (GroundVectorStack[K].x - GravityCenter.x) +
|
|
(GroundVectorStack[K].y - GravityCenter.y) * (GroundVectorStack[K].y - GravityCenter.y);
|
|
if (Distance)
|
|
{
|
|
Distance = 1.0f/ (Distance) ;
|
|
DX += Distance*GroundVectorStackDXDY[ulNumberOfGVS].x;
|
|
DY += Distance*GroundVectorStackDXDY[ulNumberOfGVS].y;
|
|
Pond += Distance;
|
|
}*/
|
|
}
|
|
|
|
LastAverage = NewAverage / (float)ulNumberOfGVS;
|
|
GravityCenter *= 1.0f / Pond;
|
|
DX /= Pond;
|
|
DY /= Pond;//*/
|
|
|
|
u32 LastNumber = 0, NewNumber = 0;
|
|
|
|
float NewDX = DX,NewDY = DY;
|
|
Vector NewGravity;
|
|
NewGravity = GravityCenter;
|
|
for (int X = 0 ; X < ZMap->SX; X++)
|
|
for (int Y = 0 ; Y < ZMap->SY ; Y++)
|
|
{
|
|
s32 *pBase = pBASE + X + (Y ) * ZMap ->PITCH;
|
|
if (*pBase)
|
|
{
|
|
float Distance = *(pBase) - ((((float)X - GravityCenter.x) * DX + ((float)Y - GravityCenter.y) * DY + GravityCenter.z));
|
|
if (fabs(Distance) < EcartType_Tolerance * 2.0f) LastNumber++;
|
|
}
|
|
}
|
|
|
|
if (Pond && !IsNan(DX) && !IsNan(DY))
|
|
{
|
|
if (abs(MEGADX) > 1000000.0f) MEGADX = DX;
|
|
if (abs(MEGADY) > 1000000.0f) MEGADY = DY;
|
|
if ((BasicTimeStamp & TIMESTAMPAND) > TIMESTAMPAND - (TIMESTAMPAND>>2))
|
|
{
|
|
MEGADX = DX;
|
|
MEGADY = DY;
|
|
MEGADZ = GravityCenter;
|
|
}
|
|
DX = MEGADX;
|
|
DY = MEGADY;
|
|
GravityCenter = MEGADZ;
|
|
}//*/
|
|
|
|
for (int X = 0 ; X < ZMap->SX; X++)
|
|
for (int Y = 0 ; Y < ZMap->SY ; Y++)
|
|
{
|
|
s32 *pBase = pBASE + X + (Y ) * ZMap ->PITCH;
|
|
if (*pBase)
|
|
{
|
|
float Distance = *(pBase) - ((((float)X - GravityCenter.x) * DX + ((float)Y - GravityCenter.y) * DY + GravityCenter.z));
|
|
if (fabs(Distance) < EcartType_Tolerance * 4.0f) NewNumber++;
|
|
}
|
|
}
|
|
|
|
if (NewNumber << 3 < LastNumber )
|
|
{
|
|
MEGADX = DX = NewDX;
|
|
MEGADY = DY = NewDY;
|
|
MEGADZ = GravityCenter = NewGravity;
|
|
}
|
|
#ifdef DRAWGROUND
|
|
float DX2 = DX,DY2 = DY;
|
|
DX2 *= 10.0f;
|
|
DY2 *= 10.0f;
|
|
DX2 += 128.0f;
|
|
DY2 += 128.0f;
|
|
if(DX2 > 255) DX2 = 255;
|
|
if(DY2 > 255) DY2 = 255;
|
|
if(DX2 < 0) DX2 = 0;
|
|
if(DY2 < 0) DY2 = 0;
|
|
u32 Color = DX2 + ((u32)DY2<<8);
|
|
|
|
PrimsensZMapSave = (u16*)ZMAXMAP;
|
|
s32 * pBASE = ZMap->GetBase();
|
|
{
|
|
s32 *pBase;
|
|
pBase = pBASE + ZMap->SX * ZMap->SY;
|
|
for (s32 Y = ZMap->SY-1 ;Y>=0 ; Y--)
|
|
for (s32 X = ZMap->SX-1 ;X>=0 ; X--)
|
|
{
|
|
pBase--;
|
|
if ((*pBase & 0xff000000) == 0)
|
|
{
|
|
if (*PrimsensZMapSave)
|
|
*(pBase) = XOZ / (s32)*PrimsensZMapSave;
|
|
else
|
|
*(pBase) = 0;
|
|
}
|
|
PrimsensZMapSave++;
|
|
}
|
|
PrimsensZMapSave -= ZMap->SX * ZMap->SY;
|
|
}
|
|
//PRIMESENS_GroundDistance = XOZ / pBASE[(ZMap->SX>>1) + ZMap->PITCH * (ZMap->SY>>1)];
|
|
for (int X = 0 ; X < ZMap->SX; X++)
|
|
for (int Y = 0 ; Y < ZMap->SY-11 ; Y++)
|
|
{
|
|
float TheoricalD;
|
|
s32 *pBase = pBASE + X + (Y+10)* ZMap ->PITCH;
|
|
float TheoricalD2 = ((((float)X - GravityCenter.x) * DX + ((float)(Y+10) - GravityCenter.y) * DY + GravityCenter.z));
|
|
|
|
pBase = pBASE + X + Y* ZMap ->PITCH;
|
|
TheoricalD = ((((float)X - GravityCenter.x) * DX + ((float)Y - GravityCenter.y) * DY + GravityCenter.z));
|
|
//void PRIMESENS_2D_to_3D_Coordinate(MAP *ZMap, Vector *p2D , Vector *p3D)
|
|
/* if ((TheoricalD > 0) && (TheoricalD2<0))//fabs(Distance) < EcartType_Tolerance * 2)
|
|
{
|
|
*pBase = 0xffffffff;
|
|
} else
|
|
*pBase = 0;*/
|
|
|
|
if (*pBase)
|
|
{
|
|
float Distance = *(pBase) - TheoricalD;
|
|
if (TheoricalD > 0)//fabs(Distance) < EcartType_Tolerance * 2)
|
|
{
|
|
*pBase = Color;
|
|
*pBase = 0xffffffff;
|
|
} else
|
|
if ((*pBase & 0xff000000) == 0)
|
|
{
|
|
u8 *pImage = (u8 *)pBase;
|
|
s32 Value = 2024*fabs(Distance) / *pBase;
|
|
pImage[0] = (Value>>0) & 0xff;
|
|
if ((Value >>0) & 256) pImage[0] ^= 0xff;
|
|
pImage[1] = (Value>>2) & 0xff;
|
|
if ((Value >>2) & 256) pImage[1] ^= 0xff;
|
|
pImage[2] = (Value>>4) & 0xff;
|
|
if ((Value >>3) & 256) pImage[2] ^= 0xff;
|
|
if (Value & 0x100)
|
|
pImage[2] ^= 0xff;
|
|
}
|
|
}
|
|
}
|
|
u8 TEXT[512];
|
|
sprintf((char*)TEXT,"Camera Z = %.2f m ",PRIMESENS_GroundDistance/1000);
|
|
DRawTEXT(TEXT,(u32*)ZMap->GetBase(),10,10,ZMap->PITCH,ZMap->SY,0xff00ffff);
|
|
//pBASE[(ZMap->SX>>1) + ZMap->PITCH * (ZMap->SY>>1)]= 0xff;
|
|
|
|
#endif
|
|
}
|
|
|
|
/* Compute RealZ of Plane */
|
|
{
|
|
Vector Point4[4];
|
|
/*
|
|
2 3
|
|
0 1
|
|
|
|
XE = M->SX/2 + M->SX/2 * Focale * XW / ZW
|
|
XW = (XE - M->SX/2) * ZW / (M->SX/2 * Focale)
|
|
*/
|
|
DY *= PRIMESENSDISTORTION_Y;
|
|
GravityCenter.y *= PRIMESENSDISTORTION_Y;
|
|
|
|
float FX,FY,CosZ,Z;
|
|
Vector GravityCenterW;
|
|
|
|
Point4[0].z = (float)XOZ / GravityCenter.z;
|
|
Point4[0].x = (GravityCenter.x - ((float)ZMap->SX/2.0f)) * Point4[0].z / (((float)ZMap->SX/2.0f) * Focale);
|
|
Point4[0].y = (GravityCenter.y - ((float)ZMap->SY/2.0f)) * Point4[0].z / (((float)ZMap->SX/2.0f) * Focale);
|
|
|
|
GravityCenterW = GravityCenter;
|
|
GravityCenterW.x += DX / 10.0f;
|
|
GravityCenterW.y += DY / 10.0f;
|
|
GravityCenterW.z = (((GravityCenterW.x - GravityCenter.x) * DX + (GravityCenterW.y - GravityCenter.y) * DY + GravityCenter.z));
|
|
Point4[1].z = (float)XOZ / GravityCenterW.z;
|
|
Point4[1].x = (GravityCenterW.x - ((float)ZMap->SX/2.0f)) * Point4[1].z / (((float)ZMap->SX/2.0f) * Focale);
|
|
Point4[1].y = (GravityCenterW.y - ((float)ZMap->SY/2.0f)) * Point4[1].z / (((float)ZMap->SX/2.0f) * Focale);
|
|
|
|
GravityCenterW = GravityCenter;
|
|
GravityCenterW.x += DY / 10.0f;
|
|
GravityCenterW.y -= DX / 10.0f;
|
|
GravityCenterW.z = (((GravityCenterW.x - GravityCenter.x) * DX + (GravityCenterW.y - GravityCenter.y) * DY + GravityCenter.z));
|
|
Point4[2].z = (float)XOZ / GravityCenterW.z;
|
|
Point4[2].x = (GravityCenterW.x - ((float)ZMap->SX/2.0f)) * Point4[2].z / (((float)ZMap->SX/2.0f) * Focale);
|
|
Point4[2].y = (GravityCenterW.y - ((float)ZMap->SY/2.0f)) * Point4[2].z / (((float)ZMap->SX/2.0f) * Focale);
|
|
|
|
//CosZ =
|
|
|
|
Vector AXIS1 = Point4[2] - Point4[0];
|
|
Vector AXIS2 = Point4[1] - Point4[0];
|
|
|
|
PRIMESENS_GroundZ = AXIS1 ^ AXIS2;
|
|
PRIMESENS_GroundZ = PRIMESENS_GroundZ * -1.0;
|
|
//PRIMESENS_GroundZ.z /= 2.0f;
|
|
PRIMESENS_GroundZ.Normalize();
|
|
|
|
static float Eval1 = 100.0f;
|
|
PRIMESENS_GroundDistance = Point4[0] * PRIMESENS_GroundZ;
|
|
if (PRIMESENS_GroundDistance < 0) PRIMESENS_GroundDistance = -PRIMESENS_GroundDistance;
|
|
}
|
|
|
|
//MAP_PPRIMESENS_ReLightWithZ(PrimsensZMap, ZMap, NULL, &GravityCenter, DX , DY);
|
|
}
|
|
|
|
|
|
#endif |