void KDTree::findKNearestNeighbours(const NodePtr& src, LimitedPriorityQueue& results,
const VertexPtr& target){
if(src->isLeaf()){
for(VertexPtr vrtx: src->getBucket()){
float dist = norm((*vrtx)-(*target));
results.push(VertexDistPair(vrtx, dist));
}
HyperSphere sphere(target, std::get<1>(results.top()), m_K);
//Required Number of points are found, sphere was completely in region,
//there can't be any closer results
if(results.full() && sphere.withinRegion(src->getDomain())){
return;
}
}
//Uncool, get sphere again
float dist = FLT_MAX;
if(!results.empty()){
dist = std::get<1>(results.top());
}
HyperSphere sphere(target, dist, m_K );
if(src->getLeft()){
if(sphere.intersectsRegion(src->getLeft()->getDomain())){
findKNearestNeighbours(src->getLeft(), results, target);
}
}
if(src->getRight()){
if(sphere.intersectsRegion(src->getRight()->getDomain())){
findKNearestNeighbours(src->getRight(), results, target);
}
};
};
void KDTree::findInRadius(const NodePtr& src, const HyperSphere& sphere,
VertexList& result) const{
if(src->isLeaf()){
for(VertexPtr vrtx : src->getBucket()){
if(sphere.contains(vrtx)){
result.push_back(vrtx);
}
}
}
if(src->getLeft() != nullptr){
if(sphere.intersectsRegion(src->getDomain())){
findInRadius(src->getLeft(), sphere, result);
}
}
if(src->getRight() != nullptr){
if(sphere.intersectsRegion(src->getDomain())){
findInRadius(src->getRight(), sphere, result);
}
}
}
