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