1 #include <Eigen/StdVector>
2 #include <unsupported/Eigen/BVH>
3 #include <iostream>
4
5 using namespace Eigen;
6 typedef AlignedBox<double, 2> Box2d;
7
8 namespace Eigen {
9 namespace internal {
bounding_box(const Vector2d & v)10 Box2d bounding_box(const Vector2d &v) { return Box2d(v, v); } //compute the bounding box of a single point
11 }
12 }
13
14 struct PointPointMinimizer //how to compute squared distances between points and rectangles
15 {
PointPointMinimizerPointPointMinimizer16 PointPointMinimizer() : calls(0) {}
17 typedef double Scalar;
18
minimumOnVolumeVolumePointPointMinimizer19 double minimumOnVolumeVolume(const Box2d &r1, const Box2d &r2) { ++calls; return r1.squaredExteriorDistance(r2); }
minimumOnVolumeObjectPointPointMinimizer20 double minimumOnVolumeObject(const Box2d &r, const Vector2d &v) { ++calls; return r.squaredExteriorDistance(v); }
minimumOnObjectVolumePointPointMinimizer21 double minimumOnObjectVolume(const Vector2d &v, const Box2d &r) { ++calls; return r.squaredExteriorDistance(v); }
minimumOnObjectObjectPointPointMinimizer22 double minimumOnObjectObject(const Vector2d &v1, const Vector2d &v2) { ++calls; return (v1 - v2).squaredNorm(); }
23
24 int calls;
25 };
26
main()27 int main()
28 {
29 typedef std::vector<Vector2d, aligned_allocator<Vector2d> > StdVectorOfVector2d;
30 StdVectorOfVector2d redPoints, bluePoints;
31 for(int i = 0; i < 100; ++i) { //initialize random set of red points and blue points
32 redPoints.push_back(Vector2d::Random());
33 bluePoints.push_back(Vector2d::Random());
34 }
35
36 PointPointMinimizer minimizer;
37 double minDistSq = std::numeric_limits<double>::max();
38
39 //brute force to find closest red-blue pair
40 for(int i = 0; i < (int)redPoints.size(); ++i)
41 for(int j = 0; j < (int)bluePoints.size(); ++j)
42 minDistSq = std::min(minDistSq, minimizer.minimumOnObjectObject(redPoints[i], bluePoints[j]));
43 std::cout << "Brute force distance = " << sqrt(minDistSq) << ", calls = " << minimizer.calls << std::endl;
44
45 //using BVH to find closest red-blue pair
46 minimizer.calls = 0;
47 KdBVH<double, 2, Vector2d> redTree(redPoints.begin(), redPoints.end()), blueTree(bluePoints.begin(), bluePoints.end()); //construct the trees
48 minDistSq = BVMinimize(redTree, blueTree, minimizer); //actual BVH minimization call
49 std::cout << "BVH distance = " << sqrt(minDistSq) << ", calls = " << minimizer.calls << std::endl;
50
51 return 0;
52 }
53