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