/**
* internal help method to find the k nearest neighbors
*/
void spKNNSearch(SPPoint queryFeature, const SPKDTreeNode node, SPBPQueue q){
SPListElement element;
int index, distance;
bool distanceFlag = false;
if(!node){
return;
}
if(node->dim==INVALID){ //** this is a leaf **//
index = spPointGetIndex(node->data);
distance = spPointL2SquaredDistance(queryFeature, node->data);
element = spListElementCreate(index, distance);
spBPQueueEnqueue(q, element);
spListElementDestroy(element);
return;
}
//** go to the left sub tree **//
if(spPointGetAxisCoor(queryFeature, node->dim)<= node->val){
spKNNSearch(queryFeature, node->left, q);
distance = pow((spPointGetAxisCoor(queryFeature,
node->dim) - node->val),2);
distanceFlag = distance < spBPQueueMaxValue(q);
if(!spBPQueueIsFull(q) || distanceFlag){
spKNNSearch(queryFeature, node->right, q);
}
}else{
spKNNSearch(queryFeature, node->right, q);
distance = pow((spPointGetAxisCoor(queryFeature,
node->dim) - node->val),2);
distanceFlag = distance < spBPQueueMaxValue(q);
if(!spBPQueueIsFull(q) || distanceFlag){
spKNNSearch(queryFeature, node->left, q);
}
}
return;
}
SP_BPQUEUE_MSG spBPQueueEnqueue(SPBPQueue source, SPListElement element){
// variables declaration//
int i;
SPList source_list = source->list;
SPListElement elem_to_comp;
int list_size = spListGetSize(source_list);
// check for bad arguments or allocation problems//
if (!source || !element || !source_list){
return SP_BPQUEUE_INVALID_ARGUMENT;
}
// insertion process//
if (spBPQueueIsEmpty(source)){
spListInsertFirst(source_list, element);
return SP_BPQUEUE_SUCCESS;
}
else{
elem_to_comp = spListGetFirst(source_list);
/*starts from the bottom and loop for next source list element up until
you find the one which is bigger than the inserted one and insert the element
before that one. you will stop after you ran out of elements to look for */
for(i=0; i<list_size; i++){
if (spListElementGetValue(element) <= spListElementGetValue(elem_to_comp)){// element is smaller than next elem, insert before current
if (spBPQueueIsFull(source)){
if (spListInsertBeforeCurrent(source_list, element) == SP_LIST_OUT_OF_MEMORY){
return SP_BPQUEUE_OUT_OF_MEMORY;
}
spListGetLast(source_list);
spListRemoveCurrent(source_list);
return SP_BPQUEUE_SUCCESS;
}
else{
if (spListInsertBeforeCurrent(source_list, element) == SP_LIST_OUT_OF_MEMORY){
return SP_BPQUEUE_OUT_OF_MEMORY;
}
return SP_BPQUEUE_SUCCESS;
}
}else{
elem_to_comp = spListGetNext(source_list);
}
}
}
if (!spBPQueueIsFull(source)){//queue isn't full but this item is the biggest
if (spListInsertLast(source_list, element)== SP_LIST_OUT_OF_MEMORY){
return SP_BPQUEUE_OUT_OF_MEMORY;
}
return SP_BPQUEUE_SUCCESS;
}
return SP_BPQUEUE_SUCCESS; // the element is larger than all others
}
SP_BPQUEUE_MSG spBPQueueEnqueue(SPBPQueue source, SPListElement element) {
if (source == NULL || element == NULL) {
return SP_BPQUEUE_INVALID_ARGUMENT;
}
bool wasFull = spBPQueueIsFull(source);
SPList list = source->innerList;
bool wasInserted = false;
// Advance iterator to the proper element's place
SP_LIST_FOREACH(SPListElement, currentElement, list) {
if (spListElementCompare(element, currentElement) < 0) {
// Modifying while iterating - bad practice but is more efficient here.
SP_LIST_MSG returnMSG = spListInsertBeforeCurrent(list, element);
switch (returnMSG) {
case SP_LIST_OUT_OF_MEMORY:
return SP_BPQUEUE_OUT_OF_MEMORY;
default:
break;
}
wasInserted = true;
break;
}
}
if (wasInserted) {
// Remove the last element if needed
if (wasFull) {
spListGetLast(list);
spListRemoveCurrent(list);
}
return SP_BPQUEUE_SUCCESS;
} else {
if (spBPQueueIsFull(source)) {
return SP_BPQUEUE_FULL;
} else {
// The item is largest than everything, insert last
SP_LIST_MSG returnMSG = spListInsertLast(list, element);
switch (returnMSG) {
case SP_LIST_OUT_OF_MEMORY:
return SP_BPQUEUE_OUT_OF_MEMORY;
default:
return SP_BPQUEUE_SUCCESS;
}
}
}
}
void SPKDTreeKNNRecursive(SPKDTreeNode treeNode, SPPoint p, SPBPQueue bpq, SP_KDTREE_MSG* msg)
{
SPListElement listElement;
SPPoint treePoint;
bool searchedLeft;
double dist;
if(bpq == NULL || treeNode == NULL)
{
*msg = SP_KDTREE_INVALID_ARGUMENT;
return;
}
// If treeNode is a leaf
if(treeNode->left == NULL && treeNode->right == NULL)
{
treePoint = *(treeNode->data);
listElement = spListElementCreate(spPointGetIndex(treePoint), spPointL2SquaredDistance(p, treePoint));
spBPQueueEnqueue(bpq, listElement);
spListElementDestroy(listElement);
*msg = SP_KDTREE_SUCCESS;
return;
}
// Turn to search the tree that would've contain the point p (if it was in the tree)
if(spPointGetAxisCoor(p, treeNode->dim) <= treeNode->val)
{
searchedLeft = true;
SPKDTreeKNNRecursive(treeNode->left, p, bpq, msg);
if (*msg != SP_KDTREE_SUCCESS)
return;
}
else
{
searchedLeft = false;
SPKDTreeKNNRecursive(treeNode->right, p, bpq, msg);
if (*msg != SP_KDTREE_SUCCESS)
return;
}
// dist = |treeNode.val - p[treeNode.dim]|
dist = treeNode->val - spPointGetAxisCoor(p, treeNode->dim);
if(dist < 0)
dist *= -1;
//dist *= dist;
if(!spBPQueueIsFull(bpq) || dist < spBPQueueMaxValue(bpq))
{
if(searchedLeft)
SPKDTreeKNNRecursive(treeNode->right, p, bpq, msg);
else
SPKDTreeKNNRecursive(treeNode->left, p, bpq, msg);
}
}
static bool bpqFullTest() {
SPBPQueue source;
SPListElement e1;
source = NULL;
ASSERT_TRUE(spBPQueueIsFull(source) == false); // check edge case
source = spBPQueueCreate(maxSize);
// insert maxSize element and check that full at the end
while (spBPQueueSize(source) < maxSize) {
ASSERT_TRUE(spBPQueueIsFull(source) == false); // check that not full in the process
e1 = spListElementCreate(1, 1.0);
spBPQueueEnqueue(source, e1);
spListElementDestroy(e1);
}
ASSERT_TRUE(spBPQueueIsFull(source) == true);
// free memory
spBPQueueDestroy(source);
return true;
}
/**
* Given a kd-tree and a point p, the function stores the nearest neighbors of p to bpq
*
* @param curr - the kd-tree containing the points
* @param bpq - the bounded priority queue to store the nearest neighbors in
* @param p - the point to find the nearest neighbors to
*
* does nothing if curr == NULL or bpq == NULL or p == NULL
*/
void nearestNeighbors(KDTreeNode curr, SPBPQueue bpq, SPPoint p) {
SPListElement node;
SPPoint q;
bool isLeft;
double coorDis;
if (curr == NULL || bpq == NULL || p == NULL )
return;
q = curr->data;
/* Add the current point to the BPQ. Note that this is a no-op if the
* point is not as good as the points we've seen so far.*/
if (curr->dim == -1) {
int index;
double dis;
index = spPointGetIndex(q);
dis = spPointL2SquaredDistance(p, curr->data);
node = spListElementCreate(index, dis);
spBPQueueEnqueue(bpq, node);
spListElementDestroy(node);
return;
}
/* Recursively search the half of the tree that contains the test point. */
if (spPointGetAxisCoor(p, curr->dim) <= curr->val) {
nearestNeighbors(curr->left, bpq, p);
isLeft = true;
} else {
nearestNeighbors(curr->right, bpq, p);
isLeft = false;
}
/* If the candidate hypersphere crosses this splitting plane, look on the
* other side of the plane by examining the other subtree*/
coorDis = abs(spPointGetAxisCoor(p, curr->dim) - curr->val);
if (!spBPQueueIsFull(bpq) || coorDis*coorDis < spBPQueueMaxValue(bpq)) {
if (isLeft)
nearestNeighbors(curr->right, bpq, p);
else
nearestNeighbors(curr->left, bpq, p);
}
}
SP_BPQUEUE_MSG spBPQueueEnqueue(SPBPQueue source, SPListElement element) {
spMinimalVerifyArguments(source != NULL && source->queue != NULL && element != NULL,
SP_BPQUEUE_INVALID_ARGUMENT);
if (spBPQueueGetMaxSize(source) == 0)
return SP_BPQUEUE_FULL;
// the list is full and the element is greater than all the current items
if (spBPQueueIsFull(source) && spListElementCompare(element, source->maxElement) >= 0) {
return SP_BPQUEUE_FULL;
}
if (spBPQueueIsEmpty(source))
return spBPQueueInsertIfEmpty(source,element);
//insert to a non empty queue
return spBPQueueInsertNotEmpty(source, element);
}
