[136] | 1 | // This file is part of Eigen, a lightweight C++ template library
|
---|
| 2 | // for linear algebra.
|
---|
| 3 | //
|
---|
| 4 | // Copyright (C) 2009 Ilya Baran <ibaran@mit.edu>
|
---|
| 5 | //
|
---|
| 6 | // This Source Code Form is subject to the terms of the Mozilla
|
---|
| 7 | // Public License v. 2.0. If a copy of the MPL was not distributed
|
---|
| 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
---|
| 9 |
|
---|
| 10 | #include "main.h"
|
---|
| 11 | #include <Eigen/StdVector>
|
---|
| 12 | #include <Eigen/Geometry>
|
---|
| 13 | #include <unsupported/Eigen/BVH>
|
---|
| 14 |
|
---|
| 15 | namespace Eigen {
|
---|
| 16 |
|
---|
| 17 | template<typename Scalar, int Dim> AlignedBox<Scalar, Dim> bounding_box(const Matrix<Scalar, Dim, 1> &v) { return AlignedBox<Scalar, Dim>(v); }
|
---|
| 18 |
|
---|
| 19 | }
|
---|
| 20 |
|
---|
| 21 |
|
---|
| 22 | template<int Dim>
|
---|
| 23 | struct Ball
|
---|
| 24 | {
|
---|
| 25 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(double, Dim)
|
---|
| 26 |
|
---|
| 27 | typedef Matrix<double, Dim, 1> VectorType;
|
---|
| 28 |
|
---|
| 29 | Ball() {}
|
---|
| 30 | Ball(const VectorType &c, double r) : center(c), radius(r) {}
|
---|
| 31 |
|
---|
| 32 | VectorType center;
|
---|
| 33 | double radius;
|
---|
| 34 | };
|
---|
| 35 | template<int Dim> AlignedBox<double, Dim> bounding_box(const Ball<Dim> &b)
|
---|
| 36 | { return AlignedBox<double, Dim>(b.center.array() - b.radius, b.center.array() + b.radius); }
|
---|
| 37 |
|
---|
| 38 | inline double SQR(double x) { return x * x; }
|
---|
| 39 |
|
---|
| 40 | template<int Dim>
|
---|
| 41 | 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
|
---|
| 42 | {
|
---|
| 43 | typedef double Scalar;
|
---|
| 44 | typedef Matrix<double, Dim, 1> VectorType;
|
---|
| 45 | typedef Ball<Dim> BallType;
|
---|
| 46 | typedef AlignedBox<double, Dim> BoxType;
|
---|
| 47 |
|
---|
| 48 | BallPointStuff() : calls(0), count(0) {}
|
---|
| 49 | BallPointStuff(const VectorType &inP) : p(inP), calls(0), count(0) {}
|
---|
| 50 |
|
---|
| 51 |
|
---|
| 52 | bool intersectVolume(const BoxType &r) { ++calls; return r.contains(p); }
|
---|
| 53 | bool intersectObject(const BallType &b) {
|
---|
| 54 | ++calls;
|
---|
| 55 | if((b.center - p).squaredNorm() < SQR(b.radius))
|
---|
| 56 | ++count;
|
---|
| 57 | return false; //continue
|
---|
| 58 | }
|
---|
| 59 |
|
---|
| 60 | bool intersectVolumeVolume(const BoxType &r1, const BoxType &r2) { ++calls; return !(r1.intersection(r2)).isNull(); }
|
---|
| 61 | bool intersectVolumeObject(const BoxType &r, const BallType &b) { ++calls; return r.squaredExteriorDistance(b.center) < SQR(b.radius); }
|
---|
| 62 | bool intersectObjectVolume(const BallType &b, const BoxType &r) { ++calls; return r.squaredExteriorDistance(b.center) < SQR(b.radius); }
|
---|
| 63 | bool intersectObjectObject(const BallType &b1, const BallType &b2){
|
---|
| 64 | ++calls;
|
---|
| 65 | if((b1.center - b2.center).norm() < b1.radius + b2.radius)
|
---|
| 66 | ++count;
|
---|
| 67 | return false;
|
---|
| 68 | }
|
---|
| 69 | bool intersectVolumeObject(const BoxType &r, const VectorType &v) { ++calls; return r.contains(v); }
|
---|
| 70 | bool intersectObjectObject(const BallType &b, const VectorType &v){
|
---|
| 71 | ++calls;
|
---|
| 72 | if((b.center - v).squaredNorm() < SQR(b.radius))
|
---|
| 73 | ++count;
|
---|
| 74 | return false;
|
---|
| 75 | }
|
---|
| 76 |
|
---|
| 77 | double minimumOnVolume(const BoxType &r) { ++calls; return r.squaredExteriorDistance(p); }
|
---|
| 78 | double minimumOnObject(const BallType &b) { ++calls; return (std::max)(0., (b.center - p).squaredNorm() - SQR(b.radius)); }
|
---|
| 79 | double minimumOnVolumeVolume(const BoxType &r1, const BoxType &r2) { ++calls; return r1.squaredExteriorDistance(r2); }
|
---|
| 80 | double minimumOnVolumeObject(const BoxType &r, const BallType &b) { ++calls; return SQR((std::max)(0., r.exteriorDistance(b.center) - b.radius)); }
|
---|
| 81 | double minimumOnObjectVolume(const BallType &b, const BoxType &r) { ++calls; return SQR((std::max)(0., r.exteriorDistance(b.center) - b.radius)); }
|
---|
| 82 | double minimumOnObjectObject(const BallType &b1, const BallType &b2){ ++calls; return SQR((std::max)(0., (b1.center - b2.center).norm() - b1.radius - b2.radius)); }
|
---|
| 83 | double minimumOnVolumeObject(const BoxType &r, const VectorType &v) { ++calls; return r.squaredExteriorDistance(v); }
|
---|
| 84 | double minimumOnObjectObject(const BallType &b, const VectorType &v){ ++calls; return SQR((std::max)(0., (b.center - v).norm() - b.radius)); }
|
---|
| 85 |
|
---|
| 86 | VectorType p;
|
---|
| 87 | int calls;
|
---|
| 88 | int count;
|
---|
| 89 | };
|
---|
| 90 |
|
---|
| 91 |
|
---|
| 92 | template<int Dim>
|
---|
| 93 | struct TreeTest
|
---|
| 94 | {
|
---|
| 95 | typedef Matrix<double, Dim, 1> VectorType;
|
---|
| 96 | typedef std::vector<VectorType, aligned_allocator<VectorType> > VectorTypeList;
|
---|
| 97 | typedef Ball<Dim> BallType;
|
---|
| 98 | typedef std::vector<BallType, aligned_allocator<BallType> > BallTypeList;
|
---|
| 99 | typedef AlignedBox<double, Dim> BoxType;
|
---|
| 100 |
|
---|
| 101 | void testIntersect1()
|
---|
| 102 | {
|
---|
| 103 | BallTypeList b;
|
---|
| 104 | for(int i = 0; i < 500; ++i) {
|
---|
| 105 | b.push_back(BallType(VectorType::Random(), 0.5 * internal::random(0., 1.)));
|
---|
| 106 | }
|
---|
| 107 | KdBVH<double, Dim, BallType> tree(b.begin(), b.end());
|
---|
| 108 |
|
---|
| 109 | VectorType pt = VectorType::Random();
|
---|
| 110 | BallPointStuff<Dim> i1(pt), i2(pt);
|
---|
| 111 |
|
---|
| 112 | for(int i = 0; i < (int)b.size(); ++i)
|
---|
| 113 | i1.intersectObject(b[i]);
|
---|
| 114 |
|
---|
| 115 | BVIntersect(tree, i2);
|
---|
| 116 |
|
---|
| 117 | VERIFY(i1.count == i2.count);
|
---|
| 118 | }
|
---|
| 119 |
|
---|
| 120 | void testMinimize1()
|
---|
| 121 | {
|
---|
| 122 | BallTypeList b;
|
---|
| 123 | for(int i = 0; i < 500; ++i) {
|
---|
| 124 | b.push_back(BallType(VectorType::Random(), 0.01 * internal::random(0., 1.)));
|
---|
| 125 | }
|
---|
| 126 | KdBVH<double, Dim, BallType> tree(b.begin(), b.end());
|
---|
| 127 |
|
---|
| 128 | VectorType pt = VectorType::Random();
|
---|
| 129 | BallPointStuff<Dim> i1(pt), i2(pt);
|
---|
| 130 |
|
---|
| 131 | double m1 = (std::numeric_limits<double>::max)(), m2 = m1;
|
---|
| 132 |
|
---|
| 133 | for(int i = 0; i < (int)b.size(); ++i)
|
---|
| 134 | m1 = (std::min)(m1, i1.minimumOnObject(b[i]));
|
---|
| 135 |
|
---|
| 136 | m2 = BVMinimize(tree, i2);
|
---|
| 137 |
|
---|
| 138 | VERIFY_IS_APPROX(m1, m2);
|
---|
| 139 | }
|
---|
| 140 |
|
---|
| 141 | void testIntersect2()
|
---|
| 142 | {
|
---|
| 143 | BallTypeList b;
|
---|
| 144 | VectorTypeList v;
|
---|
| 145 |
|
---|
| 146 | for(int i = 0; i < 50; ++i) {
|
---|
| 147 | b.push_back(BallType(VectorType::Random(), 0.5 * internal::random(0., 1.)));
|
---|
| 148 | for(int j = 0; j < 3; ++j)
|
---|
| 149 | v.push_back(VectorType::Random());
|
---|
| 150 | }
|
---|
| 151 |
|
---|
| 152 | KdBVH<double, Dim, BallType> tree(b.begin(), b.end());
|
---|
| 153 | KdBVH<double, Dim, VectorType> vTree(v.begin(), v.end());
|
---|
| 154 |
|
---|
| 155 | BallPointStuff<Dim> i1, i2;
|
---|
| 156 |
|
---|
| 157 | for(int i = 0; i < (int)b.size(); ++i)
|
---|
| 158 | for(int j = 0; j < (int)v.size(); ++j)
|
---|
| 159 | i1.intersectObjectObject(b[i], v[j]);
|
---|
| 160 |
|
---|
| 161 | BVIntersect(tree, vTree, i2);
|
---|
| 162 |
|
---|
| 163 | VERIFY(i1.count == i2.count);
|
---|
| 164 | }
|
---|
| 165 |
|
---|
| 166 | void testMinimize2()
|
---|
| 167 | {
|
---|
| 168 | BallTypeList b;
|
---|
| 169 | VectorTypeList v;
|
---|
| 170 |
|
---|
| 171 | for(int i = 0; i < 50; ++i) {
|
---|
| 172 | b.push_back(BallType(VectorType::Random(), 1e-7 + 1e-6 * internal::random(0., 1.)));
|
---|
| 173 | for(int j = 0; j < 3; ++j)
|
---|
| 174 | v.push_back(VectorType::Random());
|
---|
| 175 | }
|
---|
| 176 |
|
---|
| 177 | KdBVH<double, Dim, BallType> tree(b.begin(), b.end());
|
---|
| 178 | KdBVH<double, Dim, VectorType> vTree(v.begin(), v.end());
|
---|
| 179 |
|
---|
| 180 | BallPointStuff<Dim> i1, i2;
|
---|
| 181 |
|
---|
| 182 | double m1 = (std::numeric_limits<double>::max)(), m2 = m1;
|
---|
| 183 |
|
---|
| 184 | for(int i = 0; i < (int)b.size(); ++i)
|
---|
| 185 | for(int j = 0; j < (int)v.size(); ++j)
|
---|
| 186 | m1 = (std::min)(m1, i1.minimumOnObjectObject(b[i], v[j]));
|
---|
| 187 |
|
---|
| 188 | m2 = BVMinimize(tree, vTree, i2);
|
---|
| 189 |
|
---|
| 190 | VERIFY_IS_APPROX(m1, m2);
|
---|
| 191 | }
|
---|
| 192 | };
|
---|
| 193 |
|
---|
| 194 |
|
---|
| 195 | void test_BVH()
|
---|
| 196 | {
|
---|
| 197 | for(int i = 0; i < g_repeat; i++) {
|
---|
| 198 | #ifdef EIGEN_TEST_PART_1
|
---|
| 199 | TreeTest<2> test2;
|
---|
| 200 | CALL_SUBTEST(test2.testIntersect1());
|
---|
| 201 | CALL_SUBTEST(test2.testMinimize1());
|
---|
| 202 | CALL_SUBTEST(test2.testIntersect2());
|
---|
| 203 | CALL_SUBTEST(test2.testMinimize2());
|
---|
| 204 | #endif
|
---|
| 205 |
|
---|
| 206 | #ifdef EIGEN_TEST_PART_2
|
---|
| 207 | TreeTest<3> test3;
|
---|
| 208 | CALL_SUBTEST(test3.testIntersect1());
|
---|
| 209 | CALL_SUBTEST(test3.testMinimize1());
|
---|
| 210 | CALL_SUBTEST(test3.testIntersect2());
|
---|
| 211 | CALL_SUBTEST(test3.testMinimize2());
|
---|
| 212 | #endif
|
---|
| 213 |
|
---|
| 214 | #ifdef EIGEN_TEST_PART_3
|
---|
| 215 | TreeTest<4> test4;
|
---|
| 216 | CALL_SUBTEST(test4.testIntersect1());
|
---|
| 217 | CALL_SUBTEST(test4.testMinimize1());
|
---|
| 218 | CALL_SUBTEST(test4.testIntersect2());
|
---|
| 219 | CALL_SUBTEST(test4.testMinimize2());
|
---|
| 220 | #endif
|
---|
| 221 | }
|
---|
| 222 | }
|
---|