1 // This file is part of Eigen, a lightweight C++ template library 2 // for linear algebra. 3 // 4 // Copyright (C) 2009 Ilya Baran <ibaran (at) 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 } 223
