/** @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;
}
}
int build_recursively(List <List <int> > sortidx, List <int> pidx, int dim)
{
GameKdTreeNode node = null;
// Stop condition
if (pidx.Count == 1)
{
node = new GameKdTreeNode(); // create a new node
int gamenodeidx = nodesPtrs.Count; // its address is
nodesPtrs.Add(node); // important to push back here
node.LIdx = -1; // no child
node.RIdx = -1; // no child
node.pIdx = pidx[0]; // the only index available
node.key = 0; // key is useless here
return(gamenodeidx);
}
// allocate the vectors
List <int> Larray = ListPool.TrySpawn <List <int> >();
List <int> Rarray = ListPool.TrySpawn <List <int> >();
// initialize the "partition" array
// Setting parray to -1 indicates we are not using the point
for (int i = 0; i < npoints; i++)
{
workarray[i] = -1;
}
for (int i = 0; i < pidx.Count; i++)
{
workarray[sortidx[dim][pidx[i]]] = pidx[i];
}
int pivot = -1; //index of the median element
if (pidx.Count * Math.Log(pidx.Count) < npoints)
{
Larray.Resize(pidx.Count / 2 + (pidx.Count % 2 == 0 ? 0 : 1), -1);
Rarray.Resize(pidx.Count / 2, -1);
heapsort(dim, pidx, pidx.Count);
Copy(pidx, Larray, 0, Larray.Count, 0);
Copy(pidx, Rarray, Larray.Count, pidx.Count, 0);
pivot = pidx[(pidx.Count - 1) / 2];
}
else
{
// The middle valid value of parray is the pivot,
// the left go to a node on the left, the right
// and the pivot go to a node on the right.
int TH = (pidx.Count - 1) / 2; //defines median offset
int cnt = 0; //number of points found
for (int i = 0; i < npoints; i++)
{
// Is the current point not in the current selection? skip
if (workarray[i] == -1)
{
continue;
}
// len/2 is on the "right" of pivot.
// Pivot is still put on the left side
if (cnt == TH)
{
pivot = workarray[i];
Larray.Add(workarray[i]);
}
else if (cnt > TH)
{
Rarray.Add(workarray[i]);
}
else
{
Larray.Add(workarray[i]);
}
// Don't overwork, if we already read all the necessary just stop.
cnt++;
if (cnt > pidx.Count)
{
break;
}
}
}
// CREATE THE NODE
node = new GameKdTreeNode();
int nodeIdx = nodesPtrs.Count; //why it's not +1? should not happend after push back? -> no since size() is the index of last element+1!!
nodesPtrs.Add(node); //important to push back here
node.pIdx = -1; //not a leaf
node.key = points[pivot][dim];
node.LIdx = build_recursively(sortidx, Larray, (dim + 1) % ndim);
ListPool.TryDespawn(Larray);
node.RIdx = build_recursively(sortidx, Rarray, (dim + 1) % ndim);
ListPool.TryDespawn(Rarray);
return(nodeIdx);
}
