BVAlgorithms.h
Go to the documentation of this file.
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>
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:
56 };
57 
58 template<typename Volume2, typename Object2, typename Object1, typename Intersector>
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:
68 };
69 
70 } // end namespace internal
71 
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 
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;
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>
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:
193 };
194 
195 template<typename Volume2, typename Object2, typename Object1, typename Minimizer>
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:
206 };
207 
208 } // end namespace internal
209 
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 
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;
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
Scalar minimumOnObject(const Object1 &obj)
Definition: BVAlgorithms.h:188
Minimizer::Scalar BVMinimize(const BVH &tree, Minimizer &minimizer)
Definition: BVAlgorithms.h:219
iterative scaling algorithm to equilibrate rows and column norms in matrices
Definition: matrix.hpp:471
bool intersectObject(const Object1 &obj)
Definition: BVAlgorithms.h:51
bool intersectVolume(const Volume1 &vol)
Definition: BVAlgorithms.h:50
Scalar minimumOnVolume(const Volume2 &vol)
Definition: BVAlgorithms.h:200
bool intersect_helper(const BVH &tree, Intersector &intersector, typename BVH::Index root)
Definition: BVAlgorithms.h:19
Scalar minimumOnVolume(const Volume1 &vol)
Definition: BVAlgorithms.h:187
minimizer_helper2(const Object1 &inStored, Minimizer &m)
Definition: BVAlgorithms.h:199
intersector_helper1(const Object2 &inStored, Intersector &in)
Definition: BVAlgorithms.h:49
Scalar minimumOnObject(const Object2 &obj)
Definition: BVAlgorithms.h:201
intersector_helper1 & operator=(const intersector_helper1 &)
bool intersectObject(const Object2 &obj)
Definition: BVAlgorithms.h:63
minimizer_helper1(const Object2 &inStored, Minimizer &m)
Definition: BVAlgorithms.h:186
intersector_helper2(const Object1 &inStored, Intersector &in)
Definition: BVAlgorithms.h:61
void BVIntersect(const BVH &tree, Intersector &intersector)
Definition: BVAlgorithms.h:79
bool intersectVolume(const Volume2 &vol)
Definition: BVAlgorithms.h:62
Minimizer::Scalar minimize_helper(const BVH &tree, Minimizer &minimizer, typename BVH::Index root, typename Minimizer::Scalar minimum)
Definition: BVAlgorithms.h:149


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:34:29