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
13 namespace Eigen {
14
15 namespace internal {
16
17 #ifndef EIGEN_PARSED_BY_DOXYGEN
18 template<typename BVH, typename Intersector>
intersect_helper(const BVH & tree,Intersector & intersector,typename BVH::Index root)19 bool 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
46 template<typename Volume1, typename Object1, typename Object2, typename Intersector>
47 struct intersector_helper1
48 {
intersector_helper1intersector_helper149 intersector_helper1(const Object2 &inStored, Intersector &in) : stored(inStored), intersector(in) {}
intersectVolumeintersector_helper150 bool intersectVolume(const Volume1 &vol) { return intersector.intersectVolumeObject(vol, stored); }
intersectObjectintersector_helper151 bool intersectObject(const Object1 &obj) { return intersector.intersectObjectObject(obj, stored); }
52 Object2 stored;
53 Intersector &intersector;
54 private:
55 intersector_helper1& operator=(const intersector_helper1&);
56 };
57
58 template<typename Volume2, typename Object2, typename Object1, typename Intersector>
59 struct intersector_helper2
60 {
intersector_helper2intersector_helper261 intersector_helper2(const Object1 &inStored, Intersector &in) : stored(inStored), intersector(in) {}
intersectVolumeintersector_helper262 bool intersectVolume(const Volume2 &vol) { return intersector.intersectObjectVolume(stored, vol); }
intersectObjectintersector_helper263 bool intersectObject(const Object2 &obj) { return intersector.intersectObjectObject(stored, obj); }
64 Object1 stored;
65 Intersector &intersector;
66 private:
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 */
78 template<typename BVH, typename Intersector>
BVIntersect(const BVH & tree,Intersector & intersector)79 void 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 */
92 template<typename BVH1, typename BVH2, typename Intersector>
BVIntersect(const BVH1 & tree1,const BVH2 & tree2,Intersector & intersector)93 void 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
145 namespace internal {
146
147 #ifndef EIGEN_PARSED_BY_DOXYGEN
148 template<typename BVH, typename Minimizer>
minimize_helper(const BVH & tree,Minimizer & minimizer,typename BVH::Index root,typename Minimizer::Scalar minimum)149 typename 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
182 template<typename Volume1, typename Object1, typename Object2, typename Minimizer>
183 struct minimizer_helper1
184 {
185 typedef typename Minimizer::Scalar Scalar;
minimizer_helper1minimizer_helper1186 minimizer_helper1(const Object2 &inStored, Minimizer &m) : stored(inStored), minimizer(m) {}
minimumOnVolumeminimizer_helper1187 Scalar minimumOnVolume(const Volume1 &vol) { return minimizer.minimumOnVolumeObject(vol, stored); }
minimumOnObjectminimizer_helper1188 Scalar minimumOnObject(const Object1 &obj) { return minimizer.minimumOnObjectObject(obj, stored); }
189 Object2 stored;
190 Minimizer &minimizer;
191 private:
192 minimizer_helper1& operator=(const minimizer_helper1&);
193 };
194
195 template<typename Volume2, typename Object2, typename Object1, typename Minimizer>
196 struct minimizer_helper2
197 {
198 typedef typename Minimizer::Scalar Scalar;
minimizer_helper2minimizer_helper2199 minimizer_helper2(const Object1 &inStored, Minimizer &m) : stored(inStored), minimizer(m) {}
minimumOnVolumeminimizer_helper2200 Scalar minimumOnVolume(const Volume2 &vol) { return minimizer.minimumOnObjectVolume(stored, vol); }
minimumOnObjectminimizer_helper2201 Scalar minimumOnObject(const Object2 &obj) { return minimizer.minimumOnObjectObject(stored, obj); }
202 Object1 stored;
203 Minimizer &minimizer;
204 private:
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 */
218 template<typename BVH, typename Minimizer>
BVMinimize(const BVH & tree,Minimizer & minimizer)219 typename 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 */
234 template<typename BVH1, typename BVH2, typename Minimizer>
BVMinimize(const BVH1 & tree1,const BVH2 & tree2,Minimizer & minimizer)235 typename 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