47 FCL_REAL sqrDistLowerBound1 = 0, sqrDistLowerBound2 = 0;
48 bool l1 = node->isFirstNodeLeaf(b1);
49 bool l2 = node->isSecondNodeLeaf(b2);
54 node->leafCollides(b1, b2, sqrDistLowerBound);
58 if (node->BVDisjoints(b1, b2, sqrDistLowerBound)) {
62 if (node->firstOverSecond(b1, b2)) {
63 unsigned int c1 = (
unsigned int)node->getFirstLeftChild(b1);
64 unsigned int c2 = (
unsigned int)node->getFirstRightChild(b1);
69 if (node->canStop() && !front_list)
return;
72 sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2);
74 unsigned int c1 = (
unsigned int)node->getSecondLeftChild(b2);
75 unsigned int c2 = (
unsigned int)node->getSecondRightChild(b2);
80 if (node->canStop() && !front_list)
return;
83 sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2);
90 typedef std::pair<unsigned int, unsigned int> BVPair_t;
92 typedef std::vector<BVPair_t> Stack_t;
96 sqrDistLowerBound = std::numeric_limits<FCL_REAL>::infinity();
97 FCL_REAL sdlb = std::numeric_limits<FCL_REAL>::infinity();
99 pairs.push_back(BVPair_t(0, 0));
101 while (!pairs.empty()) {
102 unsigned int a = pairs.back().first,
b = pairs.back().second;
105 bool la = node->isFirstNodeLeaf(
a);
106 bool lb = node->isSecondNodeLeaf(
b);
117 node->leafCollides(
a,
b, sdlb);
118 if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb;
119 if (node->canStop() && !front_list)
return;
129 if (node->BVDisjoints(
a,
b, sdlb)) {
130 if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb;
135 if (node->firstOverSecond(
a,
b)) {
136 unsigned int c1 = (
unsigned int)node->getFirstLeftChild(
a);
137 unsigned int c2 = (
unsigned int)node->getFirstRightChild(
a);
138 pairs.push_back(BVPair_t(
c2,
b));
139 pairs.push_back(BVPair_t(c1,
b));
141 unsigned int c1 = (
unsigned int)node->getSecondLeftChild(
b);
142 unsigned int c2 = (
unsigned int)node->getSecondRightChild(
b);
143 pairs.push_back(BVPair_t(
a,
c2));
144 pairs.push_back(BVPair_t(
a, c1));
155 bool l1 = node->isFirstNodeLeaf(b1);
156 bool l2 = node->isSecondNodeLeaf(b2);
161 node->leafComputeDistance(b1, b2);
165 unsigned int a1, a2, c1,
c2;
167 if (node->firstOverSecond(b1, b2)) {
168 a1 = (
unsigned int)node->getFirstLeftChild(b1);
170 c1 = (
unsigned int)node->getFirstRightChild(b1);
174 a2 = (
unsigned int)node->getSecondLeftChild(b2);
176 c2 = (
unsigned int)node->getSecondRightChild(b2);
179 FCL_REAL d1 = node->BVDistanceLowerBound(a1, a2);
180 FCL_REAL d2 = node->BVDistanceLowerBound(c1,
c2);
183 if (!node->canStop(d2))
188 if (!node->canStop(
d1))
193 if (!node->canStop(
d1))
198 if (!node->canStop(d2))
206 struct HPP_FCL_LOCAL
BVT {
217 return lhs.
d > rhs.
d;
224 bool empty()
const {
return pq.empty(); }
226 size_t size()
const {
return pq.size(); }
228 const BVT&
top()
const {
return pq.top(); }
234 bool full()
const {
return (pq.size() + 1 >= qsize); }
244 unsigned int qsize) {
253 bool l1 = node->isFirstNodeLeaf(min_test.
b1);
254 bool l2 = node->isSecondNodeLeaf(min_test.
b2);
259 node->leafComputeDistance(min_test.
b1, min_test.
b2);
260 }
else if (bvtq.
full()) {
268 if (node->firstOverSecond(min_test.
b1, min_test.
b2)) {
269 unsigned int c1 = (
unsigned int)node->getFirstLeftChild(min_test.
b1);
270 unsigned int c2 = (
unsigned int)node->getFirstRightChild(min_test.
b1);
272 bvt1.
b2 = min_test.
b2;
273 bvt1.
d = node->BVDistanceLowerBound(bvt1.
b1, bvt1.
b2);
276 bvt2.
b2 = min_test.
b2;
277 bvt2.
d = node->BVDistanceLowerBound(bvt2.
b1, bvt2.
b2);
279 unsigned int c1 = (
unsigned int)node->getSecondLeftChild(min_test.
b2);
280 unsigned int c2 = (
unsigned int)node->getSecondRightChild(min_test.
b2);
281 bvt1.
b1 = min_test.
b1;
283 bvt1.
d = node->BVDistanceLowerBound(bvt1.
b1, bvt1.
b2);
285 bvt2.
b1 = min_test.
b1;
287 bvt2.
d = node->BVDistanceLowerBound(bvt2.
b1, bvt2.
b2);
297 min_test = bvtq.
top();
300 if (node->canStop(min_test.
d)) {
312 FCL_REAL sqrDistLowerBound = -1, sqrDistLowerBound1 = 0,
313 sqrDistLowerBound2 = 0;
314 BVHFrontList::iterator front_iter;
316 for (front_iter = front_list->begin(); front_iter != front_list->end();
318 unsigned int b1 = front_iter->left;
319 unsigned int b2 = front_iter->right;
320 bool l1 = node->isFirstNodeLeaf(b1);
321 bool l2 = node->isSecondNodeLeaf(b2);
324 front_iter->valid =
false;
328 if (!node->BVDisjoints(b1, b2, sqrDistLowerBound)) {
329 front_iter->valid =
false;
330 if (node->firstOverSecond(b1, b2)) {
331 unsigned int c1 = (
unsigned int)node->getFirstLeftChild(b1);
332 unsigned int c2 = (
unsigned int)node->getFirstRightChild(b1);
336 sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2);
338 unsigned int c1 = (
unsigned int)node->getSecondLeftChild(b2);
339 unsigned int c2 = (
unsigned int)node->getSecondRightChild(b2);
343 sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2);
351 for (front_iter = front_list->begin(); front_iter != front_list->end();) {
352 if (!front_iter->valid)
353 front_iter = front_list->erase(front_iter);
358 for (front_iter = append.begin(); front_iter != append.end(); ++front_iter) {
359 front_list->push_back(*front_iter);