private void KNearestNeighborSearch(Vector2f pos, uint k, ref float distanceSquared, FastPriorityQueue <ItemNode <T> > results)
{
if (m_Leaf)
{
for (int i = 0; i < m_Objects.Count; i++)
{
T obj = m_Objects[i];
float ds = (pos - obj.Position).SquaredLength();
if (ds > distanceSquared)
{
continue;
}
//If results list has empty elements
if (results.Count < k)
{
results.Enqueue(new ItemNode <T>(obj), -ds);
continue;
}
if (-ds > results.First.Priority)
{
results.Dequeue();
results.Enqueue(new ItemNode <T>(obj), -ds);
distanceSquared = -results.First.Priority;
}
}
return;
}
// Search in order of closeness to the given position
int quad = GetQuadrant(pos);
switch (quad)
{
case NORTH_WEST:
if (m_NorthWest != null && m_NorthWest.m_Region.SquaredDistance(pos) < distanceSquared)
{
m_NorthWest.KNearestNeighborSearch(pos, k, ref distanceSquared, results);
}
if (m_NorthEast != null && m_NorthEast.m_Region.SquaredDistance(pos) < distanceSquared)
{
m_NorthEast.KNearestNeighborSearch(pos, k, ref distanceSquared, results);
}
if (m_SouthWest != null && m_SouthWest.m_Region.SquaredDistance(pos) < distanceSquared)
{
m_SouthWest.KNearestNeighborSearch(pos, k, ref distanceSquared, results);
}
if (m_SouthEast != null && m_SouthEast.m_Region.SquaredDistance(pos) < distanceSquared)
{
m_SouthEast.KNearestNeighborSearch(pos, k, ref distanceSquared, results);
}
break;
case NORTH_EAST:
if (m_NorthEast != null && m_NorthEast.m_Region.SquaredDistance(pos) < distanceSquared)
{
m_NorthEast.KNearestNeighborSearch(pos, k, ref distanceSquared, results);
}
if (m_NorthWest != null && m_NorthWest.m_Region.SquaredDistance(pos) < distanceSquared)
{
m_NorthWest.KNearestNeighborSearch(pos, k, ref distanceSquared, results);
}
if (m_SouthEast != null && m_SouthEast.m_Region.SquaredDistance(pos) < distanceSquared)
{
m_SouthEast.KNearestNeighborSearch(pos, k, ref distanceSquared, results);
}
if (m_SouthWest != null && m_SouthWest.m_Region.SquaredDistance(pos) < distanceSquared)
{
m_SouthWest.KNearestNeighborSearch(pos, k, ref distanceSquared, results);
}
break;
case SOUTH_WEST:
if (m_SouthWest != null && m_SouthWest.m_Region.SquaredDistance(pos) < distanceSquared)
{
m_SouthWest.KNearestNeighborSearch(pos, k, ref distanceSquared, results);
}
if (m_SouthEast != null && m_SouthEast.m_Region.SquaredDistance(pos) < distanceSquared)
{
m_SouthEast.KNearestNeighborSearch(pos, k, ref distanceSquared, results);
}
if (m_NorthWest != null && m_NorthWest.m_Region.SquaredDistance(pos) < distanceSquared)
{
m_NorthWest.KNearestNeighborSearch(pos, k, ref distanceSquared, results);
}
if (m_NorthEast != null && m_NorthEast.m_Region.SquaredDistance(pos) < distanceSquared)
{
m_NorthEast.KNearestNeighborSearch(pos, k, ref distanceSquared, results);
}
break;
case SOUTH_EAST:
if (m_SouthEast != null && m_SouthEast.m_Region.SquaredDistance(pos) < distanceSquared)
{
m_SouthEast.KNearestNeighborSearch(pos, k, ref distanceSquared, results);
}
if (m_SouthWest != null && m_SouthWest.m_Region.SquaredDistance(pos) < distanceSquared)
{
m_SouthWest.KNearestNeighborSearch(pos, k, ref distanceSquared, results);
}
if (m_NorthEast != null && m_NorthEast.m_Region.SquaredDistance(pos) < distanceSquared)
{
m_NorthEast.KNearestNeighborSearch(pos, k, ref distanceSquared, results);
}
if (m_NorthWest != null && m_NorthWest.m_Region.SquaredDistance(pos) < distanceSquared)
{
m_NorthWest.KNearestNeighborSearch(pos, k, ref distanceSquared, results);
}
break;
}
}
