-
Notifications
You must be signed in to change notification settings - Fork 33
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #394 from MFraters/add_kd-tree
Add kd tree as first step to variable plate depth
- Loading branch information
Showing
3 changed files
with
577 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,156 @@ | ||
/* | ||
Copyright (C) 2022 by the authors of the World Builder code. | ||
This file is part of the World Builder. | ||
This program is free software: you can redistribute it and/or modify | ||
it under the terms of the GNU Lesser General Public License as published | ||
by the Free Software Foundation, either version 2 of the License, or | ||
(at your option) any later version. | ||
This program is distributed in the hope that it will be useful, | ||
but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | ||
GNU Lesser General Public License for more details. | ||
You should have received a copy of the GNU Lesser General Public License | ||
along with this program. If not, see <https://www.gnu.org/licenses/>. | ||
*/ | ||
|
||
#ifndef KD_TREE_H | ||
#define KD_TREE_H | ||
|
||
#include <vector> | ||
#include <cstddef> | ||
#include <iostream> | ||
#include <limits> | ||
|
||
#include "world_builder/assert.h" | ||
#include "world_builder/point.h" | ||
|
||
namespace WorldBuilder | ||
{ | ||
namespace KDTree | ||
{ | ||
/** | ||
* A struct to store an index for a point and the | ||
* distance to that point. | ||
*/ | ||
struct IndexDistance | ||
{ | ||
size_t index; | ||
double distance; | ||
}; | ||
|
||
|
||
/** | ||
* A struct to store an index for a point and the | ||
* distance to that point. | ||
*/ | ||
struct IndexDistances | ||
{ | ||
size_t min_index; | ||
double min_distance; | ||
std::vector<IndexDistance> vector; | ||
}; | ||
|
||
/** | ||
* Each item contains an index and an x and y coordinate | ||
*/ | ||
struct Node | ||
{ | ||
size_t index; | ||
double x; | ||
double y; | ||
|
||
Node(size_t index_, double x_, double y_) | ||
: | ||
index(index_), | ||
x(x_), | ||
y(y_) | ||
{} | ||
|
||
|
||
/** | ||
* access index (const) | ||
*/ | ||
inline | ||
const double &operator[](const bool y_axis) const | ||
{ | ||
WBAssert(abs((y_axis ? y : x) - *(&x+y_axis)) < std::numeric_limits<double>::epsilon(), | ||
"Internal error: y_axis=" << y_axis << ", x=" << x << ", y=" << y <<", *(&x+y_axis)=" << *(&x+y_axis) << ", ((bool)y_axis ? x : y) - *(&x+y_axis)=" << abs(((bool)y_axis ? x : y) - *(&x+y_axis))); | ||
return *(&x+y_axis); | ||
} | ||
}; | ||
|
||
class KDTree | ||
{ | ||
public: | ||
/** | ||
* Constructor. Requires a list of Nodes. | ||
*/ | ||
KDTree(const std::vector<Node> point_list); | ||
|
||
/** | ||
* Create a tree based on the current information in the | ||
* node vector. | ||
*/ | ||
void create_tree(const size_t left, | ||
const size_t right, | ||
const bool x_axis); | ||
|
||
/** | ||
* Retun a reference to the vector containing the nodes. | ||
*/ | ||
std::vector<Node> &get_nodes(); | ||
|
||
/** | ||
* Returns the index of the closest point and the distance | ||
* of that point to the check point. | ||
*/ | ||
IndexDistance find_closest_point(const Point<2> &check_point) const; | ||
|
||
|
||
|
||
/** | ||
* Returns the index of the closest point and the distance | ||
* of that point to the check point. Stores the points searched | ||
* through in a unsorted vector. | ||
* Note: I can only guarentee that the point with the least distance | ||
* is the closest point. The point in the list with the second/third/etc. | ||
* smallest distance may or may not actually be the global point | ||
* with the second/third/etc. smallest distance. It will most likely be a | ||
* very good guess though. | ||
*/ | ||
IndexDistances find_closest_points(const Point<2> &check_point) const; | ||
|
||
|
||
private: | ||
/** | ||
* Returns the index of the closest point and the distance | ||
* of that point to the check point. This function is used | ||
* by find_closest_point to find the correct point. | ||
*/ | ||
void find_closest_point_recursive(const Point<2> &check_point, | ||
const size_t left, | ||
const size_t right, | ||
const bool y_axis, | ||
IndexDistance &index_distance) const; | ||
|
||
|
||
/** | ||
* Returns the index of the closest point and the distance | ||
* of that point to the check point. This function is used | ||
* by find_closest_point to find the correct point. | ||
*/ | ||
void find_closest_points_recursive(const Point<2> &check_point, | ||
const size_t left, | ||
const size_t right, | ||
const bool y_axis, | ||
IndexDistances &index_distances) const; | ||
|
||
std::vector<Node> nodes; | ||
}; | ||
} | ||
} | ||
#endif |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,207 @@ | ||
/* | ||
Copyright (C) 2022 by the authors of the World Builder code. | ||
This file is part of the World Builder. | ||
This program is free software: you can redistribute it and/or modify | ||
it under the terms of the GNU Lesser General Public License as published | ||
by the Free Software Foundation, either version 2 of the License, or | ||
(at your option) any later version. | ||
This program is distributed in the hope that it will be useful, | ||
but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | ||
GNU Lesser General Public License for more details. | ||
You should have received a copy of the GNU Lesser General Public License | ||
along with this program. If not, see <https://www.gnu.org/licenses/>. | ||
*/ | ||
|
||
#include "world_builder/kd_tree.h" | ||
#include <algorithm> | ||
|
||
namespace WorldBuilder | ||
{ | ||
namespace KDTree | ||
{ | ||
KDTree::KDTree(std::vector<Node> point_list) | ||
: nodes(point_list) | ||
{} | ||
|
||
|
||
void | ||
KDTree::create_tree(const size_t left, | ||
const size_t right, | ||
const bool y_axis) | ||
{ | ||
size_t mid=(left+right)>>1; | ||
std::nth_element(nodes.begin()+left,nodes.begin()+mid,nodes.begin()+right+1, | ||
[y_axis](Node& i, Node& j) -> bool | ||
{ | ||
return i[y_axis] < j[y_axis]; | ||
}); | ||
|
||
if (left<mid) | ||
create_tree(left,mid-1,!y_axis); | ||
if (right>mid) | ||
create_tree(mid+1,right,!y_axis); | ||
} | ||
|
||
|
||
std::vector<Node> & | ||
KDTree::get_nodes() | ||
{ | ||
return nodes; | ||
} | ||
|
||
|
||
IndexDistance | ||
KDTree::find_closest_point(const Point<2> &check_point) const | ||
{ | ||
size_t start_node_index = 0; | ||
IndexDistance index_distance = {start_node_index, | ||
std::numeric_limits<double>::max() | ||
}; | ||
|
||
find_closest_point_recursive(check_point,0,nodes.size()-1,false,index_distance); | ||
|
||
return index_distance; | ||
} | ||
|
||
|
||
void | ||
KDTree::find_closest_point_recursive(const Point<2> &check_point, | ||
const size_t left, | ||
const size_t right, | ||
const bool y_axis, | ||
IndexDistance &index_distance) const | ||
{ | ||
// Calculate the index of this node | ||
const size_t mid=(left+right)>>1; | ||
if (check_point[y_axis] < nodes[mid][y_axis]) | ||
{ | ||
// Traverse left child | ||
if (left<mid) | ||
{ | ||
find_closest_point_recursive(check_point,left,mid-1,!y_axis,index_distance); | ||
} | ||
|
||
// Compare node's point to current closest point | ||
double distance = sqrt((nodes[mid][0]-check_point[0])*(nodes[mid][0]-check_point[0]) | ||
+(nodes[mid][1]-check_point[1])*(nodes[mid][1]-check_point[1])); | ||
if (index_distance.distance > distance) | ||
{ | ||
index_distance.index = mid; | ||
index_distance.distance = distance; | ||
} | ||
|
||
// Traverse right child | ||
if (right>mid) | ||
{ | ||
if ((nodes[mid][y_axis]-check_point[y_axis]) < index_distance.distance) | ||
{ | ||
find_closest_point_recursive(check_point,mid+1,right,!y_axis,index_distance); | ||
} | ||
} | ||
} | ||
else | ||
{ | ||
// Traverse right child | ||
if (right>mid) | ||
find_closest_point_recursive(check_point,mid+1,right,!y_axis,index_distance); | ||
|
||
// Compare node's point to current closest point | ||
double distance = sqrt((nodes[mid][0]-check_point[0])*(nodes[mid][0]-check_point[0]) | ||
+(nodes[mid][1]-check_point[1])*(nodes[mid][1]-check_point[1])); | ||
if (index_distance.distance > distance) | ||
{ | ||
index_distance.index = mid; | ||
index_distance.distance = distance; | ||
} | ||
|
||
// Traverse left child | ||
if (left<mid) | ||
if ((nodes[mid][y_axis]-check_point[y_axis]) < index_distance.distance) | ||
find_closest_point_recursive(check_point,left,mid-1,!y_axis,index_distance); | ||
} | ||
} | ||
|
||
|
||
|
||
IndexDistances | ||
KDTree::find_closest_points(const Point<2> &check_point) const | ||
{ | ||
size_t start_node_index = 0; | ||
IndexDistances index_distances = {start_node_index, | ||
std::numeric_limits<double>::max(), | ||
{} | ||
}; | ||
|
||
find_closest_points_recursive(check_point,0,nodes.size()-1,false,index_distances); | ||
|
||
return index_distances; | ||
} | ||
|
||
|
||
void | ||
KDTree::find_closest_points_recursive(const Point<2> &check_point, | ||
const size_t left, | ||
const size_t right, | ||
const bool y_axis, | ||
IndexDistances &index_distances) const | ||
{ | ||
// Calculate the index of this node | ||
const size_t mid=(left+right)>>1; | ||
if (check_point[y_axis] < nodes[mid][y_axis]) | ||
{ | ||
// Traverse left child | ||
if (left<mid) | ||
{ | ||
find_closest_points_recursive(check_point,left,mid-1,!y_axis,index_distances); | ||
} | ||
|
||
// Compare node's point to current closest point | ||
double distance = sqrt((nodes[mid][0]-check_point[0])*(nodes[mid][0]-check_point[0]) | ||
+(nodes[mid][1]-check_point[1])*(nodes[mid][1]-check_point[1])); | ||
if (index_distances.min_distance > distance) | ||
{ | ||
index_distances.min_index = mid; | ||
index_distances.min_distance = distance; | ||
} | ||
|
||
index_distances.vector.push_back({mid, distance}); | ||
|
||
// Traverse right child | ||
if (right>mid) | ||
{ | ||
if ((nodes[mid][y_axis]-check_point[y_axis]) < index_distances.min_distance) | ||
{ | ||
find_closest_points_recursive(check_point,mid+1,right,!y_axis,index_distances); | ||
} | ||
} | ||
} | ||
else | ||
{ | ||
// Traverse right child | ||
if (right>mid) | ||
find_closest_points_recursive(check_point,mid+1,right,!y_axis,index_distances); | ||
|
||
// Compare node's point to current closest point | ||
double distance = sqrt((nodes[mid][0]-check_point[0])*(nodes[mid][0]-check_point[0]) | ||
+(nodes[mid][1]-check_point[1])*(nodes[mid][1]-check_point[1])); | ||
if (index_distances.min_distance > distance) | ||
{ | ||
index_distances.min_index = mid; | ||
index_distances.min_distance = distance; | ||
} | ||
|
||
index_distances.vector.push_back({mid, distance}); | ||
|
||
// Traverse left child | ||
if (left<mid) | ||
if ((nodes[mid][y_axis]-check_point[y_axis]) < index_distances.min_distance) | ||
find_closest_points_recursive(check_point,left,mid-1,!y_axis,index_distances); | ||
} | ||
} | ||
} | ||
} |
Oops, something went wrong.