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>
|
---|
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 | {
|
---|
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;
|
---|
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 | {
|
---|
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;
|
---|
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>
|
---|
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>
|
---|
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>
|
---|
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;
|
---|
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;
|
---|
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;
|
---|
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;
|
---|
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>
|
---|
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>
|
---|
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
|
---|