public int NearestIndex(float x, float y, float z)
{
#if SEARCH_DEBUG
int dbgcount = 0;
#endif
int limit = 99;
int near = -1;
float distsq = float.MaxValue;
int a = IndexX(x);
int b = IndexY(y);
int c = IndexZ(z);
for (int i = 0; i <= limit; ++i)
{
for (int xx = a - i; xx <= a + i; ++xx)
{
for (int yy = b - i; yy <= b + i; ++yy)
{
for (int zz = c - i; zz <= c + i; ++zz)
{
if (xx < 0 || xx >= clusters.xx)
{
continue;
}
if (yy < 0 || yy >= clusters.yy)
{
continue;
}
if (zz < 0 || zz >= clusters.zz)
{
continue;
}
List <int> l = clusters.Get(xx, yy, zz);
if (l == null)
{
continue;
}
foreach (int j in l)
{
Point3 p = points[j];
p.x -= x;
p.y -= y;
p.z -= z;
float d = p.x * p.x + p.y * p.y + p.z * p.z;
#if SEARCH_DEBUG
++dbgcount;
#endif
if (d >= distsq)
{
continue;
}
if (limit == 99)
{
limit = i + 1;
}
distsq = d;
near = j;
}
}
}
}
}
#if SEARCH_DEBUG
System.Diagnostics.Debug.WriteLine(string.Format(
"dbgcount:{0} index:{1} distance:{2}", dbgcount, near, distsq));
#endif
return(near);
}