/** @see ball_query, range_query
*
* Returns all the points withing the ball bounding box and their distances
*
* @note this is similar to "range_query" i just replaced "lies_in_range" with "euclidean_distance"
*/
void ball_bbox_query(int nodeIdx, KInt2 pmin, KInt2 pmax, List <int> inrange_idxs, List <float> distances, KInt2 point, float radiusSquared, int dim = 0)
{
GameKdTreeNode node = nodesPtrs[nodeIdx];
// if it's a leaf and it lies in R
if (node.isLeaf())
{
float distance = (points[node.pIdx] - point).sqrMagnitude;
if (distance <= radiusSquared)
{
inrange_idxs.Add(node.pIdx);
if (distances != null)
{
distances.Add(distance);
}
return;
}
}
else
{
if (node.key >= pmin[dim] && node.LIdx != -1)
{
ball_bbox_query(node.LIdx, pmin, pmax, inrange_idxs, distances, point, radiusSquared, (dim + 1) % ndim);
}
if (node.key <= pmax[dim] && node.RIdx != -1)
{
ball_bbox_query(node.RIdx, pmin, pmax, inrange_idxs, distances, point, radiusSquared, (dim + 1) % ndim);
}
}
}
void ball_bbox_query(int nodeIdx, KInt2 pmin, KInt2 pmax, List <KInt2> inrange_idxs, List <float> distances, KInt2 point, float radiusSquared, int dim = 0)
{
GameKdTreeNode node = nodesPtrs[nodeIdx];
// if it's a leaf and it lies in R
if (node.isLeaf())
{
float distance = (points[node.pIdx] - point).sqrMagnitude;
if (distance <= radiusSquared)
{
bool insert = false;
for (int i = 0; i < distances.Count; ++i)
{
if (distance < distances[i])
{
insert = true;
distances.Insert(i, distance);
inrange_idxs.Insert(i, this.points[node.pIdx]);
break;
}
}
if (!insert)
{
distances.Add(distance);
inrange_idxs.Add(this.points[node.pIdx]);
}
return;
}
}
else
{
if (node.key >= pmin[dim] && node.LIdx != -1)
{
ball_bbox_query(node.LIdx, pmin, pmax, inrange_idxs, distances, point, radiusSquared, (dim + 1) % ndim);
}
if (node.key <= pmax[dim] && node.RIdx != -1)
{
ball_bbox_query(node.RIdx, pmin, pmax, inrange_idxs, distances, point, radiusSquared, (dim + 1) % ndim);
}
}
}
void knn_search(KInt2 Xq, int nodeIdx = 0, int dim = 0)
{
// cout << "at node: " << nodeIdx << endl;
GameKdTreeNode node = nodesPtrs[nodeIdx];
float temp;
// We are in LEAF
if (node.isLeaf())
{
float distance = (Xq - points[node.pIdx]).sqrMagnitude;
// pqsize is at maximum size k, if overflow and current record is closer
// pop further and insert the new one
if (pq.size() == k && pq.top().Key > distance)
{
pq.pop(); // remove farther record
pq.push(distance, node.pIdx); //push new one
}
else if (pq.size() < k)
{
pq.push(distance, node.pIdx);
}
return;
}
////// Explore the sons //////
// recurse on closer son
if (Xq[dim] <= node.key)
{
temp = Bmax[dim]; Bmax[dim] = node.key;
knn_search(Xq, node.LIdx, (dim + 1) % ndim);
Bmax[dim] = temp;
}
else
{
temp = Bmin[dim]; Bmin[dim] = node.key;
knn_search(Xq, node.RIdx, (dim + 1) % ndim);
Bmin[dim] = temp;
}
// recurse on farther son
if (Xq[dim] <= node.key)
{
temp = Bmin[dim]; Bmin[dim] = node.key;
if (bounds_overlap_ball(Xq))
{
knn_search(Xq, node.RIdx, (dim + 1) % ndim);
}
Bmin[dim] = temp;
}
else
{
temp = Bmax[dim]; Bmax[dim] = node.key;
if (bounds_overlap_ball(Xq))
{
knn_search(Xq, node.LIdx, (dim + 1) % ndim);
}
Bmax[dim] = temp;
}
}