bool pointBasicL2Distance() {
double data1[2] = { 1.0, 1.0 };
double data2[2] = { 1.0, 0.0 };
int dim1 = 2;
int dim2 = 2;
int index1 = 1;
int index2 = 1;
SPPoint p = spPointCreate((double *)data1, dim1, index1);
SPPoint q = spPointCreate((double *)data2, dim2, index2);
ASSERT_TRUE(spPointL2SquaredDistance(p,p) == 0.0);
ASSERT_TRUE(spPointL2SquaredDistance(q,q) == 0.0);
ASSERT_FALSE(spPointL2SquaredDistance(p,q) == 0.0);
spPointDestroy(p);
spPointDestroy(q);
return true;
}
/**
* 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;
}
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);
}
}
distanceWithPoint* createAndSortDistancesArray(int size, SPPoint queryPoint, SPPoint* pointsArray){
distanceWithPoint* distancesArray = NULL;
int i;
distancesArray = (distanceWithPoint*)calloc(sizeof(distanceWithPoint),size);
if (distancesArray == NULL){
spLoggerSafePrintError(ERROR_ALLOCATING_MEMORY,__FILE__,__FUNCTION__,__LINE__);
return NULL;
}
for (i=0;i<size;i++){
distancesArray[i].point = pointsArray[i];
distancesArray[i].distance = spPointL2SquaredDistance(queryPoint, distancesArray[i].point);
}
qsort(distancesArray, size, sizeof(distanceWithPoint), distanceWithPointComparator);
return distancesArray;
}
/**
* 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);
}
}
