[136] | 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 {
|
---|
| 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 | {
|
---|
| 16 | PointPointMinimizer() : calls(0) {}
|
---|
| 17 | typedef double Scalar;
|
---|
| 18 |
|
---|
| 19 | double minimumOnVolumeVolume(const Box2d &r1, const Box2d &r2) { ++calls; return r1.squaredExteriorDistance(r2); }
|
---|
| 20 | double minimumOnVolumeObject(const Box2d &r, const Vector2d &v) { ++calls; return r.squaredExteriorDistance(v); }
|
---|
| 21 | double minimumOnObjectVolume(const Vector2d &v, const Box2d &r) { ++calls; return r.squaredExteriorDistance(v); }
|
---|
| 22 | double minimumOnObjectObject(const Vector2d &v1, const Vector2d &v2) { ++calls; return (v1 - v2).squaredNorm(); }
|
---|
| 23 |
|
---|
| 24 | int calls;
|
---|
| 25 | };
|
---|
| 26 |
|
---|
| 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 | }
|
---|