in SpatialUnderstanding/Src/PlaySpace/PlaySpaceInfos_W.cpp [1101:1380]
Bool PlaySpaceInfos_W::GetBestCentroidPosition(Vec3f& result, S32 granularity)
{
if (!m_pSurfaceInfosV3 || granularity <= 0 || (m_NbCellX < granularity) || (m_NbCellY < granularity))
return FALSE;
// Init
HS32DA tab2D;
S32 sizeX = m_NbCellX / granularity;
S32 sizeY = m_NbCellY / granularity;
Float sizeVox = m_SizeVoxel * granularity;
S32 cx = sizeX - 1;
S32 cz = sizeY - 1;
Float invSizeVoxel = 1.0f / sizeVox;
S32 size = sizeX * sizeY; // m_NbCellX * m_NbCellY;
tab2D.SetSize(size);
S32 *pTab2D = tab2D.GetArrayPtr();
memset(pTab2D, -1, tab2D.GetSize() * sizeof(S32));
Float t0 = GetAbsoluteTime();
// Add Seen Face Blocs.
S32 nb = 0;
Vec3f middlePos = VEC3F_NULL;
S32* pCurCell = pTab2D;
for (S32 z = 0; z < sizeY; z++)
{
S32 z2 = z * granularity;
for (S32 x = 0; x < sizeX; x++)
{
S32 x2 = x * granularity;
Bool empty = FALSE;
for (S32 i = 0; i < granularity; i++)
{
for (S32 j = 0; j < granularity; j++)
{
if (((x2 + i) < m_NbCellX) && ((z2 + j) < m_NbCellY))
{
PlaySpace_CellInfos *pCell = m_pSurfaceInfosV3->m_Surfels.GetCell(x2 + i, z2 + j);
if (!pCell || !pCell->pFirst)
empty = TRUE;
}
}
}
if (!empty)
{
*pCurCell = 0; // init with 0
Vec3f curPos = Vec3f((float(x) + 0.5f) * sizeVox, 1.0f, (float(z) + 0.5f) * sizeVox);
curPos += m_vMinAvailable;
middlePos += curPos;
nb++;
}
pCurCell++;
}
}
if (nb)
middlePos /= Float(nb);
Float t1 = GetAbsoluteTime();
////////////////////////////////////////////////////////// Compute distance to border
pCurCell = pTab2D;
for (S32 z = 0; z < sizeY; z++)
{
for (S32 x = 0; x < sizeX; x++)
{
if ((*pCurCell == 0) && ((x == 0) || (x == cx) || (z == 0) || (z == cz) || (pCurCell[-1] == -1) || (pCurCell[1] == -1) || (pCurCell[-sizeX] == -1) || (pCurCell[sizeX] == -1)))
*pCurCell = 0x10000;
pCurCell++;
}
}
S32 curVal = 0x10000;
Bool somethingToDo = TRUE;
while (somethingToDo)
{
somethingToDo = FALSE;
pCurCell = pTab2D;
for (S32 z = 0; z < sizeY; z++)
{
for (S32 x = 0; x < sizeX; x++)
{
if ((*pCurCell == 0) && ((pCurCell[-1] == curVal) || (pCurCell[1] == curVal) || (pCurCell[-sizeX] == curVal) || (pCurCell[sizeX] == curVal)))
{
*pCurCell = curVal + 0x10000;
somethingToDo = TRUE;
}
pCurCell++;
}
}
curVal += 0x10000;
}
Float t2 = GetAbsoluteTime();
////////////////////////////////////////////////////////// Compute corners visibility
S32 nbCorners = 0;
S32 max = 0;
pCurCell = pTab2D;
for (S32 z = 0; z < sizeY; z++)
{
U8 zmask = 0;
if (z == 0)
zmask |= 4;
else if (z == cz)
zmask |= 8;
for (S32 x = 0; x < sizeX; x++)
{
if (*pCurCell >= 0)
{
U8 xmask = zmask;
if (x == 0)
xmask |= 1;
else if (x == cx)
xmask |= 2;
U8 mask = xmask;
if (!(mask & 1) && (pCurCell[-1] < 0))
mask |= 1;
else if (!(mask & 2) && (pCurCell[1] < 0))
mask |= 2;
if (!(mask & 4) && (pCurCell[-sizeX] < 0))
mask |= 4;
else if (!(mask & 8) && (pCurCell[sizeX] < 0))
mask |= 8;
if ((mask != 5) && (mask != 9) && (mask != 6) && (mask != 10))
{
pCurCell++;
continue;
}
nbCorners++;
S32 x1 = ((xmask & 1) ? cx : 0);
S32 x2 = ((xmask & 2) ? 0 : cx);
S32 tx1_0 = (((xmask == 1) || (xmask == 9)) ? cx : 0);
S32 tx1_1 = (((xmask == 1) || (xmask == 5)) ? cx : 0);
S32 tx2_0 = (((xmask == 2) || (xmask == 10)) ? cx : 0);
S32 tx2_1 = (((xmask == 2) || (xmask == 6)) ? cx : 0);
S32 incr = (((xmask == 0) || (xmask == 4) || (xmask == 8)) ? cx : 1);
for (S32 dz = 0; dz < sizeY; dz++)
{
S32 xBeg = x1, xEnd = x2, xIncr = incr;
if (dz == 0)
{
xBeg ^= tx1_0;
xEnd ^= tx2_0;
if ((xmask == 0) || (xmask == 8))
xIncr = 1;
}
else if (dz == cz)
{
xBeg ^= tx1_1;
xEnd ^= tx2_1;
if ((xmask == 0) || (xmask == 4))
xIncr = 1;
}
for (S32 dx = xBeg; dx <= xEnd; dx += xIncr)
{
Bresenham2DInt_Z Bres;
Bres.InitSead(x, z, dx, dz);
S32 X, Y;
while (Bres.GetNextPointSead(X, Y))
{
if ((X >= 0) && (X <= cx) && (Y >= 0) && (Y <= cz))
{
S32& ptr = pTab2D[(Y * sizeX) + X];
if (ptr < 0)
break;
if (!(ptr & 0x40000000)) // if bit 30 not set
{
ptr += 0x40000001;
if ((ptr & 0xFFFF) > max)
max = ptr & 0xFFFF;
}
}
}
}
}
S32* pCurCell2 = pTab2D;
for (S32 i = 0; i < size; i++)
{
if (*pCurCell2 > 0)
*pCurCell2 &= 0x3FFFFFFF; // reset bit 30 if bit 31 not set
pCurCell2++;
}
}
pCurCell++;
}
}
S32 max1 = max, max2 = max >> 1;
if (max > 3)
max1 = max - 1;
if (max2 < 2)
max2 = 2;
Float t3 = GetAbsoluteTime();
//////////////////////////////////////////////////////////
nb = 0;
Vec3f centerPos = VEC3F_NULL;
pCurCell = pTab2D;
for (S32 z = 0; z < sizeY; z++)
{
for (S32 x = 0; x < sizeX; x++)
{
if ((*pCurCell & 0xFFFF) <= max1)
{
Vec3f curPos = Vec3f((float(x) + 0.5f) * sizeVox, 1.0f, (float(z) + 0.5f) * sizeVox);
curPos += m_vMinAvailable;
centerPos += curPos;
nb++;
//DRAW_DEBUG_SPHERE3D(pos, COLOR_GREEN * 0.99f, 0.05f, .displayDuration(2.f));
}
pCurCell++;
}
}
if (nb)
centerPos /= Float(nb);
Vec3f bestPos = (middlePos + centerPos) * 0.5f;
S32 centerX = FLOORINT(((bestPos.x - m_vMinAvailable.x) * invSizeVoxel) + 0.5f);
S32 centerY = FLOORINT(((bestPos.z - m_vMinAvailable.z) * invSizeVoxel) + 0.5f);
S32* ptr = pTab2D + ((centerY * sizeX) + centerX);
if ((centerX >= 0) && (centerX <= cx) && (centerY >= 0) && (centerY <= cz) && (*ptr >= 0) && ((*ptr & 0xFFFF) >= max2) && ((*ptr >> 16) > 10))
{
//DRAW_DEBUG_SPHERE3D(bestPos, COLOR_BLUE, 0.1f, .displayDuration(5.f));
result = bestPos;
return TRUE;
}
Bool ok = FALSE;
Vec3f pos;
Float minDist = 1e8f;
pCurCell = pTab2D;
for (S32 z = 0; z < sizeY; z++)
{
for (S32 x = 0; x < sizeX; x++)
{
if ((*pCurCell >= 0) && ((*pCurCell & 0xFFFF) >= max2) && ((*pCurCell >> 16) > 10))
{
Vec3f curPos = Vec3f((float(x) + 0.5f) * sizeVox, 1.0f, (float(z) + 0.5f) * sizeVox);
curPos += m_vMinAvailable;
Float dist = (curPos - bestPos).GetNorm2();
if (dist < minDist)
{
pos = curPos;
minDist = dist;
ok = TRUE;
}
}
pCurCell++;
}
}
if (!ok)
{
pCurCell = pTab2D;
for (S32 z = 0; z < sizeY; z++)
{
for (S32 x = 0; x < sizeX; x++)
{
if ((*pCurCell >= 0) && ((*pCurCell & 0xFFFF) >= max2) && ((*pCurCell >> 16) > 5))
{
Vec3f curPos = Vec3f((float(x) + 0.5f) * sizeVox, 1.0f, (float(z) + 0.5f) * sizeVox);
curPos += m_vMinAvailable;
Float dist = (curPos - bestPos).GetNorm2();
if (dist < minDist)
{
pos = curPos;
minDist = dist;
ok = TRUE;
}
}
pCurCell++;
}
}
}
if (ok)
{
result = pos;
//DRAW_DEBUG_SPHERE3D(pos, COLOR_RED, 0.1f, .displayDuration(5.f));
}
Float t4 = GetAbsoluteTime();
//OUTPUT_Z("CENTROID !!! %.3f %.3f %.3f %.3f",t4-t3,t3-t2,t2-t1,t1-t0);
return ok;
}