20 #pragma warning(disable : 4512) // boost code: no assignment operator
21 #include <boost/geometry.hpp>
22 #include <boost/geometry/geometries/register/point.hpp>
23 #include <boost/geometry/index/rtree.hpp>
24 #include <boost/iterator/counting_iterator.hpp>
52 bPt(
double a_x,
double a_y,
double a_z)
82 namespace bg = boost::geometry;
83 namespace bgi = bg::index;
86 typedef bg::model::box<bPt> box;
90 typedef bgi::quadratic<8> qRtree;
112 template <
typename value>
143 explicit idx_pt(BSHP<std::vector<Pt3d>> a_,
bool a_2d)
156 return bPt(
m_[i].x,
m_[i].y,
m_2d ? 0 :
m_[i].z);
158 return bPt((*
m_v)[i].x, (*
m_v)[i].y,
m_2d ? 0 : (*
m_v)[i].z);
163 BSHP<std::vector<Pt3d>>
m_v;
182 bool a_quad_oct_Search,
183 std::vector<int>& a_nearest)
const override;
188 bool a_quad_oct_Search,
189 std::vector<int>& a_nearest)
const override;
191 virtual bool PtInRTree(
const Pt3d& a_pt,
const double a_tol)
override;
196 std::vector<int>& a_nearest)
const override;
200 bool a_quad_oct_Search,
201 std::vector<int>& a_nearest,
213 virtual std::string
ToString()
const override;
244 GmPtSearch::GmPtSearch()
250 GmPtSearch::~GmPtSearch()
262 : m_2dSearch(a_2dSearch)
272 GmPtSearchImpl::~GmPtSearchImpl()
289 m_rTree =
new bgi::rtree<size_t, qRtree, idx_pt>(
290 boost::counting_iterator<size_t>(0), boost::counting_iterator<size_t>(
m_bshpPt3d->size()),
302 for (
size_t i = 0; i < a_nPts; ++i)
334 m_rTree =
new bgi::rtree<size_t, qRtree, idx_pt>(boost::counting_iterator<size_t>(0),
335 boost::counting_iterator<size_t>(a_->size()),
356 a_ptIdx = *std::min_element(n.begin(), n.end());
390 bool a_quad_oct_Search,
391 std::vector<int>& a_nearest)
const
399 NearestPtsToPt(a_pt, a_numPtsToFind, a_quad_oct_Search, a_nearest, fs);
418 bool a_quad_oct_Search,
419 std::vector<int>& a_nearest,
422 std::vector<value> nearest;
430 if (!a_quad_oct_Search)
433 m_rTree->query(bgi::satisfies(*a_fsat) && bgi::nearest(tmpPt, a_numPtsToFind),
434 std::back_inserter(nearest));
436 m_rTree->query(bgi::nearest(tmpPt, a_numPtsToFind), std::back_inserter(nearest));
437 a_nearest.reserve(nearest.size());
438 for (
size_t i = 0; i < nearest.size(); ++i)
440 a_nearest.push_back((
int)nearest[i]);
448 std::vector<box> boxes;
450 for (
size_t i = 0; i < boxes.size(); ++i)
452 std::vector<value> foundPts;
454 bgi::covered_by(boxes[i]) && bgi::satisfies(*fPtr) && bgi::nearest(tmpPt, a_numPtsToFind),
455 std::back_inserter(foundPts));
456 a_nearest.reserve(a_nearest.size() + foundPts.size());
457 for (
size_t j = 0; j < foundPts.size(); ++j)
459 a_nearest.push_back((
int)foundPts[j]);
460 fPtr->m_bits.set(foundPts[j]);
464 std::sort(a_nearest.begin(), a_nearest.end());
482 bool a_quad_oct_Search,
483 std::vector<int>& a_nearest)
const
488 fsat.m_bits.set(a_ptIdx);
489 NearestPtsToPt(a_pt, a_numPtsToFind, a_quad_oct_Search, a_nearest, &fsat);
520 std::vector<int>& a_nearest)
const
526 fsat.m_bits.set(a_ptIdx);
528 Pt3d bMin(a_pt - a_distance), bMax(a_pt + a_distance);
535 box aBox(bMin, bMax);
537 std::vector<value> nearest;
540 m_rTree->query(bgi::satisfies(fsat) && bgi::covered_by(aBox), std::back_inserter(nearest));
541 a_nearest.reserve(nearest.size());
542 for (
size_t i = 0; i < nearest.size(); ++i)
544 a_nearest.push_back((
int)nearest[i]);
555 if (a_activity.size() ==
m_rTree->size())
577 std::stringstream ss;
597 box bound(bmin, bmax);
599 bound.min_corner().z = pt.
z;
608 aBox.max_corner().x = pt.
x;
609 aBox.min_corner().y = pt.
y;
610 a_boxes.push_back(aBox);
613 aBox.min_corner() = pt;
615 aBox.min_corner().
z = -1;
616 a_boxes.push_back(aBox);
619 aBox.max_corner() = pt;
622 aBox.max_corner().z = 1;
623 a_boxes.push_back(aBox);
626 aBox.min_corner().x = pt.
x;
627 aBox.max_corner().y = pt.
y;
628 a_boxes.push_back(aBox);
632 for (
int i = 0; i < 4; ++i)
634 a_boxes.push_back(a_boxes[i]);
635 a_boxes.back().max_corner().z = pt.
z;
636 a_boxes.back().min_corner().z =
m_min.
z;
646 #include <boost/assign.hpp>
658 void iGetPoints2(std::vector<Pt3d>& a_pts, std::vector<float>& a_scalar, std::vector<Pt3d>& a_iPts)
660 a_pts = {{-75.0, -16.0, 0.0}, {-60.0, 32.0, 0.0}, {-34.0, 50.0, 0.0}, {-34.0, 27.0, 0.0},
661 {-8.0, 40.0, 0.0}, {16.0, 38.0, 0.0}, {-25.0, 14.0, 43.64}, {10.0, 18.0, 44.16},
662 {27.0, 26.0, 0.0}, {63.0, 35.0, 0.0}, {-32.0, 0.0, 59.04}, {-7.0, 7.0, 90.2},
663 {26.0, 6.0, 67.2}, {75.0, 7.0, 0.0}, {-37.0, -15.0, 9.24}, {-7.0, -13.0, 71.0},
664 {2.0, -3.0, 98.4}, {31.0, -15.0, 25.56}, {60.0, -13.0, 0.0}, {-50.0, -30.0, 0.0},
665 {-30.0, -28.0, 0.0}, {43.0, -22.0, 0.0}, {-32.0, -50.0, 0.0}, {27.0, -37.0, 0.0},
668 a_scalar.resize(a_pts.size());
669 for (
size_t i = 0; i < a_pts.size(); ++i)
670 a_scalar[i] = (
float)a_pts[i].z;
671 a_iPts = {{-90, 60, 0}, {90, 60, 0}, {-90, -60, 0}, {90, -60, 0}, {60, -33, 0}};
675 #define _TS_ASSERT_POINTS_FOUND(a_file, a_line, a_required, a_optional, a_found) \
676 iAssertPointsFound(a_file, a_line, a_required, a_optional, a_found)
677 #define TS_ASSERT_POINTS_FOUND(a_required, a_optional, a_found) \
679 _TS_ASSERT_POINTS_FOUND(__FILE__, __LINE__, a_required, a_optional, a_found)
690 const std::vector<int>& a_required,
691 const std::vector<int>& a_optional,
692 const std::vector<int>& a_found)
695 std::vector<bool> handled(a_found.size(),
false);
696 for (
size_t i = 0; i < a_required.size(); ++i)
698 int idx = a_required[i];
699 auto it = std::find(a_found.begin(), a_found.end(), idx);
700 bool found = it != a_found.end();
703 handled[it - a_found.begin()] = found;
707 std::stringstream ss;
708 ss <<
"Result failed to include " << idx;
709 _TS_FAIL(a_file, a_line, ss.str().c_str());
714 for (
size_t i = 0; i < handled.size(); ++i)
718 int idx = a_found[i];
719 bool found = std::find(a_optional.begin(), a_optional.end(), idx) != a_optional.end();
722 std::stringstream ss;
723 ss <<
"Failed to find " << idx <<
" in optional points";
724 _TS_FAIL(a_file, a_line, ss.str().c_str());
746 static void iSetupPts(std::vector<Pt3d>& pts,
bool a_2d)
750 for (
int i = 1; i < 11; ++i)
752 for (
int j = 1; j < 11; ++j)
754 pts.push_back(
Pt3d(i, j, 0));
756 pts.push_back(
Pt3d(-i, j, 0));
757 pts.push_back(
Pt3d(i, -j, 0));
759 pts.push_back(
Pt3d(-i, -j, 0));
764 std::vector<Pt3d> pts1(pts);
765 for (
size_t i = 0; i < pts1.size(); ++i)
766 pts1[i].z = .1 * (
double)(i + 1);
767 pts.insert(pts.end(), pts1.begin(), pts1.end());
768 for (
size_t i = 0; i < pts1.size(); ++i)
769 pts1[i].z = -.1 * (
double)(i + 1);
770 pts.insert(pts.end(), pts1.begin(), pts1.end());
777 BSHP<std::vector<Pt3d>> pts(
new std::vector<Pt3d>());
781 iPts->PtsToSearch(pts);
782 TS_ASSERT_EQUALS(
true, iPts->Is2D());
785 std::vector<int> foundPts, requiredPts, optionalPts;
786 iPts->NearestPtsToPt(aPt, 8,
false, foundPts);
787 requiredPts = {0, 1, 2, 3, 4, 6, 40, 41};
788 std::sort(foundPts.begin(), foundPts.end());
789 TS_ASSERT_EQUALS(8, foundPts.size());
792 iPts->NearestPtsToPt(aPt, 4,
true, foundPts);
793 requiredPts = {0, 1, 2, 3, 4, 5, 6, 7, 9, 11, 40, 41, 42, 43, 61, 63};
794 std::sort(foundPts.begin(), foundPts.end());
795 TS_ASSERT_EQUALS(16, foundPts.size());
800 iPts->NearestPtsToPt(aPt, 8,
false, foundPts);
801 requiredPts = {61, 65, 69, 73, 121, 125, 129, 133};
802 std::sort(foundPts.begin(), foundPts.end());
803 TS_ASSERT_EQUALS(8, foundPts.size());
806 iPts->NearestPtsToPt(aPt, 4,
true, foundPts);
807 requiredPts = {1, 5, 9, 61, 65, 69, 73, 77, 121, 125, 129, 133, 137, 181, 185, 189};
808 std::sort(foundPts.begin(), foundPts.end());
809 TS_ASSERT_EQUALS(16, foundPts.size());
814 iPts->NearestPtsToPt(aPt, 4,
true, foundPts);
815 requiredPts = {0, 1, 2, 3, 4, 5, 6, 7, 9, 11, 40, 41, 61, 63, 65, 67};
816 std::sort(foundPts.begin(), foundPts.end());
817 TS_ASSERT_EQUALS(16, foundPts.size());
820 iPts->NearestPtsToPt(aPt, 4,
true, foundPts);
821 requiredPts = {0, 1, 2, 3, 4, 5, 6, 7, 9, 11, 40, 41, 61, 63, 65, 67};
822 std::sort(foundPts.begin(), foundPts.end());
823 TS_ASSERT_EQUALS(16, foundPts.size());
831 BSHP<std::vector<Pt3d>> pts(
new std::vector<Pt3d>());
832 std::vector<Pt3d> ipts;
833 std::vector<float> scalar, s, vBase;
837 iPts->PtsToSearch(pts);
839 std::vector<int> foundPts, requiredPts, optionalPts;
840 iPts->NearestPtsToPt(ipts[0], 16,
false, foundPts);
841 requiredPts = {0, 1, 2, 3, 4, 5, 6, 7, 8, 10, 11, 14, 15, 16, 19, 20};
842 std::sort(foundPts.begin(), foundPts.end());
843 TS_ASSERT_EQUALS(16, foundPts.size());
851 BSHP<std::vector<Pt3d>> pts(
new std::vector<Pt3d>());
855 iPts->PtsToSearch(pts);
858 std::vector<int> foundPts, requiredPts, optionalPts;
859 iPts->NearestPtsToPt(aPt, 16,
false, foundPts);
860 requiredPts = {0, 1, 2, 3, 300, 301, 302, 303, 600, 601, 602, 603};
861 optionalPts = {4, 5, 6, 7, 40, 41};
862 TS_ASSERT_EQUALS(16, foundPts.size());
872 pts.push_back(
Pt3d(1, 0, 0));
873 pts.push_back(
Pt3d(2, 0, 0));
874 pts.push_back(
Pt3d(-1, 0, 0));
875 pts.push_back(
Pt3d(-2, 0, 0));
876 pts.push_back(
Pt3d(0, 1, 0));
877 pts.push_back(
Pt3d(0, 2, 0));
878 pts.push_back(
Pt3d(0, -1, 0));
879 pts.push_back(
Pt3d(0, -2, 0));
880 pts.push_back(
Pt3d(0, 0, 1));
881 pts.push_back(
Pt3d(0, 0, 2));
882 pts.push_back(
Pt3d(0, 0, -1));
883 pts.push_back(
Pt3d(0, 0, -2));
887 pts.push_back(
Pt3d(1, 1, 2));
888 pts.push_back(
Pt3d(2, 1, 2));
889 pts.push_back(
Pt3d(1, 2, 2));
890 pts.push_back(
Pt3d(1, 1, 3));
892 pts.push_back(
Pt3d(-1, 1, 2));
893 pts.push_back(
Pt3d(-2, 1, 2));
894 pts.push_back(
Pt3d(-1, 2, 2));
895 pts.push_back(
Pt3d(-1, 1, 3));
897 pts.push_back(
Pt3d(1, -1, 2));
898 pts.push_back(
Pt3d(2, -1, 2));
899 pts.push_back(
Pt3d(1, -2, 2));
900 pts.push_back(
Pt3d(1, -1, 3));
902 pts.push_back(
Pt3d(-1, -1, 2));
903 pts.push_back(
Pt3d(-2, -1, 2));
904 pts.push_back(
Pt3d(-1, -2, 2));
905 pts.push_back(
Pt3d(-1, -1, 3));
908 pts.push_back(
Pt3d(1, 1, -2));
909 pts.push_back(
Pt3d(2, 1, -2));
910 pts.push_back(
Pt3d(1, 2, -2));
911 pts.push_back(
Pt3d(1, 1, -3));
913 pts.push_back(
Pt3d(-1, 1, -2));
914 pts.push_back(
Pt3d(-2, 1, -2));
915 pts.push_back(
Pt3d(-1, 2, -2));
916 pts.push_back(
Pt3d(-1, 1, -3));
918 pts.push_back(
Pt3d(1, -1, -2));
919 pts.push_back(
Pt3d(2, -1, -2));
920 pts.push_back(
Pt3d(1, -2, -2));
921 pts.push_back(
Pt3d(1, -1, -3));
923 pts.push_back(
Pt3d(-1, -1, -2));
924 pts.push_back(
Pt3d(-2, -1, -2));
925 pts.push_back(
Pt3d(-1, -2, -2));
926 pts.push_back(
Pt3d(-1, -1, -3));
934 BSHP<std::vector<Pt3d>> pts(
new std::vector<Pt3d>());
938 iPts->PtsToSearch(pts);
941 std::vector<int> foundPts;
942 iPts->NearestPtsToPt(aPt, 4,
true, foundPts);
943 std::vector<int> requiredPts = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 20, 21, 22,
944 23, 24, 28, 29, 30, 31, 32, 36, 37, 38, 39, 40, 41, 42, 43};
945 std::vector<int> optionalPts = {33, 34};
946 std::sort(foundPts.begin(), foundPts.end());
947 TS_ASSERT_EQUALS(32, foundPts.size());
950 iPts->NearestPtsToPt(aPt, 4,
true, foundPts);
951 std::sort(foundPts.begin(), foundPts.end());
952 TS_ASSERT_EQUALS(32, foundPts.size());
960 BSHP<std::vector<Pt3d>> pts(
new std::vector<Pt3d>());
964 iPts->PtsToSearch(pts);
968 iPts->SetActivity(activity_wrongSize);
971 std::vector<int> foundPts, requiredPts, optionalPts;
972 iPts->NearestPtsToPt(aPt, 8,
false, foundPts);
973 requiredPts = {0, 1, 2, 3, 4, 6, 40, 41};
974 std::sort(foundPts.begin(), foundPts.end());
975 TS_ASSERT_EQUALS(8, foundPts.size());
982 iPts->SetActivity(act);
983 iPts->NearestPtsToPt(aPt, 8,
false, foundPts);
984 requiredPts = {0, 1, 3, 4, 6, 41, 42};
985 optionalPts = {7, 43};
986 std::sort(foundPts.begin(), foundPts.end());
987 TS_ASSERT_EQUALS(8, foundPts.size());
995 BSHP<std::vector<Pt3d>> pts(
new std::vector<Pt3d>());
999 iPts->PtsToSearch(pts);
1003 iPts->SetActivity(activity_wrongSize);
1006 std::vector<int> foundPts;
1007 iPts->NearestPtsToPt(aPt, 4,
true, foundPts);
1008 std::vector<int> requiredPts = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 20, 21, 22,
1009 23, 24, 28, 29, 30, 31, 32, 36, 37, 38, 39, 40, 41, 42, 43};
1010 std::vector<int> optionalPts = {33, 34};
1011 std::sort(foundPts.begin(), foundPts.end());
1012 TS_ASSERT_EQUALS(32, foundPts.size());
1019 iPts->SetActivity(act);
1020 iPts->NearestPtsToPt(aPt, 4,
true, foundPts);
1021 requiredPts = {0, 1, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 20, 21, 22,
1022 23, 24, 28, 29, 30, 31, 32, 36, 37, 38, 39, 41, 42, 43};
1023 optionalPts = {13, 14, 25, 33, 34};
1024 std::sort(foundPts.begin(), foundPts.end());
1025 TS_ASSERT_EQUALS(31, foundPts.size());
1033 BSHP<std::vector<Pt3d>> pts(
new std::vector<Pt3d>());
1034 *pts = {{0, 0, 0}, {1, 0, 0}, {2, 0, 0}, {1, 1, 0}, {1, -1, 0}};
1041 std::vector<int> base;
1043 base = {0, 2, 3, 4};
1055 BSHP<std::vector<Pt3d>> pts(
new std::vector<Pt3d>());
1056 *pts = {{0, 0, 0}, {1, 0, 0}, {2, 0, 0}, {1, 1, 0}, {1, -1, 0}};
1065 TS_ASSERT_EQUALS(0, ptIdx);
1067 TS_ASSERT_EQUALS(5, ptIdx);
virtual bool PtInRTree(const Pt3d &a_pt, const double a_tol) override
Checks if the point is in the Rtree within tolerance.
Spatial index for searching points.
DynBitset m_activity
point activity
void iGetPoints2(std::vector< Pt3d > &a_pts, std::vector< float > &a_scalar, std::vector< Pt3d > &a_iPts)
Get GMS tutorial data.
bool m_2dSearch
flag specifying 2d searching only
BOOST_GEOMETRY_REGISTER_POINT_3D(bPt, double, cs::cartesian, x, y, z)
result_type operator()(size_t i) const
operator to return the point location based on an index
bool operator()(value const &a_) const
Determines if a point can be returned. If the bit is not set yet.
static void iSetupPts(std::vector< Pt3d > &pts, bool a_2d)
helper function for testing
virtual bool Is2D() const override
Returns true if class only searches in 2D.
void testTest3d()
testing in 3d
const bPt result_type
created to follow boost examples
virtual void PtsToSearch(BSHP< std::vector< Pt3d >> a_pts) override
Adds the point locations to the class.
GmPtSearchImpl(bool a_2dSearch)
Constructor.
virtual bool AddPtToVectorIfUnique(const Pt3d &a_, double a_tol, int &a_ptIdx) override
Adds the point to the R-tree if the point location is unique based on the passed in tolerance...
fSatisfies(size_t a_nVals)
Constructor.
void VecToStream(std::stringstream &a_ss, const T &a_v, std::string a_label)
Pt3d m_min
minimum extents of points
static void iSetupPtsOctant(std::vector< Pt3d > &pts)
helper function for testing
void testActivity2d()
testing searching with activity
#define TS_ASSERT_POINTS_FOUND(a_required, a_optional, a_found)
macro for testing
BSHP< std::vector< Pt3d > > m_v
vector of point locations
DynBitset m_bits
bitset that is the size of the points
boost::dynamic_bitset< size_t > DynBitset
void testCreateClass()
tests creating the class
const Pt3d * m_
array of point locations
virtual void NearestPtsToPtInRtree(int a_ptIdx, const Pt3d &a_pt, int a_numPtsToFind, bool a_quad_oct_Search, std::vector< int > &a_nearest) const override
Finds the nearest points to the input a_pt with an option to search quadrants/octants. Similar to NearestPtsToPt but this method will always pass the fSatisfies class to the RTree. This method is used to find the nearest points to a point that is included in the RTree.
virtual DynBitset GetActivity() override
Returns the point activity.
void CreateOctants(const Pt3d &a_pt, std::vector< box > &a_boxes) const
Creates octants (or quadrants for 2d) to be used in the rtree query.
void testTest2dTutData()
testing tutorial data from GMS in 2d
virtual void PtsWithinDistanceToPtInRtree(int a_ptIdx, const Pt3d &a_pt, double a_dist, std::vector< int > &a_nearest) const override
Finds the nearest points to the input a_pt with an option to search quadrants/octants. Similar to NearestPtsToPt but this method will always pass the fSatisfies class to the RTree. This method is used to find the nearest points to a point that is included in the RTree.
virtual void NearestPtsToPt(const Pt3d &a_pt, int a_numPtsToFind, bool a_quad_oct_Search, std::vector< int > &a_nearest) const override
Finds the nearest points to the input a_pt with an option to search quadrants/octants.
static const float XM_FLT_HIGHEST
class for indexing the points
virtual void VectorThatGrowsToSearch(BSHP< std::vector< Pt3d >> a_) override
Adds the point locations to the class.
void UpdateMinMax(const Pt3d *a_pts, size_t a_npts)
Updates the m_min, m_max variables.
a class used with the boost::geometry::index::satisfies function
void testTest3dOct()
testing octant searching in 3d
bool m_2d
flag specifying 2d searching only
idx_pt(const Pt3d *a_, bool a_2d)
constructor
static BSHP< GmPtSearch > New(bool a_2dSearch)
Creates an PtSearch class.
idx_pt(BSHP< std::vector< Pt3d >> a_, bool a_2d)
constructor
Pt3d m_max
maximum extents of points
void testVectorThatGrows()
testing VectorThatGrows functionality
bgi::rtree< value, qRtree, idx_pt > * m_rTree
spatial index for searching the points
virtual void SetActivity(DynBitset &a_activity) override
Sets activity on the points in the Rtree so that points can be ignored when interpolating.
virtual const BSHP< VecPt3d > GetPointsPtr() const override
Returns shared point to the point locations in the RTree.
Implementation of GmPtSearch. Generic class for searching location data. Uses a boost R-tree to query...
BSHP< VecPt3d > m_bshpPt3d
Vector of point locations.
void testActivity3d()
testing searching with activity
void testPtsWithinDist()
testing Point within a distance to a specified point
void testTest2d()
testing in 2d
void iAssertPointsFound(const char *a_file, int a_line, const std::vector< int > &a_required, const std::vector< int > &a_optional, const std::vector< int > &a_found)
helper function for testing
virtual std::string ToString() const override
Write the internals to a string.
static const float XM_FLT_LOWEST