in SpatialUnderstanding/Src/PlaySpace/PlaySpace_Mesh_W.cpp [4734:5193]
void Playspace_Mesh::PlanarFilter_AccurateNew()
{
ComputePointsLinks();
ComputeFacesToolNormal();
ComputePointsToolNormal(); // Compute Face too.
InvalidatePointsToolNormal();
InvalidateFaceToolNormal();
S32 NbFaces = m_TabQuad.GetSize();
S32 NbPoints = m_TabPoints.GetSize();
HugeDynArray_Z<Vec3f, 8, FALSE, FALSE> TabLocalPoints;
HugeDynArray_Z<PlanarInfos, 8, FALSE, FALSE> TabInfosPlanar;
DynArray_Z<ReprojectInfos,8,FALSE,FALSE> TabRepro;
DynArray_Z<GroupInfos,8,FALSE,FALSE> TabGroups;
TabInfosPlanar.SetSize(NbPoints);
// Init Planar Datas.
PlanarInfos *pInfos = TabInfosPlanar.GetArrayPtr();
for (S32 i = 0; i<NbPoints; i++)
{
pInfos->IdNext = -1;
pInfos->IsDone = 0;
pInfos++;
}
// First : Create Groups
Float MinError = Cos(DegToRad(10.f));
Float ForceUpError = Cos(DegToRad(15.f));
Float ForceVertError = Sin(DegToRad(15.f));
Float MergeDist = 0.15f;
Float MergeAngle = Cos(DegToRad(25.f));
for (S32 i = 0; i<NbPoints; i++)
{
PlanarInfos *pCurInfos = &TabInfosPlanar[i];
if (pCurInfos->IsDone)
continue;
if (m_TabPointsToolNormal[i].Error < MinError)
continue;
S32 CurPoint = i;
S32 FirstPoint = i;
S32 LastPoint = i;
S32 NbPointsOnZone = 1;
S32 CurGroupId = TabGroups.GetSize();
TabInfosPlanar[CurPoint].IsDone = 1;
TabInfosPlanar[CurPoint].GroupId = CurGroupId;
TabInfosPlanar[CurPoint].IdNext = -1;
Vec3f SumNormal = m_TabPointsToolNormal[i].Normal;
Vec3f SumNormalN = SumNormal;
Vec3f Center = m_TabPoints[i];
Vec3f CenterN = Center;
Float CenterWeight = 1.f;
while (CurPoint >= 0)
{
// Check Voisins.
Playspace_Mesh::PointsLinks &PointLinks = m_TabPointsLinks[CurPoint];
Playspace_Mesh::ToolPointNormal &PointNormalInfos = m_TabPointsToolNormal[CurPoint];
Vec3f MyPos = m_TabPoints[CurPoint];
Vec3f MyNormal = PointNormalInfos.Normal;
S32 NbFaces = PointLinks.GetNbFaces();
for (S32 j = 0; j<NbFaces; j++)
{
// Same surface ?
S32 LinkedFace = PointLinks.GetFace(j);
Playspace_Mesh::Face &CurFace = m_TabQuad[LinkedFace];
S32 NbTriPoints = CurFace.IsTri ? 3 : 4;
for (S32 f = 0; f<NbTriPoints; f++)
{
// AddPoint ?
S32 OtherPoint = CurFace.TabPoints[f];
if (TabInfosPlanar[OtherPoint].IsDone)
continue;
// CheckContinuity.
Playspace_Mesh::ToolPointNormal &OtherNormalInfos = m_TabPointsToolNormal[OtherPoint];
Vec3f &OtherNormal = OtherNormalInfos.Normal;
if ((SumNormalN * OtherNormal) < MergeAngle)
continue;
// Check Plane Dist.
Vec3f OtherPos = m_TabPoints[OtherPoint];
Vec3f vDelta = OtherPos - CenterN;
Float AbsProj = Abs(SumNormalN * vDelta);
if (AbsProj >= MergeDist)
continue;
// Add Point.
Float Surface = OtherNormalInfos.Surface * (1.f / (0.04f * 0.04f));
SumNormal += OtherNormal * Surface;
SumNormalN = SumNormal;
SumNormalN.CNormalize();
Center += OtherPos * Surface;
CenterWeight += Surface;
CenterN = Center * (1.f / CenterWeight);
NbPointsOnZone++;
TabInfosPlanar[OtherPoint].IsDone = 1;
TabInfosPlanar[OtherPoint].GroupId = CurGroupId;
TabInfosPlanar[OtherPoint].IdNext = -1;
TabInfosPlanar[LastPoint].IdNext = OtherPoint;
LastPoint = OtherPoint;
}
}
// Next Point !
CurPoint = TabInfosPlanar[CurPoint].IdNext;
}
// Manage it...
if (NbPointsOnZone < 30)
{
// Clear All.
CurPoint = FirstPoint;
while (CurPoint >= 0)
{
TabInfosPlanar[CurPoint].IsDone = 0;
TabInfosPlanar[CurPoint].GroupId = -1;
CurPoint = TabInfosPlanar[CurPoint].IdNext;
}
continue;
}
// Compute Plane.
TabLocalPoints.SetSize(NbPointsOnZone, TRUE);
CurPoint = FirstPoint;
S32 CurPos = 0;
while (CurPoint >= 0)
{
TabLocalPoints[CurPos++] = m_TabPoints[CurPoint];
CurPoint = TabInfosPlanar[CurPoint].IdNext;
}
Vec3f eigenvectors[3];
Float eigenvalues[3];
GenericEigen3D(TabLocalPoints.GetArrayPtr(), CurPos, eigenvectors, eigenvalues);
// Sort Eigen
if (eigenvalues[2] > eigenvalues[1])
{
::Swap(eigenvalues[2], eigenvalues[1]);
::Swap(eigenvectors[2], eigenvectors[1]);
}
if (eigenvalues[1] > eigenvalues[0])
{
::Swap(eigenvalues[0], eigenvalues[1]);
::Swap(eigenvectors[0], eigenvectors[1]);
}
if (eigenvalues[2] > eigenvalues[1])
{
::Swap(eigenvalues[2], eigenvalues[1]);
::Swap(eigenvectors[2], eigenvectors[1]);
}
if (eigenvalues[1] < 0.2f)
{
SumNormalN = eigenvectors[0] ^ eigenvectors[1];
SumNormalN.ANormalize();
}
if (SumNormalN.y > ForceUpError)
SumNormalN = VEC3F_UP;
else if (SumNormalN.y < -ForceUpError)
SumNormalN = VEC3F_DOWN;
if (Abs(SumNormalN.y) < ForceVertError)
{
SumNormalN.y = 0.f;
SumNormalN.ANormalize();
}
// Create Group.
S32 NewGroup = TabGroups.Add();
EXCEPTIONC_Z(CurGroupId == NewGroup,"LAAAAAAAAAAAAAA");
TabGroups[NewGroup].Normal = SumNormalN;
TabGroups[NewGroup].Center = CenterN;
TabGroups[NewGroup].IdFirst = FirstPoint;
TabGroups[NewGroup].IdLast = LastPoint;
TabGroups[NewGroup].Nb = NbPointsOnZone;
}
// Merge little noise.
S32DA TabConnectGroup;
for (S32 i = 0; i<NbPoints; i++)
{
PlanarInfos *pCurInfos = &TabInfosPlanar[i];
if (pCurInfos->IsDone)
continue;
S32 CurPoint = i;
S32 FirstPoint = i;
S32 LastPoint = i;
S32 NbPointsOnZone = 1;
TabInfosPlanar[CurPoint].IsDone = 1;
TabInfosPlanar[CurPoint].GroupId = -1;
TabInfosPlanar[CurPoint].IdNext = -1;
Vec3f vMin = m_TabPoints[i];
Vec3f vMax = vMin;
TabConnectGroup.SetSize(0,TRUE);
while (CurPoint >= 0)
{
// Check Voisins.
Playspace_Mesh::PointsLinks &PointLinks = m_TabPointsLinks[CurPoint];
S32 NbFaces = PointLinks.GetNbFaces();
for (S32 j = 0; j<NbFaces; j++)
{
// Same surface ?
S32 LinkedFace = PointLinks.GetFace(j);
Playspace_Mesh::Face &CurFace = m_TabQuad[LinkedFace];
S32 NbTriPoints = CurFace.IsTri ? 3 : 4;
for (S32 f = 0; f<NbTriPoints; f++)
{
// AddPoint ?
S32 OtherPoint = CurFace.TabPoints[f];
if (TabInfosPlanar[OtherPoint].IsDone)
{
S32 OtherGroupId = TabInfosPlanar[OtherPoint].GroupId;
if (OtherGroupId >= 0)
{
if (TabConnectGroup.Contains(OtherGroupId) < 0)
TabConnectGroup.Add(OtherGroupId);
}
continue;
}
Vec3f pos = m_TabPoints[OtherPoint];
vMin = Min(pos,vMin);
vMax = Max(pos,vMax);
NbPointsOnZone++;
TabInfosPlanar[OtherPoint].IsDone = 1;
TabInfosPlanar[OtherPoint].GroupId = -1;
TabInfosPlanar[OtherPoint].IdNext = -1;
TabInfosPlanar[LastPoint].IdNext = OtherPoint;
LastPoint = OtherPoint;
}
}
// Next Point !
CurPoint = TabInfosPlanar[CurPoint].IdNext;
}
S32 CurGroup = -1;
S32 BetterNb = 0;
Float BetterDist = 1e6f;
for (S32 j=0 ; j<TabConnectGroup.GetSize() ; j++)
{
Vec3f Normal = TabGroups[TabConnectGroup[j]].Normal;
Vec3f Center = TabGroups[TabConnectGroup[j]].Center;
Float d1 = (vMax-Center) * Normal;
Float d2 = (Center-vMin) * Normal;
Float d = Max(Abs(d1),Abs(d2));
S32 Nb = TabGroups[TabConnectGroup[j]].Nb;
if ((Nb > BetterNb) && (d < 0.2f))
{
BetterNb = Nb;
BetterDist = d;
CurGroup = TabConnectGroup[j];
}
}
if (CurGroup < 0)
continue;
if (TabConnectGroup.GetSize() == 1)
{
if (BetterDist > 0.2f)
continue;
}
else if (TabConnectGroup.GetSize() == 2)
{
if (BetterDist > 0.1f)
continue;
}
else if (BetterDist > 0.05f)
continue;
// Do It.
TabInfosPlanar[TabGroups[CurGroup].IdLast].IdNext = FirstPoint;
TabGroups[CurGroup].IdLast = LastPoint;
CurPoint = FirstPoint;
while (CurPoint >= 0)
{
TabInfosPlanar[CurPoint].IsDone = 3;
TabInfosPlanar[CurPoint].GroupId = CurGroup;
CurPoint = TabInfosPlanar[CurPoint].IdNext;
}
}
// Do planar.
for (S32 g=0 ; g<TabGroups.GetSize() ; g++)
{
S32 CurPoint = TabGroups[g].IdFirst;
S32 FirstPoint = CurPoint;
Vec3f SumNormalN = TabGroups[g].Normal;
Vec3f CenterN = TabGroups[g].Center;
// Set to the plane.
Vec3f vFront,vLeft;
SumNormalN.Get2OrthoVector(vFront,vLeft);
// Compute Min Max.
CurPoint = FirstPoint;
Float xMin = 1e8f;
Float xMax = -1e8f;
Float yMin = 1e8f;
Float yMax = -1e8f;
while (CurPoint >= 0)
{
Vec3f pos = m_TabPoints[CurPoint];
Float fx = pos * vLeft;
Float fy = pos * vFront;
if (fx < xMin)
xMin = fx;
if (fx > xMax)
xMax = fx;
if (fy < yMin)
yMin = fy;
if (fy > yMax)
yMax = fy;
CurPoint = TabInfosPlanar[CurPoint].IdNext;
}
xMin -= 0.805f; // 2 cases + security.
yMin -= 0.805f; // 2 cases + security.
Float Size = 0.4f;
Float InvSize = 1.f / Size;
S32 NbCellX = (S32)((xMax-xMin) * InvSize) + 3; // 3 cases : 2 plus la "courrante"
S32 NbCellY = (S32)((yMax-yMin) * InvSize) + 3; // 3 cases : 2 plus la "courrante"
Float MinXToMiddle = -xMin + Size*0.5f;
Float MinYToMiddle = -yMin + Size*0.5f;
TabRepro.SetSize(NbCellX*NbCellY,TRUE);
TabRepro.Null();
CurPoint = FirstPoint;
while (CurPoint >= 0)
{
if (TabInfosPlanar[CurPoint].IsDone != 3)
{
Vec3f pos = m_TabPoints[CurPoint];
Vec3f delta = CenterN - pos;
Float dx = pos * vLeft;
Float dy = pos * vFront;
Float dh = pos * SumNormalN;
S32 ix = (S32)((dx + MinXToMiddle) * InvSize);
S32 iy = (S32)((dy + MinYToMiddle) * InvSize);
ReprojectInfos &CurInfos = TabRepro[ix + iy*NbCellX];
Vec3f &Normal = m_TabPointsToolNormal[CurPoint].Normal;
CurInfos.Nb++;
CurInfos.Dist += dh;
}
CurPoint = TabInfosPlanar[CurPoint].IdNext;
}
for (S32 j=0 ; j<TabRepro.GetSize() ; j++)
{
ReprojectInfos &CurInfos = TabRepro[j];
if (CurInfos.Nb)
{
Float iRatio = 1.f / (Float)CurInfos.Nb;
CurInfos.Dist *= iRatio;
}
}
// Do planar...
{
CurPoint = FirstPoint;
while (CurPoint >= 0)
{
Vec3f pos = m_TabPoints[CurPoint];
Vec3f delta = CenterN - pos;
Float dx = pos * vLeft;
Float dy = pos * vFront;
dx = (dx - xMin) * InvSize;
dy = (dy - yMin) * InvSize;
S32 ix = (S32)dx;
S32 iy = (S32)dy;
EXCEPTIONC_Z(ix >= 1,"BAD POINT PlanarFilter_AccurateNew");
EXCEPTIONC_Z(iy >= 1,"BAD POINT PlanarFilter_AccurateNew");
EXCEPTIONC_Z(ix < (NbCellX-2),"BAD POINT PlanarFilter_AccurateNew");
EXCEPTIONC_Z(iy < (NbCellY-2),"BAD POINT PlanarFilter_AccurateNew");
ReprojectInfos *pI00 = &TabRepro[ix + iy*NbCellX];
ReprojectInfos *pI01 = pI00+1;
ReprojectInfos *pI10 = pI00+NbCellX;
ReprojectInfos *pI11 = pI10+1;
if ((pI00->Nb + pI01->Nb + pI10->Nb + pI11->Nb) == 0)
{
TabInfosPlanar[CurPoint].IsDone = 0;
CurPoint = TabInfosPlanar[CurPoint].IdNext;
continue;
}
if (!pI00->Nb)
pI00->ComputeDistFromOtthers(NbCellX);
if (!pI01->Nb)
pI01->ComputeDistFromOtthers(NbCellX);
if (!pI10->Nb)
pI10->ComputeDistFromOtthers(NbCellX);
if (!pI11->Nb)
pI11->ComputeDistFromOtthers(NbCellX);
Float ratioX = dx - ix;
Float ratioY = dy - iy;
Float d00 = pI00->Dist;
Float d01 = pI01->Dist;
Float d10 = pI10->Dist;
Float d11 = pI11->Dist;
Float d0 = d00 + (d01-d00) * ratioX;
Float d1 = d10 + (d11-d10) * ratioX;
Float d = d0 + (d1-d0) * ratioY;
Float dh = pos * SumNormalN;
pos += (d-dh) * SumNormalN;
MovePoint(CurPoint, pos);
TabInfosPlanar[CurPoint].IsDone = 2;
// Next Point !
CurPoint = TabInfosPlanar[CurPoint].IdNext;
}
}
}
}