#include <iostream>
|
#include <vector>
|
#include <stack>
|
#include <cmath>
|
#include <algorithm>
|
#include <math.h>
|
#include "kdTree.h"
|
|
using namespace std;
|
|
|
bool cmp1(const coordinate& a, const coordinate& b) {
|
return a.x > b.x;
|
}
|
|
bool cmp2(const coordinate& a, const coordinate& b) {
|
return a.y > b.y;
|
}
|
|
bool cmp3(const coordinate& a, const coordinate& b) {
|
return a.z > b.z;
|
}
|
|
bool equal(const coordinate& a, const coordinate& b) {
|
return (a.x == b.x && a.y == b.y && a.z == b.z);
|
}
|
|
void ChooseSplit(coordinate exm_set[], int size, UL &split, coordinate &SplitChoice) {
|
/*compute the variance on every dimension. Set split as the dismension that have the biggest
|
variance. Then choose the instance which is the median on this split dimension.*/
|
/*compute variance on the x,y dimension. DX=EX^2-(EX)^2*/
|
double tmp1, tmp2;
|
tmp1 = tmp2 = 0;
|
for (int i = 0; i < size; ++i)
|
{
|
tmp1 += 1.0 / (double)size * exm_set[i].x * exm_set[i].x;
|
tmp2 += 1.0 / (double)size * exm_set[i].x;
|
}
|
double v1 = tmp1 - tmp2 * tmp2; //compute variance on the x dimension ¼ÆËãx·½Ïò·½²î
|
|
tmp1 = tmp2 = 0;
|
for (int i = 0; i < size; ++i)
|
{
|
tmp1 += 1.0 / (double)size * exm_set[i].y * exm_set[i].y;
|
tmp2 += 1.0 / (double)size * exm_set[i].y;
|
}
|
double v2 = tmp1 - tmp2 * tmp2; //compute variance on the y dimension ¼ÆËãy·½Ïò·½²î
|
|
tmp1 = tmp2 = 0;
|
for (int i = 0; i < size; ++i)
|
{
|
tmp1 += 1.0 / (double)size * exm_set[i].z * exm_set[i].z;
|
tmp2 += 1.0 / (double)size * exm_set[i].z;
|
}
|
double v3 = tmp1 - tmp2 * tmp2; //compute variance on the z dimension ¼ÆËãz·½Ïò·½²î
|
|
|
//split = v1 > v2 ? 0 : 1; //set the split dimension
|
|
|
if (v1 >= v2 && v1 >= v3)
|
{
|
split = 0;
|
std::nth_element(exm_set, exm_set + size / 2, exm_set + size, cmp1);
|
}
|
else if (v2 >= v1 && v2 >= v3)
|
{
|
split = 1;
|
std::nth_element(exm_set, exm_set + size / 2, exm_set + size, cmp2);
|
}
|
else
|
{
|
split = 2;
|
std::nth_element(exm_set, exm_set + size / 2, exm_set + size, cmp3);
|
}
|
|
//set the split point value
|
SplitChoice.x = exm_set[size / 2 ].x;
|
SplitChoice.y = exm_set[size / 2 ].y;
|
SplitChoice.z = exm_set[size / 2 ].z;
|
|
}
|
|
TreeNode* build_kdtree(coordinate exm_set[], int size, TreeNode* T) {
|
//call function ChooseSplit to choose the split dimension and split point
|
if (size == 0) {
|
return nullptr;
|
}
|
else {
|
UL split;
|
coordinate dom_elt;
|
ChooseSplit(exm_set, size, split, dom_elt);
|
coordinate *exm_set_right = new coordinate[size * 9 / 16 + 10];
|
coordinate *exm_set_left = new coordinate[size * 9 / 16 + 10];
|
int size_left, size_right;
|
size_left = size_right = 0;
|
|
switch (split)
|
{
|
case 0:
|
for (UL i = 0; i < size; ++i)
|
{
|
|
if (!equal(exm_set[i], dom_elt) && exm_set[i].x <= dom_elt.x)
|
{
|
|
exm_set_left[size_left].x = exm_set[i].x;
|
exm_set_left[size_left].y = exm_set[i].y;
|
exm_set_left[size_left].z = exm_set[i].z;
|
size_left++;
|
}
|
else if (!equal(exm_set[i], dom_elt) && exm_set[i].x > dom_elt.x)
|
{
|
|
exm_set_right[size_right].x = exm_set[i].x;
|
exm_set_right[size_right].y = exm_set[i].y;
|
exm_set_right[size_right].z = exm_set[i].z;
|
size_right++;
|
}
|
}
|
break;
|
case 1:
|
for (UL i = 0; i < size; ++i)
|
{
|
if (!equal(exm_set[i], dom_elt) && exm_set[i].y <= dom_elt.y)
|
{
|
|
exm_set_left[size_left].x = exm_set[i].x;
|
exm_set_left[size_left].y = exm_set[i].y;
|
exm_set_left[size_left].z = exm_set[i].z;
|
size_left++;
|
}
|
else if (!equal(exm_set[i], dom_elt) && exm_set[i].y > dom_elt.y)
|
{
|
|
exm_set_right[size_right].x = exm_set[i].x;
|
exm_set_right[size_right].y = exm_set[i].y;
|
exm_set_right[size_right].z = exm_set[i].z;
|
size_right++;
|
}
|
}
|
break;
|
default:
|
for (UL i = 0; i < size; ++i)
|
{
|
if (!equal(exm_set[i], dom_elt) && exm_set[i].z <= dom_elt.z)
|
{
|
|
exm_set_left[size_left].x = exm_set[i].x;
|
exm_set_left[size_left].y = exm_set[i].y;
|
exm_set_left[size_left].z = exm_set[i].z;
|
size_left++;
|
}
|
else if (!equal(exm_set[i], dom_elt) && exm_set[i].z > dom_elt.z)
|
{
|
|
exm_set_right[size_right].x = exm_set[i].x;
|
exm_set_right[size_right].y = exm_set[i].y;
|
exm_set_right[size_right].z = exm_set[i].z;
|
size_right++;
|
}
|
}
|
break;
|
}
|
|
|
T = new TreeNode;
|
T->dom_elt.x = dom_elt.x;
|
T->dom_elt.y = dom_elt.y;
|
T->dom_elt.z = dom_elt.z;
|
T->split = split;
|
T->left = build_kdtree(exm_set_left, size_left, T->left);
|
T->right = build_kdtree(exm_set_right, size_right, T->right);
|
|
delete[] exm_set_left;
|
delete[] exm_set_right;
|
|
return T;
|
|
}
|
}
|
|
double Distance(coordinate a, coordinate b) {
|
double tmp = (a.x - b.x) * (a.x - b.x)
|
+ (a.y - b.y) * (a.y - b.y)
|
+ (a.z - b.z) * (a.z - b.z);
|
return sqrt(tmp);
|
}
|
|
void searchNearest(TreeNode * Kd, coordinate target, coordinate &nearestpoint, double & distance) {
|
|
//1. Èç¹ûKdÊǿյģ¬ÔòÉèdistΪÎÞÇî´ó·µ»Ø
|
|
//2. ÏòÏÂËÑË÷Ö±µ½Ò¶×Ó½áµã
|
|
stack<TreeNode*> search_path;
|
TreeNode* pSearch = Kd;
|
coordinate nearest;
|
double dist;
|
|
while (pSearch != nullptr)
|
{
|
//pSearch¼ÓÈëµ½search_pathÖÐ;
|
search_path.push(pSearch);
|
|
switch (pSearch->split)
|
{
|
case 0:
|
if (target.x <= pSearch->dom_elt.x) /* Èç¹ûСÓھͽøÈë×ó×ÓÊ÷ */
|
{
|
pSearch = pSearch->left;
|
}
|
else
|
{
|
pSearch = pSearch->right;
|
}
|
break;
|
case 1:
|
if (target.y <= pSearch->dom_elt.y) /* Èç¹ûСÓھͽøÈë×ó×ÓÊ÷ */
|
{
|
pSearch = pSearch->left;
|
}
|
else
|
{
|
pSearch = pSearch->right;
|
}
|
break;
|
default:
|
if (target.z <= pSearch->dom_elt.z) /* Èç¹ûСÓھͽøÈë×ó×ÓÊ÷ */
|
{
|
pSearch = pSearch->left;
|
}
|
else
|
{
|
pSearch = pSearch->right;
|
}
|
break;
|
}
|
|
|
}
|
//È¡³ösearch_path×îºóÒ»¸ö¸³¸ønearest
|
nearest.x = search_path.top()->dom_elt.x;
|
nearest.y = search_path.top()->dom_elt.y;
|
nearest.z = search_path.top()->dom_elt.z;
|
search_path.pop();
|
|
|
dist = Distance(nearest, target);
|
//3. »ØËÝËÑË÷·¾¶
|
|
TreeNode* pBack;
|
|
while (!search_path.empty())
|
{
|
//È¡³ösearch_path×îºóÒ»¸ö½áµã¸³¸øpBack
|
pBack = search_path.top();
|
search_path.pop();
|
|
if (pBack->left == nullptr && pBack->right == nullptr) /* Èç¹ûpBackΪҶ×Ó½áµã */
|
{
|
|
if (Distance(nearest, target) > Distance(pBack->dom_elt, target))
|
{
|
nearest = pBack->dom_elt;
|
dist = Distance(pBack->dom_elt, target);
|
}
|
|
}
|
|
else
|
|
{
|
|
UL s = pBack->split;
|
|
switch (s)
|
{
|
case 0:
|
if (fabs(pBack->dom_elt.x - target.x) < dist) /* Èç¹ûÒÔtargetΪÖÐÐĵÄÔ²£¨Çò»ò³¬Çò£©£¬°ë¾¶ÎªdistµÄÔ²Óë·Ö¸î³¬Æ½ÃæÏཻ£¬ ÄÇô¾ÍÒªÌøµ½ÁíÒ»±ßµÄ×Ó¿Õ¼äÈ¥ËÑË÷ */
|
{
|
if (Distance(nearest, target) > Distance(pBack->dom_elt, target))
|
{
|
nearest = pBack->dom_elt;
|
dist = Distance(pBack->dom_elt, target);
|
}
|
if (target.x <= pBack->dom_elt.x) /* Èç¹ûtargetλÓÚpBackµÄ×ó×ӿռ䣬ÄÇô¾ÍÒªÌøµ½ÓÒ×Ó¿Õ¼äÈ¥ËÑË÷ */
|
pSearch = pBack->right;
|
else
|
pSearch = pBack->left; /* Èç¹ûtargetλÓÚpBackµÄÓÒ×ӿռ䣬ÄÇô¾ÍÒªÌøµ½×ó×Ó¿Õ¼äÈ¥ËÑË÷ */
|
if (pSearch != nullptr)
|
//pSearch¼ÓÈëµ½search_pathÖÐ
|
search_path.push(pSearch);
|
}
|
break;
|
case 1:
|
if (fabs(pBack->dom_elt.y - target.y) < dist) /* Èç¹ûÒÔtargetΪÖÐÐĵÄÔ²£¨Çò»ò³¬Çò£©£¬°ë¾¶ÎªdistµÄÔ²Óë·Ö¸î³¬Æ½ÃæÏཻ£¬ ÄÇô¾ÍÒªÌøµ½ÁíÒ»±ßµÄ×Ó¿Õ¼äÈ¥ËÑË÷ */
|
{
|
if (Distance(nearest, target) > Distance(pBack->dom_elt, target))
|
{
|
nearest = pBack->dom_elt;
|
dist = Distance(pBack->dom_elt, target);
|
}
|
if (target.y <= pBack->dom_elt.y) /* Èç¹ûtargetλÓÚpBackµÄ×ó×ӿռ䣬ÄÇô¾ÍÒªÌøµ½ÓÒ×Ó¿Õ¼äÈ¥ËÑË÷ */
|
pSearch = pBack->right;
|
else
|
pSearch = pBack->left; /* Èç¹ûtargetλÓÚpBackµÄÓÒ×ӿռ䣬ÄÇô¾ÍÒªÌøµ½×ó×Ó¿Õ¼äÈ¥ËÑË÷ */
|
if (pSearch != nullptr)
|
// pSearch¼ÓÈëµ½search_pathÖÐ
|
search_path.push(pSearch);
|
}
|
break;
|
default:
|
if (fabs(pBack->dom_elt.z - target.z) < dist) /* Èç¹ûÒÔtargetΪÖÐÐĵÄÔ²£¨Çò»ò³¬Çò£©£¬°ë¾¶ÎªdistµÄÔ²Óë·Ö¸î³¬Æ½ÃæÏཻ£¬ ÄÇô¾ÍÒªÌøµ½ÁíÒ»±ßµÄ×Ó¿Õ¼äÈ¥ËÑË÷ */
|
{
|
if (Distance(nearest, target) > Distance(pBack->dom_elt, target))
|
{
|
nearest = pBack->dom_elt;
|
dist = Distance(pBack->dom_elt, target);
|
}
|
if (target.z <= pBack->dom_elt.z) /* Èç¹ûtargetλÓÚpBackµÄ×ó×ӿռ䣬ÄÇô¾ÍÒªÌøµ½ÓÒ×Ó¿Õ¼äÈ¥ËÑË÷ */
|
pSearch = pBack->right;
|
else
|
pSearch = pBack->left; /* Èç¹ûtargetλÓÚpBackµÄÓÒ×ӿռ䣬ÄÇô¾ÍÒªÌøµ½×ó×Ó¿Õ¼äÈ¥ËÑË÷ */
|
if (pSearch != nullptr)
|
// pSearch¼ÓÈëµ½search_pathÖÐ
|
search_path.push(pSearch);
|
}
|
break;
|
}
|
}
|
}
|
|
nearestpoint.x = nearest.x;
|
nearestpoint.y = nearest.y;
|
nearestpoint.z = nearest.z;
|
distance = dist;
|
|
}
|
|
void searchNearest(const TreeElement* treeArray,int size, coordinate target, coordinate &nearestpoint, double & distance)
|
{
|
stack<int> search_path;
|
|
int index = 0;
|
TreeElement pSearch = treeArray[index];
|
|
coordinate nearest;
|
double dist;
|
|
while (!pSearch.isEmpty)
|
{
|
search_path.push(index);
|
|
if ((2 * index + 1) >= size)
|
{
|
break;
|
}
|
|
switch (pSearch.split)
|
{
|
case 0:
|
if (target.x <= pSearch.dom_elt.x) /* Èç¹ûСÓھͽøÈë×ó×ÓÊ÷ */
|
{
|
index = 2 * index + 1;
|
pSearch = treeArray[index];
|
}
|
else
|
{
|
index = 2 * index + 2;
|
pSearch = treeArray[index];
|
}
|
break;
|
case 1:
|
if (target.y <= pSearch.dom_elt.y) /* Èç¹ûСÓھͽøÈë×ó×ÓÊ÷ */
|
{
|
index = 2 * index + 1;
|
pSearch = treeArray[index];
|
}
|
else
|
{
|
index = 2 * index + 2;
|
pSearch = treeArray[index];
|
}
|
break;
|
default:
|
if (target.z <= pSearch.dom_elt.z) /* Èç¹ûСÓھͽøÈë×ó×ÓÊ÷ */
|
{
|
index = 2 * index + 1;
|
pSearch = treeArray[index];
|
}
|
else
|
{
|
index = 2 * index + 2;
|
pSearch = treeArray[index];
|
}
|
break;
|
}
|
}
|
//È¡³ösearch_path×îºóÒ»¸ö¸³¸ønearest
|
index = search_path.top();
|
nearest.x = treeArray[index].dom_elt.x;
|
nearest.y = treeArray[index].dom_elt.y;
|
nearest.z = treeArray[index].dom_elt.z;
|
search_path.pop();
|
dist = Distance(nearest, target);
|
|
//3. »ØËÝËÑË÷·¾¶
|
TreeElement pBack;
|
|
while (!search_path.empty())
|
{
|
//È¡³ösearch_path×îºóÒ»¸ö½áµã¸³¸øpBack
|
index = search_path.top();
|
pBack = treeArray[index];
|
search_path.pop();
|
|
if ((2 * index + 1) >= size) /* Èç¹ûpBackΪҶ×Ó½áµã */
|
{
|
if (Distance(nearest, target) > Distance(pBack.dom_elt, target))
|
{
|
nearest = pBack.dom_elt;
|
dist = Distance(pBack.dom_elt, target);
|
}
|
}
|
else if (treeArray[2 * index + 1].isEmpty == true && treeArray[2 * index + 2].isEmpty == true) /* Èç¹ûpBackΪҶ×Ó½áµã */
|
{
|
if (Distance(nearest, target) > Distance(pBack.dom_elt, target))
|
{
|
nearest = pBack.dom_elt;
|
dist = Distance(pBack.dom_elt, target);
|
}
|
}
|
else
|
{
|
UL s = pBack.split;
|
switch (s)
|
{
|
case 0:
|
if (fabs(pBack.dom_elt.x - target.x) < dist) /* Èç¹ûÒÔtargetΪÖÐÐĵÄÔ²£¨Çò»ò³¬Çò£©£¬°ë¾¶ÎªdistµÄÔ²Óë·Ö¸î³¬Æ½ÃæÏཻ£¬ ÄÇô¾ÍÒªÌøµ½ÁíÒ»±ßµÄ×Ó¿Õ¼äÈ¥ËÑË÷ */
|
{
|
if (Distance(nearest, target) > Distance(pBack.dom_elt, target))
|
{
|
nearest = pBack.dom_elt;
|
dist = Distance(pBack.dom_elt, target);
|
}
|
|
if (target.x <= pBack.dom_elt.x) /* Èç¹ûtargetλÓÚpBackµÄ×ó×ӿռ䣬ÄÇô¾ÍÒªÌøµ½ÓÒ×Ó¿Õ¼äÈ¥ËÑË÷ */
|
{
|
index = 2 * index + 2;
|
pSearch = treeArray[index];
|
}
|
else /* Èç¹ûtargetλÓÚpBackµÄÓÒ×ӿռ䣬ÄÇô¾ÍÒªÌøµ½×ó×Ó¿Õ¼äÈ¥ËÑË÷ */
|
{
|
index = 2 * index + 1;
|
pSearch = treeArray[index];
|
}
|
if (!pSearch.isEmpty)
|
//pSearch¼ÓÈëµ½search_pathÖÐ
|
search_path.push(index);
|
}
|
break;
|
case 1:
|
if (fabs(pBack.dom_elt.y - target.y) < dist) /* Èç¹ûÒÔtargetΪÖÐÐĵÄÔ²£¨Çò»ò³¬Çò£©£¬°ë¾¶ÎªdistµÄÔ²Óë·Ö¸î³¬Æ½ÃæÏཻ£¬ ÄÇô¾ÍÒªÌøµ½ÁíÒ»±ßµÄ×Ó¿Õ¼äÈ¥ËÑË÷ */
|
{
|
if (Distance(nearest, target) > Distance(pBack.dom_elt, target))
|
{
|
nearest = pBack.dom_elt;
|
dist = Distance(pBack.dom_elt, target);
|
}
|
if (target.y <= pBack.dom_elt.y) /* Èç¹ûtargetλÓÚpBackµÄ×ó×ӿռ䣬ÄÇô¾ÍÒªÌøµ½ÓÒ×Ó¿Õ¼äÈ¥ËÑË÷ */
|
{
|
index = 2 * index + 2;
|
pSearch = treeArray[index];
|
}
|
else /* Èç¹ûtargetλÓÚpBackµÄÓÒ×ӿռ䣬ÄÇô¾ÍÒªÌøµ½×ó×Ó¿Õ¼äÈ¥ËÑË÷ */
|
{
|
index = 2 * index + 1;
|
pSearch = treeArray[index];
|
}
|
if (!pSearch.isEmpty)
|
//pSearch¼ÓÈëµ½search_pathÖÐ
|
search_path.push(index);
|
}
|
break;
|
default:
|
if (fabs(pBack.dom_elt.z - target.z) < dist) /* Èç¹ûÒÔtargetΪÖÐÐĵÄÔ²£¨Çò»ò³¬Çò£©£¬°ë¾¶ÎªdistµÄÔ²Óë·Ö¸î³¬Æ½ÃæÏཻ£¬ ÄÇô¾ÍÒªÌøµ½ÁíÒ»±ßµÄ×Ó¿Õ¼äÈ¥ËÑË÷ */
|
{
|
if (Distance(nearest, target) > Distance(pBack.dom_elt, target))
|
{
|
nearest = pBack.dom_elt;
|
dist = Distance(pBack.dom_elt, target);
|
}
|
if (target.z <= pBack.dom_elt.z) /* Èç¹ûtargetλÓÚpBackµÄ×ó×ӿռ䣬ÄÇô¾ÍÒªÌøµ½ÓÒ×Ó¿Õ¼äÈ¥ËÑË÷ */
|
{
|
index = 2 * index + 2;
|
pSearch = treeArray[index];
|
}
|
else /* Èç¹ûtargetλÓÚpBackµÄÓÒ×ӿռ䣬ÄÇô¾ÍÒªÌøµ½×ó×Ó¿Õ¼äÈ¥ËÑË÷ */
|
{
|
index = 2 * index + 1;
|
pSearch = treeArray[index];
|
}
|
if (!pSearch.isEmpty)
|
//pSearch¼ÓÈëµ½search_pathÖÐ
|
search_path.push(index);
|
}
|
break;
|
}
|
}
|
|
}
|
|
nearestpoint.x = nearest.x;
|
nearestpoint.y = nearest.y;
|
nearestpoint.z = nearest.z;
|
distance = dist;
|
}
|
|
int getHeight(TreeNode * T)
|
{
|
if (T == nullptr)
|
{
|
return 0;
|
}
|
|
int leftheight = getHeight(T->left);
|
|
int rightheight = getHeight(T->right);
|
|
return max(leftheight, rightheight) + 1;
|
|
}
|
|
void TreeToArray(TreeNode* T, TreeElement* treeArray, int index)
|
{
|
//TreeElement *treeArray = new TreeElement[size];
|
|
if (T != nullptr)
|
{
|
treeArray[index].dom_elt = T->dom_elt;
|
treeArray[index].split = T->split;
|
treeArray[index].isEmpty = false;
|
TreeToArray(T->left, treeArray, 2 * index + 1);
|
TreeToArray(T->right, treeArray, 2 * index + 2);
|
}
|
/*else
|
{
|
treeArray[index].dom_elt = *(new coordinate);
|
treeArray[index].split = 0;
|
treeArray[index].isEmpty = true;
|
}*/
|
}
|
