mirror of
https://gitlab.com/libeigen/eigen.git
synced 2024-12-27 07:29:52 +08:00
222 lines
7.5 KiB
C++
222 lines
7.5 KiB
C++
// This file is part of Eigen, a lightweight C++ template library
|
|
// for linear algebra. Eigen itself is part of the KDE project.
|
|
//
|
|
// Copyright (C) 2009 Ilya Baran <ibaran@mit.edu>
|
|
//
|
|
// Eigen is free software; you can redistribute it and/or
|
|
// modify it under the terms of the GNU Lesser General Public
|
|
// License as published by the Free Software Foundation; either
|
|
// version 3 of the License, or (at your option) any later version.
|
|
//
|
|
// Alternatively, you can redistribute it and/or
|
|
// modify it under the terms of the GNU General Public License as
|
|
// published by the Free Software Foundation; either version 2 of
|
|
// the License, or (at your option) any later version.
|
|
//
|
|
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
|
|
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
|
|
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
|
|
// GNU General Public License for more details.
|
|
//
|
|
// You should have received a copy of the GNU Lesser General Public
|
|
// License and a copy of the GNU General Public License along with
|
|
// Eigen. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
#include <Eigen/StdVector>
|
|
#include "main.h"
|
|
#include <unsupported/Eigen/BVH>
|
|
|
|
inline double SQR(double x) { return x * x; }
|
|
|
|
template<int Dim>
|
|
struct Ball
|
|
{
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(double, Dim)
|
|
|
|
typedef Matrix<double, Dim, 1> VectorType;
|
|
|
|
Ball() {}
|
|
Ball(const VectorType &c, double r) : center(c), radius(r) {}
|
|
|
|
VectorType center;
|
|
double radius;
|
|
};
|
|
|
|
template<int Dim> AlignedBox<double, Dim> ei_bounding_box(const Matrix<double, Dim, 1> &v) { return AlignedBox<double, Dim>(v); }
|
|
template<int Dim> AlignedBox<double, Dim> ei_bounding_box(const Ball<Dim> &b)
|
|
{ return AlignedBox<double, Dim>(b.center.cwise() - b.radius, b.center.cwise() + b.radius); }
|
|
|
|
template<int Dim>
|
|
struct BallPointStuff //this class provides functions to be both an intersector and a minimizer, both for a ball and a point and for two trees
|
|
{
|
|
typedef double Scalar;
|
|
typedef Matrix<double, Dim, 1> VectorType;
|
|
typedef Ball<Dim> BallType;
|
|
typedef AlignedBox<double, Dim> BoxType;
|
|
|
|
BallPointStuff() : calls(0), count(0) {}
|
|
BallPointStuff(const VectorType &inP) : p(inP), calls(0), count(0) {}
|
|
|
|
|
|
bool intersectVolume(const BoxType &r) { ++calls; return r.contains(p); }
|
|
bool intersectObject(const BallType &b) {
|
|
++calls;
|
|
if((b.center - p).squaredNorm() < SQR(b.radius))
|
|
++count;
|
|
return false; //continue
|
|
}
|
|
|
|
bool intersectVolumeVolume(const BoxType &r1, const BoxType &r2) { ++calls; return !(r1.intersection(r2)).isNull(); }
|
|
bool intersectVolumeObject(const BoxType &r, const BallType &b) { ++calls; return r.squaredExteriorDistance(b.center) < SQR(b.radius); }
|
|
bool intersectObjectVolume(const BallType &b, const BoxType &r) { ++calls; return r.squaredExteriorDistance(b.center) < SQR(b.radius); }
|
|
bool intersectObjectObject(const BallType &b1, const BallType &b2){
|
|
++calls;
|
|
if((b1.center - b2.center).norm() < b1.radius + b2.radius)
|
|
++count;
|
|
return false;
|
|
}
|
|
bool intersectVolumeObject(const BoxType &r, const VectorType &v) { ++calls; return r.contains(v); }
|
|
bool intersectObjectObject(const BallType &b, const VectorType &v){
|
|
++calls;
|
|
if((b.center - v).squaredNorm() < SQR(b.radius))
|
|
++count;
|
|
return false;
|
|
}
|
|
|
|
double minimumOnVolume(const BoxType &r) { ++calls; return r.squaredExteriorDistance(p); }
|
|
double minimumOnObject(const BallType &b) { ++calls; return std::max(0., (b.center - p).squaredNorm() - SQR(b.radius)); }
|
|
double minimumOnVolumeVolume(const BoxType &r1, const BoxType &r2) { ++calls; return r1.squaredExteriorDistance(r2); }
|
|
double minimumOnVolumeObject(const BoxType &r, const BallType &b) { ++calls; return SQR(std::max(0., r.exteriorDistance(b.center) - b.radius)); }
|
|
double minimumOnObjectVolume(const BallType &b, const BoxType &r) { ++calls; return SQR(std::max(0., r.exteriorDistance(b.center) - b.radius)); }
|
|
double minimumOnObjectObject(const BallType &b1, const BallType &b2){ ++calls; return SQR(std::max(0., (b1.center - b2.center).norm() - b1.radius - b2.radius)); }
|
|
double minimumOnVolumeObject(const BoxType &r, const VectorType &v) { ++calls; return r.squaredExteriorDistance(v); }
|
|
double minimumOnObjectObject(const BallType &b, const VectorType &v){ ++calls; return SQR(std::max(0., (b.center - v).norm() - b.radius)); }
|
|
|
|
VectorType p;
|
|
int calls;
|
|
int count;
|
|
};
|
|
|
|
template<int Dim>
|
|
struct TreeTest
|
|
{
|
|
typedef Matrix<double, Dim, 1> VectorType;
|
|
typedef Ball<Dim> BallType;
|
|
typedef AlignedBox<double, Dim> BoxType;
|
|
|
|
void testIntersect1()
|
|
{
|
|
std::vector<BallType> b;
|
|
for(int i = 0; i < 500; ++i) {
|
|
b.push_back(BallType(VectorType::Random(), 0.5 * ei_random(0., 1.)));
|
|
}
|
|
KdBVH<double, Dim, BallType> tree(b.begin(), b.end());
|
|
|
|
VectorType pt = VectorType::Random();
|
|
BallPointStuff<Dim> i1(pt), i2(pt);
|
|
|
|
for(int i = 0; i < (int)b.size(); ++i)
|
|
i1.intersectObject(b[i]);
|
|
|
|
BVIntersect(tree, i2);
|
|
|
|
VERIFY(i1.count == i2.count);
|
|
}
|
|
|
|
void testMinimize1()
|
|
{
|
|
std::vector<BallType> b;
|
|
for(int i = 0; i < 500; ++i) {
|
|
b.push_back(BallType(VectorType::Random(), 0.01 * ei_random(0., 1.)));
|
|
}
|
|
KdBVH<double, Dim, BallType> tree(b.begin(), b.end());
|
|
|
|
VectorType pt = VectorType::Random();
|
|
BallPointStuff<Dim> i1(pt), i2(pt);
|
|
|
|
double m1 = std::numeric_limits<double>::max(), m2 = m1;
|
|
|
|
for(int i = 0; i < (int)b.size(); ++i)
|
|
m1 = std::min(m1, i1.minimumOnObject(b[i]));
|
|
|
|
m2 = BVMinimize(tree, i2);
|
|
|
|
VERIFY_IS_APPROX(m1, m2);
|
|
}
|
|
|
|
void testIntersect2()
|
|
{
|
|
std::vector<BallType> b;
|
|
std::vector<VectorType> v;
|
|
|
|
for(int i = 0; i < 50; ++i) {
|
|
b.push_back(BallType(VectorType::Random(), 0.5 * ei_random(0., 1.)));
|
|
for(int j = 0; j < 3; ++j)
|
|
v.push_back(VectorType::Random());
|
|
}
|
|
|
|
KdBVH<double, Dim, BallType> tree(b.begin(), b.end());
|
|
KdBVH<double, Dim, VectorType> vTree(v.begin(), v.end());
|
|
|
|
BallPointStuff<Dim> i1, i2;
|
|
|
|
for(int i = 0; i < (int)b.size(); ++i)
|
|
for(int j = 0; j < (int)v.size(); ++j)
|
|
i1.intersectObjectObject(b[i], v[j]);
|
|
|
|
BVIntersect(tree, vTree, i2);
|
|
|
|
VERIFY(i1.count == i2.count);
|
|
}
|
|
|
|
void testMinimize2()
|
|
{
|
|
std::vector<BallType> b;
|
|
std::vector<VectorType> v;
|
|
|
|
for(int i = 0; i < 50; ++i) {
|
|
b.push_back(BallType(VectorType::Random(), 1e-7 + 1e-6 * ei_random(0., 1.)));
|
|
for(int j = 0; j < 3; ++j)
|
|
v.push_back(VectorType::Random());
|
|
}
|
|
|
|
KdBVH<double, Dim, BallType> tree(b.begin(), b.end());
|
|
KdBVH<double, Dim, VectorType> vTree(v.begin(), v.end());
|
|
|
|
BallPointStuff<Dim> i1, i2;
|
|
|
|
double m1 = std::numeric_limits<double>::max(), m2 = m1;
|
|
|
|
for(int i = 0; i < (int)b.size(); ++i)
|
|
for(int j = 0; j < (int)v.size(); ++j)
|
|
m1 = std::min(m1, i1.minimumOnObjectObject(b[i], v[j]));
|
|
|
|
m2 = BVMinimize(tree, vTree, i2);
|
|
|
|
VERIFY_IS_APPROX(m1, m2);
|
|
}
|
|
};
|
|
|
|
void test_BVH()
|
|
{
|
|
for(int i = 0; i < g_repeat; i++) {
|
|
TreeTest<2> test2;
|
|
CALL_SUBTEST(test2.testIntersect1());
|
|
CALL_SUBTEST(test2.testMinimize1());
|
|
CALL_SUBTEST(test2.testIntersect2());
|
|
CALL_SUBTEST(test2.testMinimize2());
|
|
|
|
TreeTest<3> test3;
|
|
CALL_SUBTEST(test3.testIntersect1());
|
|
CALL_SUBTEST(test3.testMinimize1());
|
|
CALL_SUBTEST(test3.testIntersect2());
|
|
CALL_SUBTEST(test3.testMinimize2());
|
|
|
|
TreeTest<4> test4;
|
|
CALL_SUBTEST(test4.testIntersect1());
|
|
CALL_SUBTEST(test4.testMinimize1());
|
|
CALL_SUBTEST(test4.testIntersect2());
|
|
CALL_SUBTEST(test4.testMinimize2());
|
|
}
|
|
}
|