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); }