BVAlgorithms.h
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 // Eigen is free software; you can redistribute it and/or
7 // modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation; either
9 // version 3 of the License, or (at your option) any later version.
10 //
11 // Alternatively, you can redistribute it and/or
12 // modify it under the terms of the GNU General Public License as
13 // published by the Free Software Foundation; either version 2 of
14 // the License, or (at your option) any later version.
15 //
16 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
17 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
18 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
19 // GNU General Public License for more details.
20 //
21 // You should have received a copy of the GNU Lesser General Public
22 // License and a copy of the GNU General Public License along with
23 // Eigen. If not, see <http://www.gnu.org/licenses/>.
24 
25 #ifndef EIGEN_BVALGORITHMS_H
26 #define EIGEN_BVALGORITHMS_H
27 
28 namespace Eigen {
29 
30 namespace internal {
31 
32 #ifndef EIGEN_PARSED_BY_DOXYGEN
33 template<typename BVH, typename Intersector>
34 bool intersect_helper(const BVH &tree, Intersector &intersector, typename BVH::Index root)
35 {
36  typedef typename BVH::Index Index;
37  typedef typename BVH::VolumeIterator VolIter;
38  typedef typename BVH::ObjectIterator ObjIter;
39 
40  VolIter vBegin = VolIter(), vEnd = VolIter();
41  ObjIter oBegin = ObjIter(), oEnd = ObjIter();
42 
43  std::vector<Index> todo(1, root);
44 
45  while(!todo.empty()) {
46  tree.getChildren(todo.back(), vBegin, vEnd, oBegin, oEnd);
47  todo.pop_back();
48 
49  for(; vBegin != vEnd; ++vBegin) //go through child volumes
50  if(intersector.intersectVolume(tree.getVolume(*vBegin)))
51  todo.push_back(*vBegin);
52 
53  for(; oBegin != oEnd; ++oBegin) //go through child objects
54  if(intersector.intersectObject(*oBegin))
55  return true; //intersector said to stop query
56  }
57  return false;
58 }
59 #endif //not EIGEN_PARSED_BY_DOXYGEN
60 
61 template<typename Volume1, typename Object1, typename Object2, typename Intersector>
62 struct intersector_helper1
63 {
64  intersector_helper1(const Object2 &inStored, Intersector &in) : stored(inStored), intersector(in) {}
65  bool intersectVolume(const Volume1 &vol) { return intersector.intersectVolumeObject(vol, stored); }
66  bool intersectObject(const Object1 &obj) { return intersector.intersectObjectObject(obj, stored); }
67  Object2 stored;
68  Intersector &intersector;
69 private:
70  intersector_helper1& operator=(const intersector_helper1&);
71 };
72 
73 template<typename Volume2, typename Object2, typename Object1, typename Intersector>
74 struct intersector_helper2
75 {
76  intersector_helper2(const Object1 &inStored, Intersector &in) : stored(inStored), intersector(in) {}
77  bool intersectVolume(const Volume2 &vol) { return intersector.intersectObjectVolume(stored, vol); }
78  bool intersectObject(const Object2 &obj) { return intersector.intersectObjectObject(stored, obj); }
79  Object1 stored;
80  Intersector &intersector;
81 private:
82  intersector_helper2& operator=(const intersector_helper2&);
83 };
84 
85 } // end namespace internal
86 
93 template<typename BVH, typename Intersector>
94 void BVIntersect(const BVH &tree, Intersector &intersector)
95 {
96  internal::intersect_helper(tree, intersector, tree.getRootIndex());
97 }
98 
107 template<typename BVH1, typename BVH2, typename Intersector>
108 void BVIntersect(const BVH1 &tree1, const BVH2 &tree2, Intersector &intersector) //TODO: tandem descent when it makes sense
109 {
110  typedef typename BVH1::Index Index1;
111  typedef typename BVH2::Index Index2;
112  typedef internal::intersector_helper1<typename BVH1::Volume, typename BVH1::Object, typename BVH2::Object, Intersector> Helper1;
113  typedef internal::intersector_helper2<typename BVH2::Volume, typename BVH2::Object, typename BVH1::Object, Intersector> Helper2;
114  typedef typename BVH1::VolumeIterator VolIter1;
115  typedef typename BVH1::ObjectIterator ObjIter1;
116  typedef typename BVH2::VolumeIterator VolIter2;
117  typedef typename BVH2::ObjectIterator ObjIter2;
118 
119  VolIter1 vBegin1 = VolIter1(), vEnd1 = VolIter1();
120  ObjIter1 oBegin1 = ObjIter1(), oEnd1 = ObjIter1();
121  VolIter2 vBegin2 = VolIter2(), vEnd2 = VolIter2(), vCur2 = VolIter2();
122  ObjIter2 oBegin2 = ObjIter2(), oEnd2 = ObjIter2(), oCur2 = ObjIter2();
123 
124  std::vector<std::pair<Index1, Index2> > todo(1, std::make_pair(tree1.getRootIndex(), tree2.getRootIndex()));
125 
126  while(!todo.empty()) {
127  tree1.getChildren(todo.back().first, vBegin1, vEnd1, oBegin1, oEnd1);
128  tree2.getChildren(todo.back().second, vBegin2, vEnd2, oBegin2, oEnd2);
129  todo.pop_back();
130 
131  for(; vBegin1 != vEnd1; ++vBegin1) { //go through child volumes of first tree
132  const typename BVH1::Volume &vol1 = tree1.getVolume(*vBegin1);
133  for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree
134  if(intersector.intersectVolumeVolume(vol1, tree2.getVolume(*vCur2)))
135  todo.push_back(std::make_pair(*vBegin1, *vCur2));
136  }
137 
138  for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree
139  Helper1 helper(*oCur2, intersector);
140  if(internal::intersect_helper(tree1, helper, *vBegin1))
141  return; //intersector said to stop query
142  }
143  }
144 
145  for(; oBegin1 != oEnd1; ++oBegin1) { //go through child objects of first tree
146  for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree
147  Helper2 helper(*oBegin1, intersector);
148  if(internal::intersect_helper(tree2, helper, *vCur2))
149  return; //intersector said to stop query
150  }
151 
152  for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree
153  if(intersector.intersectObjectObject(*oBegin1, *oCur2))
154  return; //intersector said to stop query
155  }
156  }
157  }
158 }
159 
160 namespace internal {
161 
162 #ifndef EIGEN_PARSED_BY_DOXYGEN
163 template<typename BVH, typename Minimizer>
164 typename Minimizer::Scalar minimize_helper(const BVH &tree, Minimizer &minimizer, typename BVH::Index root, typename Minimizer::Scalar minimum)
165 {
166  typedef typename Minimizer::Scalar Scalar;
167  typedef typename BVH::Index Index;
168  typedef std::pair<Scalar, Index> QueueElement; //first element is priority
169  typedef typename BVH::VolumeIterator VolIter;
170  typedef typename BVH::ObjectIterator ObjIter;
171 
172  VolIter vBegin = VolIter(), vEnd = VolIter();
173  ObjIter oBegin = ObjIter(), oEnd = ObjIter();
174  std::priority_queue<QueueElement, std::vector<QueueElement>, std::greater<QueueElement> > todo; //smallest is at the top
175 
176  todo.push(std::make_pair(Scalar(), root));
177 
178  while(!todo.empty()) {
179  tree.getChildren(todo.top().second, vBegin, vEnd, oBegin, oEnd);
180  todo.pop();
181 
182  for(; oBegin != oEnd; ++oBegin) //go through child objects
183  minimum = (std::min)(minimum, minimizer.minimumOnObject(*oBegin));
184 
185  for(; vBegin != vEnd; ++vBegin) { //go through child volumes
186  Scalar val = minimizer.minimumOnVolume(tree.getVolume(*vBegin));
187  if(val < minimum)
188  todo.push(std::make_pair(val, *vBegin));
189  }
190  }
191 
192  return minimum;
193 }
194 #endif //not EIGEN_PARSED_BY_DOXYGEN
195 
196 
197 template<typename Volume1, typename Object1, typename Object2, typename Minimizer>
198 struct minimizer_helper1
199 {
200  typedef typename Minimizer::Scalar Scalar;
201  minimizer_helper1(const Object2 &inStored, Minimizer &m) : stored(inStored), minimizer(m) {}
202  Scalar minimumOnVolume(const Volume1 &vol) { return minimizer.minimumOnVolumeObject(vol, stored); }
203  Scalar minimumOnObject(const Object1 &obj) { return minimizer.minimumOnObjectObject(obj, stored); }
204  Object2 stored;
205  Minimizer &minimizer;
206 private:
207  minimizer_helper1& operator=(const minimizer_helper1&) {}
208 };
209 
210 template<typename Volume2, typename Object2, typename Object1, typename Minimizer>
211 struct minimizer_helper2
212 {
213  typedef typename Minimizer::Scalar Scalar;
214  minimizer_helper2(const Object1 &inStored, Minimizer &m) : stored(inStored), minimizer(m) {}
215  Scalar minimumOnVolume(const Volume2 &vol) { return minimizer.minimumOnObjectVolume(stored, vol); }
216  Scalar minimumOnObject(const Object2 &obj) { return minimizer.minimumOnObjectObject(stored, obj); }
217  Object1 stored;
218  Minimizer &minimizer;
219 private:
220  minimizer_helper2& operator=(const minimizer_helper2&);
221 };
222 
223 } // end namespace internal
224 
233 template<typename BVH, typename Minimizer>
234 typename Minimizer::Scalar BVMinimize(const BVH &tree, Minimizer &minimizer)
235 {
236  return internal::minimize_helper(tree, minimizer, tree.getRootIndex(), (std::numeric_limits<typename Minimizer::Scalar>::max)());
237 }
238 
249 template<typename BVH1, typename BVH2, typename Minimizer>
250 typename Minimizer::Scalar BVMinimize(const BVH1 &tree1, const BVH2 &tree2, Minimizer &minimizer)
251 {
252  typedef typename Minimizer::Scalar Scalar;
253  typedef typename BVH1::Index Index1;
254  typedef typename BVH2::Index Index2;
255  typedef internal::minimizer_helper1<typename BVH1::Volume, typename BVH1::Object, typename BVH2::Object, Minimizer> Helper1;
256  typedef internal::minimizer_helper2<typename BVH2::Volume, typename BVH2::Object, typename BVH1::Object, Minimizer> Helper2;
257  typedef std::pair<Scalar, std::pair<Index1, Index2> > QueueElement; //first element is priority
258  typedef typename BVH1::VolumeIterator VolIter1;
259  typedef typename BVH1::ObjectIterator ObjIter1;
260  typedef typename BVH2::VolumeIterator VolIter2;
261  typedef typename BVH2::ObjectIterator ObjIter2;
262 
263  VolIter1 vBegin1 = VolIter1(), vEnd1 = VolIter1();
264  ObjIter1 oBegin1 = ObjIter1(), oEnd1 = ObjIter1();
265  VolIter2 vBegin2 = VolIter2(), vEnd2 = VolIter2(), vCur2 = VolIter2();
266  ObjIter2 oBegin2 = ObjIter2(), oEnd2 = ObjIter2(), oCur2 = ObjIter2();
267  std::priority_queue<QueueElement, std::vector<QueueElement>, std::greater<QueueElement> > todo; //smallest is at the top
268 
269  Scalar minimum = (std::numeric_limits<Scalar>::max)();
270  todo.push(std::make_pair(Scalar(), std::make_pair(tree1.getRootIndex(), tree2.getRootIndex())));
271 
272  while(!todo.empty()) {
273  tree1.getChildren(todo.top().second.first, vBegin1, vEnd1, oBegin1, oEnd1);
274  tree2.getChildren(todo.top().second.second, vBegin2, vEnd2, oBegin2, oEnd2);
275  todo.pop();
276 
277  for(; oBegin1 != oEnd1; ++oBegin1) { //go through child objects of first tree
278  for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree
279  minimum = (std::min)(minimum, minimizer.minimumOnObjectObject(*oBegin1, *oCur2));
280  }
281 
282  for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree
283  Helper2 helper(*oBegin1, minimizer);
284  minimum = (std::min)(minimum, internal::minimize_helper(tree2, helper, *vCur2, minimum));
285  }
286  }
287 
288  for(; vBegin1 != vEnd1; ++vBegin1) { //go through child volumes of first tree
289  const typename BVH1::Volume &vol1 = tree1.getVolume(*vBegin1);
290 
291  for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree
292  Helper1 helper(*oCur2, minimizer);
293  minimum = (std::min)(minimum, internal::minimize_helper(tree1, helper, *vBegin1, minimum));
294  }
295 
296  for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree
297  Scalar val = minimizer.minimumOnVolumeVolume(vol1, tree2.getVolume(*vCur2));
298  if(val < minimum)
299  todo.push(std::make_pair(val, std::make_pair(*vBegin1, *vCur2)));
300  }
301  }
302  }
303  return minimum;
304 }
305 
306 } // end namespace Eigen
307 
308 #endif // EIGEN_BVALGORITHMS_H