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#ifndef EIGEN_BVALGORITHMS_H
11#define EIGEN_BVALGORITHMS_H
12
13namespace Eigen {
14
15namespace internal {
16
17#ifndef EIGEN_PARSED_BY_DOXYGEN
18template<typename BVH, typename Intersector>
19bool intersect_helper(const BVH &tree, Intersector &intersector, typename BVH::Index root)
20{
21  typedef typename BVH::Index Index;
22  typedef typename BVH::VolumeIterator VolIter;
23  typedef typename BVH::ObjectIterator ObjIter;
24
25  VolIter vBegin = VolIter(), vEnd = VolIter();
26  ObjIter oBegin = ObjIter(), oEnd = ObjIter();
27
28  std::vector<Index> todo(1, root);
29
30  while(!todo.empty()) {
31    tree.getChildren(todo.back(), vBegin, vEnd, oBegin, oEnd);
32    todo.pop_back();
33
34    for(; vBegin != vEnd; ++vBegin) //go through child volumes
35      if(intersector.intersectVolume(tree.getVolume(*vBegin)))
36        todo.push_back(*vBegin);
37
38    for(; oBegin != oEnd; ++oBegin) //go through child objects
39      if(intersector.intersectObject(*oBegin))
40        return true; //intersector said to stop query
41  }
42  return false;
43}
44#endif //not EIGEN_PARSED_BY_DOXYGEN
45
46template<typename Volume1, typename Object1, typename Object2, typename Intersector>
47struct intersector_helper1
48{
49  intersector_helper1(const Object2 &inStored, Intersector &in) : stored(inStored), intersector(in) {}
50  bool intersectVolume(const Volume1 &vol) { return intersector.intersectVolumeObject(vol, stored); }
51  bool intersectObject(const Object1 &obj) { return intersector.intersectObjectObject(obj, stored); }
52  Object2 stored;
53  Intersector &intersector;
54private:
55  intersector_helper1& operator=(const intersector_helper1&);
56};
57
58template<typename Volume2, typename Object2, typename Object1, typename Intersector>
59struct intersector_helper2
60{
61  intersector_helper2(const Object1 &inStored, Intersector &in) : stored(inStored), intersector(in) {}
62  bool intersectVolume(const Volume2 &vol) { return intersector.intersectObjectVolume(stored, vol); }
63  bool intersectObject(const Object2 &obj) { return intersector.intersectObjectObject(stored, obj); }
64  Object1 stored;
65  Intersector &intersector;
66private:
67  intersector_helper2& operator=(const intersector_helper2&);
68};
69
70} // end namespace internal
71
72/**  Given a BVH, runs the query encapsulated by \a intersector.
73  *  The Intersector type must provide the following members: \code
74     bool intersectVolume(const BVH::Volume &volume) //returns true if volume intersects the query
75     bool intersectObject(const BVH::Object &object) //returns true if the search should terminate immediately
76  \endcode
77  */
78template<typename BVH, typename Intersector>
79void BVIntersect(const BVH &tree, Intersector &intersector)
80{
81  internal::intersect_helper(tree, intersector, tree.getRootIndex());
82}
83
84/**  Given two BVH's, runs the query on their Cartesian product encapsulated by \a intersector.
85  *  The Intersector type must provide the following members: \code
86     bool intersectVolumeVolume(const BVH1::Volume &v1, const BVH2::Volume &v2) //returns true if product of volumes intersects the query
87     bool intersectVolumeObject(const BVH1::Volume &v1, const BVH2::Object &o2) //returns true if the volume-object product intersects the query
88     bool intersectObjectVolume(const BVH1::Object &o1, const BVH2::Volume &v2) //returns true if the volume-object product intersects the query
89     bool intersectObjectObject(const BVH1::Object &o1, const BVH2::Object &o2) //returns true if the search should terminate immediately
90  \endcode
91  */
92template<typename BVH1, typename BVH2, typename Intersector>
93void BVIntersect(const BVH1 &tree1, const BVH2 &tree2, Intersector &intersector) //TODO: tandem descent when it makes sense
94{
95  typedef typename BVH1::Index Index1;
96  typedef typename BVH2::Index Index2;
97  typedef internal::intersector_helper1<typename BVH1::Volume, typename BVH1::Object, typename BVH2::Object, Intersector> Helper1;
98  typedef internal::intersector_helper2<typename BVH2::Volume, typename BVH2::Object, typename BVH1::Object, Intersector> Helper2;
99  typedef typename BVH1::VolumeIterator VolIter1;
100  typedef typename BVH1::ObjectIterator ObjIter1;
101  typedef typename BVH2::VolumeIterator VolIter2;
102  typedef typename BVH2::ObjectIterator ObjIter2;
103
104  VolIter1 vBegin1 = VolIter1(), vEnd1 = VolIter1();
105  ObjIter1 oBegin1 = ObjIter1(), oEnd1 = ObjIter1();
106  VolIter2 vBegin2 = VolIter2(), vEnd2 = VolIter2(), vCur2 = VolIter2();
107  ObjIter2 oBegin2 = ObjIter2(), oEnd2 = ObjIter2(), oCur2 = ObjIter2();
108
109  std::vector<std::pair<Index1, Index2> > todo(1, std::make_pair(tree1.getRootIndex(), tree2.getRootIndex()));
110
111  while(!todo.empty()) {
112    tree1.getChildren(todo.back().first, vBegin1, vEnd1, oBegin1, oEnd1);
113    tree2.getChildren(todo.back().second, vBegin2, vEnd2, oBegin2, oEnd2);
114    todo.pop_back();
115
116    for(; vBegin1 != vEnd1; ++vBegin1) { //go through child volumes of first tree
117      const typename BVH1::Volume &vol1 = tree1.getVolume(*vBegin1);
118      for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree
119        if(intersector.intersectVolumeVolume(vol1, tree2.getVolume(*vCur2)))
120          todo.push_back(std::make_pair(*vBegin1, *vCur2));
121      }
122
123      for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree
124        Helper1 helper(*oCur2, intersector);
125        if(internal::intersect_helper(tree1, helper, *vBegin1))
126          return; //intersector said to stop query
127      }
128    }
129
130    for(; oBegin1 != oEnd1; ++oBegin1) { //go through child objects of first tree
131      for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree
132        Helper2 helper(*oBegin1, intersector);
133        if(internal::intersect_helper(tree2, helper, *vCur2))
134          return; //intersector said to stop query
135      }
136
137      for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree
138        if(intersector.intersectObjectObject(*oBegin1, *oCur2))
139          return; //intersector said to stop query
140      }
141    }
142  }
143}
144
145namespace internal {
146
147#ifndef EIGEN_PARSED_BY_DOXYGEN
148template<typename BVH, typename Minimizer>
149typename Minimizer::Scalar minimize_helper(const BVH &tree, Minimizer &minimizer, typename BVH::Index root, typename Minimizer::Scalar minimum)
150{
151  typedef typename Minimizer::Scalar Scalar;
152  typedef typename BVH::Index Index;
153  typedef std::pair<Scalar, Index> QueueElement; //first element is priority
154  typedef typename BVH::VolumeIterator VolIter;
155  typedef typename BVH::ObjectIterator ObjIter;
156
157  VolIter vBegin = VolIter(), vEnd = VolIter();
158  ObjIter oBegin = ObjIter(), oEnd = ObjIter();
159  std::priority_queue<QueueElement, std::vector<QueueElement>, std::greater<QueueElement> > todo; //smallest is at the top
160
161  todo.push(std::make_pair(Scalar(), root));
162
163  while(!todo.empty()) {
164    tree.getChildren(todo.top().second, vBegin, vEnd, oBegin, oEnd);
165    todo.pop();
166
167    for(; oBegin != oEnd; ++oBegin) //go through child objects
168      minimum = (std::min)(minimum, minimizer.minimumOnObject(*oBegin));
169
170    for(; vBegin != vEnd; ++vBegin) { //go through child volumes
171      Scalar val = minimizer.minimumOnVolume(tree.getVolume(*vBegin));
172      if(val < minimum)
173        todo.push(std::make_pair(val, *vBegin));
174    }
175  }
176
177  return minimum;
178}
179#endif //not EIGEN_PARSED_BY_DOXYGEN
180
181
182template<typename Volume1, typename Object1, typename Object2, typename Minimizer>
183struct minimizer_helper1
184{
185  typedef typename Minimizer::Scalar Scalar;
186  minimizer_helper1(const Object2 &inStored, Minimizer &m) : stored(inStored), minimizer(m) {}
187  Scalar minimumOnVolume(const Volume1 &vol) { return minimizer.minimumOnVolumeObject(vol, stored); }
188  Scalar minimumOnObject(const Object1 &obj) { return minimizer.minimumOnObjectObject(obj, stored); }
189  Object2 stored;
190  Minimizer &minimizer;
191private:
192  minimizer_helper1& operator=(const minimizer_helper1&);
193};
194
195template<typename Volume2, typename Object2, typename Object1, typename Minimizer>
196struct minimizer_helper2
197{
198  typedef typename Minimizer::Scalar Scalar;
199  minimizer_helper2(const Object1 &inStored, Minimizer &m) : stored(inStored), minimizer(m) {}
200  Scalar minimumOnVolume(const Volume2 &vol) { return minimizer.minimumOnObjectVolume(stored, vol); }
201  Scalar minimumOnObject(const Object2 &obj) { return minimizer.minimumOnObjectObject(stored, obj); }
202  Object1 stored;
203  Minimizer &minimizer;
204private:
205  minimizer_helper2& operator=(const minimizer_helper2&);
206};
207
208} // end namespace internal
209
210/**  Given a BVH, runs the query encapsulated by \a minimizer.
211  *  \returns the minimum value.
212  *  The Minimizer type must provide the following members: \code
213     typedef Scalar //the numeric type of what is being minimized--not necessarily the Scalar type of the BVH (if it has one)
214     Scalar minimumOnVolume(const BVH::Volume &volume)
215     Scalar minimumOnObject(const BVH::Object &object)
216  \endcode
217  */
218template<typename BVH, typename Minimizer>
219typename Minimizer::Scalar BVMinimize(const BVH &tree, Minimizer &minimizer)
220{
221  return internal::minimize_helper(tree, minimizer, tree.getRootIndex(), (std::numeric_limits<typename Minimizer::Scalar>::max)());
222}
223
224/**  Given two BVH's, runs the query on their cartesian product encapsulated by \a minimizer.
225  *  \returns the minimum value.
226  *  The Minimizer type must provide the following members: \code
227     typedef Scalar //the numeric type of what is being minimized--not necessarily the Scalar type of the BVH (if it has one)
228     Scalar minimumOnVolumeVolume(const BVH1::Volume &v1, const BVH2::Volume &v2)
229     Scalar minimumOnVolumeObject(const BVH1::Volume &v1, const BVH2::Object &o2)
230     Scalar minimumOnObjectVolume(const BVH1::Object &o1, const BVH2::Volume &v2)
231     Scalar minimumOnObjectObject(const BVH1::Object &o1, const BVH2::Object &o2)
232  \endcode
233  */
234template<typename BVH1, typename BVH2, typename Minimizer>
235typename Minimizer::Scalar BVMinimize(const BVH1 &tree1, const BVH2 &tree2, Minimizer &minimizer)
236{
237  typedef typename Minimizer::Scalar Scalar;
238  typedef typename BVH1::Index Index1;
239  typedef typename BVH2::Index Index2;
240  typedef internal::minimizer_helper1<typename BVH1::Volume, typename BVH1::Object, typename BVH2::Object, Minimizer> Helper1;
241  typedef internal::minimizer_helper2<typename BVH2::Volume, typename BVH2::Object, typename BVH1::Object, Minimizer> Helper2;
242  typedef std::pair<Scalar, std::pair<Index1, Index2> > QueueElement; //first element is priority
243  typedef typename BVH1::VolumeIterator VolIter1;
244  typedef typename BVH1::ObjectIterator ObjIter1;
245  typedef typename BVH2::VolumeIterator VolIter2;
246  typedef typename BVH2::ObjectIterator ObjIter2;
247
248  VolIter1 vBegin1 = VolIter1(), vEnd1 = VolIter1();
249  ObjIter1 oBegin1 = ObjIter1(), oEnd1 = ObjIter1();
250  VolIter2 vBegin2 = VolIter2(), vEnd2 = VolIter2(), vCur2 = VolIter2();
251  ObjIter2 oBegin2 = ObjIter2(), oEnd2 = ObjIter2(), oCur2 = ObjIter2();
252  std::priority_queue<QueueElement, std::vector<QueueElement>, std::greater<QueueElement> > todo; //smallest is at the top
253
254  Scalar minimum = (std::numeric_limits<Scalar>::max)();
255  todo.push(std::make_pair(Scalar(), std::make_pair(tree1.getRootIndex(), tree2.getRootIndex())));
256
257  while(!todo.empty()) {
258    tree1.getChildren(todo.top().second.first, vBegin1, vEnd1, oBegin1, oEnd1);
259    tree2.getChildren(todo.top().second.second, vBegin2, vEnd2, oBegin2, oEnd2);
260    todo.pop();
261
262    for(; oBegin1 != oEnd1; ++oBegin1) { //go through child objects of first tree
263      for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree
264        minimum = (std::min)(minimum, minimizer.minimumOnObjectObject(*oBegin1, *oCur2));
265      }
266
267      for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree
268        Helper2 helper(*oBegin1, minimizer);
269        minimum = (std::min)(minimum, internal::minimize_helper(tree2, helper, *vCur2, minimum));
270      }
271    }
272
273    for(; vBegin1 != vEnd1; ++vBegin1) { //go through child volumes of first tree
274      const typename BVH1::Volume &vol1 = tree1.getVolume(*vBegin1);
275
276      for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree
277        Helper1 helper(*oCur2, minimizer);
278        minimum = (std::min)(minimum, internal::minimize_helper(tree1, helper, *vBegin1, minimum));
279      }
280
281      for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree
282        Scalar val = minimizer.minimumOnVolumeVolume(vol1, tree2.getVolume(*vCur2));
283        if(val < minimum)
284          todo.push(std::make_pair(val, std::make_pair(*vBegin1, *vCur2)));
285      }
286    }
287  }
288  return minimum;
289}
290
291} // end namespace Eigen
292
293#endif // EIGEN_BVALGORITHMS_H
294