Library of Assembled Shared Sources
aabp_tree.inl
Go to the documentation of this file.
1/** @file
2 * @author Bram de Greve (bram@cocamware.com)
3 * @author Tom De Muer (tom@cocamware.com)
4 *
5 * *** BEGIN LICENSE INFORMATION ***
6 *
7 * The contents of this file are subject to the Common Public Attribution License
8 * Version 1.0 (the "License"); you may not use this file except in compliance with
9 * the License. You may obtain a copy of the License at
10 * http://lass.sourceforge.net/cpal-license. The License is based on the
11 * Mozilla Public License Version 1.1 but Sections 14 and 15 have been added to cover
12 * use of software over a computer network and provide for limited attribution for
13 * the Original Developer. In addition, Exhibit A has been modified to be consistent
14 * with Exhibit B.
15 *
16 * Software distributed under the License is distributed on an "AS IS" basis, WITHOUT
17 * WARRANTY OF ANY KIND, either express or implied. See the License for the specific
18 * language governing rights and limitations under the License.
19 *
20 * The Original Code is LASS - Library of Assembled Shared Sources.
21 *
22 * The Initial Developer of the Original Code is Bram de Greve and Tom De Muer.
23 * The Original Developer is the Initial Developer.
24 *
25 * All portions of the code written by the Initial Developer are:
26 * Copyright (C) 2004-2024 the Initial Developer.
27 * All Rights Reserved.
28 *
29 * Contributor(s):
30 *
31 * Alternatively, the contents of this file may be used under the terms of the
32 * GNU General Public License Version 2 or later (the GPL), in which case the
33 * provisions of GPL are applicable instead of those above. If you wish to allow use
34 * of your version of this file only under the terms of the GPL and not to allow
35 * others to use your version of this file under the CPAL, indicate your decision by
36 * deleting the provisions above and replace them with the notice and other
37 * provisions required by the GPL License. If you do not delete the provisions above,
38 * a recipient may use your version of this file under either the CPAL or the GPL.
39 *
40 * *** END LICENSE INFORMATION ***
41 */
42
43
44
45#ifndef LASS_GUARDIAN_OF_INCLUSION_SPAT_AABP_TREE_INL
46#define LASS_GUARDIAN_OF_INCLUSION_SPAT_AABP_TREE_INL
47
48#include "spat_common.h"
49#include "aabp_tree.h"
50
51#include <cstddef>
52
53namespace lass
54{
55namespace spat
56{
57
58// --- public --------------------------------------------------------------------------------------
59
60template <typename O, typename OT, typename SH>
61AabpTree<O, OT, SH>::AabpTree(const TSplitHeuristics& heuristics):
62 SH(heuristics),
63 aabb_(TObjectTraits::aabbEmpty()),
64 objects_(),
65 nodes_(),
66 end_(new TObjectIterator)
67{
68}
69
70
71
72template <typename O, typename OT, typename SH>
73AabpTree<O, OT, SH>::AabpTree(TObjectIterator first, TObjectIterator last, const TSplitHeuristics& heuristics):
74 SH(heuristics),
75 aabb_(TObjectTraits::aabbEmpty()),
76 objects_(),
77 nodes_(),
78 end_(new TObjectIterator(last))
79{
80 const std::ptrdiff_t size = last - first;
81 if (size < 0)
82 {
83 LASS_THROW("AabpTree: invalid range");
84 }
85 if (static_cast<size_t>(size) >= maxSize)
86 {
87 LASS_THROW("AabpTree: too many objects");
88 }
89 if (first != last)
90 {
91 TInputs inputs;
92 inputs.reserve(static_cast<size_t>(size));
93 for (TObjectIterator i = first; i != last; ++i)
94 {
95 TAabb aabb = TObjectTraits::objectAabb(i);
96 aabb_ = TObjectTraits::aabbJoin(aabb_, aabb);
97 inputs.push_back(Input(aabb, i));
98 }
99 balance(inputs.begin(), inputs.end());
100 }
101}
102
103
104template <typename O, typename OT, typename SH>
105AabpTree<O, OT, SH>::AabpTree(TSelf&& other) noexcept:
106 SH(std::forward< TSplitHeuristics>(other)),
107 objects_(std::move(other.objects_)),
108 nodes_(std::move(other.nodes_)),
109 end_(std::move(other.end_))
110{
111}
112
113
114template <typename O, typename OT, typename SH>
115AabpTree<O, OT, SH>& AabpTree<O, OT, SH>::operator=(TSelf&& other) noexcept
116{
117 TSplitHeuristics::operator=(std::forward<TSplitHeuristics>(other));
118 objects_ = std::move(other.objects_);
119 nodes_ = std::move(other.nodes_);
120 end_ = std::move(other.end_);
121 return *this;
122}
123
124
125template <typename O, typename OT, typename SH>
126void AabpTree<O, OT, SH>::reset()
127{
128 TSelf temp(static_cast<const SH&>(*this));
129 swap(temp);
130}
131
132
133
134template <typename O, typename OT, typename SH>
135void AabpTree<O, OT, SH>::reset(TObjectIterator first, TObjectIterator last)
136{
137 TSelf temp(first, last, static_cast<const SH&>(*this));
138 swap(temp);
139}
140
141
142
143template <typename O, typename OT, typename SH>
144const typename AabpTree<O, OT, SH>::TAabb&
145AabpTree<O, OT, SH>::aabb() const
146{
147 return aabb_;
148}
149
150
151
152template <typename O, typename OT, typename SH>
153bool AabpTree<O, OT, SH>::contains(const TPoint& point, const TInfo* info) const
154{
155 if (isEmpty() || !TObjectTraits::aabbContains(aabb_, point))
156 {
157 return false;
158 }
159 return doContains(0, point, info);
160}
161
162
163
164template <typename O, typename OT, typename SH>
165template <typename OutputIterator>
166OutputIterator AabpTree<O, OT, SH>::find(const TPoint& point, OutputIterator result, const TInfo* info) const
167{
168 if (isEmpty() || !TObjectTraits::aabbContains(aabb_, point))
169 {
170 return result;
171 }
172 return doFind(0, point, result, info);
173}
174
175
176
177template <typename O, typename OT, typename SH>
178template <typename OutputIterator>
179OutputIterator AabpTree<O, OT, SH>::find(const TAabb& box, OutputIterator result, const TInfo* info) const
180{
181 if (isEmpty() || !TObjectTraits::aabbIntersects(aabb_, box))
182 {
183 return result;
184 }
185 return doFind(0, box, result, info);
186}
187
188
189
190template <typename O, typename OT, typename SH>
191template <typename OutputIterator>
192OutputIterator AabpTree<O, OT, SH>::find(const TRay& ray, TParam tMin, TParam tMax, OutputIterator result, const TInfo* info) const
193{
194 TValue tNear;
195 if (isEmpty() || !TObjectTraits::aabbIntersect(aabb_, ray, tNear, tMin))
196 {
197 return result;
198 }
199 TValue tFar;
200 if (!TObjectTraits::aabbIntersect(aabb_, ray, tFar, tNear))
201 {
202 tFar = tNear;
203 tNear = tMin;
204 }
205 if (tNear > tMax || tFar < tMin)
206 {
207 return result;
208 }
209 const TVector reciprocalDirection = TObjectTraits::vectorReciprocal(TObjectTraits::rayDirection(ray));
210 return doFind(0, ray, tMin, tMax, result, info, reciprocalDirection, tNear, tFar);
211}
212
213
214
215template <typename O, typename OT, typename SH>
216const typename AabpTree<O, OT, SH>::TObjectIterator
217AabpTree<O, OT, SH>::intersect(const TRay& ray, TReference t, TParam tMin, const TInfo* info) const
218{
219 TValue tNear;
220 if (isEmpty() || !TObjectTraits::aabbIntersect(aabb_, ray, tNear, tMin))
221 {
222 return *end_;
223 }
224 TValue tFar;
225 if (!TObjectTraits::aabbIntersect(aabb_, ray, tFar, tNear))
226 {
227 tFar = tNear;
228 tNear = tMin;
229 }
230 const TVector reciprocalDirection = TObjectTraits::vectorReciprocal(TObjectTraits::rayDirection(ray));
231 TObjectIterator hit = doIntersect(0, ray, t, tMin, info, reciprocalDirection, tNear, tFar);
232 LASS_ASSERT((t > tMin && t >= tNear * (1 - 1e-7f) && t <= tFar * (1 + 1e-7f)) || hit == *end_);
233 return hit;
234}
235
236
237
238template <typename O, typename OT, typename SH>
239bool AabpTree<O, OT, SH>::intersects(
240 const TRay& ray, TParam tMin, TParam tMax, const TInfo* info) const
241{
242 LASS_ASSERT(tMax > tMin || (num::isInf(tMin) && num::isInf(tMax)));
243 TValue tNear;
244 if (isEmpty() || !TObjectTraits::aabbIntersect(aabb_, ray, tNear, tMin))
245 {
246 return false;
247 }
248 TValue tFar;
249 if (!TObjectTraits::aabbIntersect(aabb_, ray, tFar, tNear))
250 {
251 tFar = tNear;
252 tNear = tMin;
253 }
254 if (tNear > tMax || tFar < tMin)
255 {
256 return false;
257 }
258 const TPoint support = TObjectTraits::raySupport(ray);
259 const TVector direction = TObjectTraits::rayDirection(ray);
260 const TVector reciprocalDirection = TObjectTraits::vectorReciprocal(direction);
261#if 1
262 struct Visit
263 {
264 TIndex index;
265 TValue tNear;
266 TValue tFar;
267 Visit(TIndex index = 0, TValue tNear = 0, TValue tFar = 0) : index(index), tNear(tNear), tFar(tFar) {}
268 };
269 Visit stack[32];
270 TIndex stackSize = 0;
271 stack[stackSize++] = Visit(0, tNear, tFar);
272 while (stackSize > 0)
273 {
274 const Visit visit = stack[--stackSize];
275 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
276 LASS_ASSERT(visit.index < nodes_.size());
277 const Node& node = nodes_[visit.index];
278
279 if (node.isLeaf())
280 {
281 for (TIndex i = node.first(); i != node.last(); ++i)
282 {
283 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
284 if (TObjectTraits::objectIntersects(objects_[i], ray, tMin, tMax, info))
285 {
286 return true;
287 }
288 }
289 continue;
290 }
291
292 const TIndex leftIndex = visit.index + 1;
293 const TIndex rightIndex = node.right();
294 const TValue s = TObjectTraits::coord(support, node.axis());
295 const TValue d = TObjectTraits::coord(direction, node.axis());
296 const TValue invD = TObjectTraits::coord(reciprocalDirection, node.axis());
297 const TValue tLeftBound = (node.leftBound() - s) * invD;
298 const TValue tRightBound = (node.rightBound() - s) * invD;
299 if (d > 0)
300 {
301 if (tRightBound < tFar)
302 {
303 stack[stackSize++] = Visit(rightIndex, std::max(tRightBound, visit.tNear), visit.tFar);
304 }
305 if (tLeftBound > tNear)
306 {
307 stack[stackSize++] = Visit(leftIndex, visit.tNear, std::min(tLeftBound, visit.tFar));
308 }
309 }
310 else if (d < 0)
311 {
312 if (tRightBound > tNear)
313 {
314 stack[stackSize++] = Visit(rightIndex, visit.tNear, std::min(tRightBound, visit.tFar));
315 }
316 if (tLeftBound < tFar)
317 {
318 stack[stackSize++] = Visit(leftIndex, std::max(tLeftBound, visit.tNear), visit.tFar);
319 }
320 }
321 else // if (d == TNumTraits::zero)
322 {
323 if (s >= node.rightBound())
324 {
325 stack[stackSize++] = Visit(rightIndex, visit.tNear, visit.tFar);
326 }
327 if (s <= node.leftBound())
328 {
329 stack[stackSize++] = Visit(leftIndex, visit.tNear, visit.tFar);
330 }
331 }
332 }
333 return false;
334#else
335 return doIntersects(0, ray, tMin, tMax, info, reciprocalDirection, tNear, tFar);
336#endif
337}
338
339
340
341template <typename O, typename OT, typename SH>
342const typename AabpTree<O, OT, SH>::Neighbour
343AabpTree<O, OT, SH>::nearestNeighbour(const TPoint& target, const TInfo* info) const
344{
345 Neighbour nearest(*end_, std::numeric_limits<TValue>::infinity());
346 if (!isEmpty())
347 {
348 doNearestNeighbour(0, target, info, nearest);
349 }
350 return nearest;
351}
352
353
354
355template <class O, class OT, typename SH>
356template <typename RandomAccessIterator>
357RandomAccessIterator
358AabpTree<O, OT, SH>::rangeSearch(
359 const TPoint& target, TParam maxRadius, size_t maxCount, RandomAccessIterator first,
360 const TInfo* info) const
361{
362 if (isEmpty() || maxRadius == 0)
363 {
364 return first;
365 }
366 TValue squaredRadius = maxRadius * maxRadius;
367 return doRangeSearch(0, target, squaredRadius, maxCount, first, first, info);
368}
369
370
371
372template <typename O, typename OT, typename SH>
373void AabpTree<O, OT, SH>::swap(TSelf& other)
374{
375 SH::swap(other);
376 std::swap(aabb_, other.aabb_);
377 nodes_.swap(other.nodes_);
378 objects_.swap(other.objects_);
379 end_.swap(other.end_);
380}
381
382
383
384template <typename O, typename OT, typename SH>
385bool AabpTree<O, OT, SH>::isEmpty() const
386{
387 return objects_.empty();
388}
389
390
391
392template <typename O, typename OT, typename SH>
393const typename AabpTree<O, OT, SH>::TObjectIterator
394AabpTree<O, OT, SH>::end() const
395{
396 return *end_;
397}
398
399
400
401// --- protected -----------------------------------------------------------------------------------
402
403
404
405// --- private -------------------------------------------------------------------------------------
406
407template <typename O, typename OT, typename SH>
408const typename AabpTree<O, OT, SH>::BalanceResult
409AabpTree<O, OT, SH>::balance(TInputIterator first, TInputIterator last)
410{
411 const SplitInfo<OT> split = TSplitHeuristics::template split<OT>(first, last);
412 if (split.isLeaf())
413 {
414 return BalanceResult(split.aabb, addLeafNode(first, last));
415 }
416
417 TInputIterator middle = std::partition(first, last, impl::Splitter<TObjectTraits>(split));
418 if (middle == first || middle == last)
419 {
420 const std::ptrdiff_t halfSize = (last - first) / 2;
421 LASS_ASSERT(halfSize > 0);
422 middle = first + halfSize;
423 std::nth_element(first, middle, last, impl::LessAxis<TObjectTraits>(split.axis));
424 }
425 LASS_ASSERT(middle != first && middle != last);
426
427 const TIndex index = addInternalNode(split.axis);
428 const BalanceResult left = balance(first, middle);
429 const BalanceResult right = balance(middle, last);
430
431 Node& node = nodes_[index];
432 node.setLeftBound(TObjectTraits::coord(TObjectTraits::aabbMax(left.aabb), split.axis));
433 node.setRightBound(TObjectTraits::coord(TObjectTraits::aabbMin(right.aabb), split.axis));
434 LASS_ASSERT(left.index == index + 1);
435 node.setRight(right.index);
436
437 return BalanceResult(split.aabb, index);
438}
439
440
441
442template <typename O, typename OT, typename SH>
443typename AabpTree<O, OT, SH>::TIndex
444AabpTree<O, OT, SH>::addLeafNode(TInputIterator first, TInputIterator last)
445{
446 LASS_ASSERT(objects_.size() <= maxSize);
447 const TIndex begin = static_cast<TIndex>(objects_.size());
448 while (first != last)
449 {
450 objects_.push_back((first++)->object);
451 }
452
453 LASS_ASSERT(objects_.size() <= maxSize);
454 const TIndex end = static_cast<TIndex>(objects_.size());
455
456 nodes_.push_back(Node(begin, end));
457
458 LASS_ASSERT(nodes_.size() > 0);
459 return static_cast<TIndex>(nodes_.size() - 1);
460}
461
462
463
464template <typename O, typename OT, typename SH>
465typename AabpTree<O, OT, SH>::TIndex
466AabpTree<O, OT, SH>::addInternalNode(size_t axis)
467{
468 nodes_.push_back(Node(axis));
469 LASS_ASSERT(nodes_.size() > 0);
470 return static_cast<TIndex>(nodes_.size() - 1);
471}
472
473
474
475template <typename O, typename OT, typename SH>
476bool AabpTree<O, OT, SH>::doContains(TIndex index, const TPoint& point, const TInfo* info) const
477{
478 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
479 LASS_ASSERT(index < nodes_.size());
480 const Node& node = nodes_[index];
481
482 if (node.isLeaf())
483 {
484 for (TIndex i = node.first(); i != node.last(); ++i)
485 {
486 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
487 if (TObjectTraits::objectContains(objects_[i], point, info))
488 {
489 return true;
490 }
491 }
492 return false;
493 }
494
495 const TValue x = TObjectTraits::coord(point, node.axis());
496 if (x <= node.leftBound() && doContains(index + 1, point, info))
497 {
498 return true;
499 }
500 if (x >= node.rightBound() && doContains(node.right(), point, info))
501 {
502 return true;
503 }
504 return false;
505}
506
507
508
509template <typename O, typename OT, typename SH>
510template <typename OutputIterator>
511OutputIterator AabpTree<O, OT, SH>::doFind(
512 TIndex index, const TPoint& point, OutputIterator result, const TInfo* info) const
513{
514 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
515 LASS_ASSERT(index < nodes_.size());
516 const Node& node = nodes_[index];
517
518 if (node.isLeaf())
519 {
520 for (TIndex i = node.first(); i != node.last(); ++i)
521 {
522 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
523 if (TObjectTraits::objectContains(objects_[i], point, info))
524 {
525 *result++ = objects_[i];
526 }
527 }
528 return result;
529 }
530
531 const TValue x = TObjectTraits::coord(point, node.axis());
532 if (x <= node.leftBound())
533 {
534 result = doFind(index + 1, point, result, info);
535 }
536 if (x >= node.rightBound())
537 {
538 result = doFind(node.right(), point, result, info);
539 }
540 return result;
541}
542
543
544
545template <typename O, typename OT, typename SH>
546template <typename OutputIterator>
547OutputIterator AabpTree<O, OT, SH>::doFind(
548 TIndex index, const TAabb& box, OutputIterator result, const TInfo* info) const
549{
550 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
551 LASS_ASSERT(index < nodes_.size());
552 const Node& node = nodes_[index];
553
554 if (node.isLeaf())
555 {
556 for (TIndex i = node.first(); i != node.last(); ++i)
557 {
558 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
559 if (TObjectTraits::objectIntersects(objects_[i], box, info))
560 {
561 *result++ = objects_[i];
562 }
563 }
564 return result;
565 }
566
567 if (TObjectTraits::coord(TObjectTraits::aabbMin(box), node.axis()) <= node.leftBound())
568 {
569 result = doFind(index + 1, box, result, info);
570 }
571 if (TObjectTraits::coord(TObjectTraits::aabbMax(box), node.axis()) >= node.rightBound())
572 {
573 result = doFind(node.right(), box, result, info);
574 }
575 return result;
576}
577
578
579
580template <typename O, typename OT, typename SH>
581template <typename OutputIterator>
582OutputIterator AabpTree<O, OT, SH>::doFind(
583 TIndex index, const TRay& ray, TParam tMin, TParam tMax, OutputIterator result, const TInfo* info,
584 const TVector& reciprocalDirection, TParam tNear, TParam tFar) const
585{
586 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
587 LASS_ASSERT(index < nodes_.size());
588 LASS_ASSERT(tFar >= tNear * (1 - 1e-6f));
589 const Node& node = nodes_[index];
590
591 if (node.isLeaf())
592 {
593 for (TIndex i = node.first(); i != node.last(); ++i)
594 {
595 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
596 if (TObjectTraits::objectIntersects(objects_[i], ray, tMin, tMax, info))
597 {
598 *result++ = objects_[i];
599 }
600 }
601 return result;
602 }
603
604 // check children
605 const TIndex leftIndex = index + 1;
606 const TIndex rightIndex = node.right();
607 const TValue s = TObjectTraits::coord(TObjectTraits::raySupport(ray), node.axis());
608 const TValue d = TObjectTraits::coord(TObjectTraits::rayDirection(ray), node.axis());
609 const TValue invD = TObjectTraits::coord(reciprocalDirection, node.axis());
610 const TValue tLeftBound = (node.leftBound() - s) * invD;
611 const TValue tRightBound = (node.rightBound() - s) * invD;
612
613 if (d > 0)
614 {
615 if (tLeftBound > tNear)
616 {
617 result = doFind(leftIndex, ray, tMin, tMax, result, info, reciprocalDirection, tNear, std::min(tLeftBound, tFar));
618 }
619 if (tRightBound < tFar)
620 {
621 result = doFind(rightIndex, ray, tMin, tMax, result, info, reciprocalDirection, std::max(tRightBound, tNear), tFar);
622 }
623 }
624 else if (d < 0)
625 {
626 if (tLeftBound < tFar)
627 {
628 result = doFind(leftIndex, ray, tMin, tMax, result, info, reciprocalDirection, std::max(tLeftBound, tNear), tFar);
629 }
630 if (tRightBound > tNear)
631 {
632 result = doFind(rightIndex, ray, tMin, tMax, result, info, reciprocalDirection, tNear, std::min(tRightBound, tFar));
633 }
634 }
635 else // if (d == TNumTraits::zero)
636 {
637 if (s <= node.leftBound())
638 {
639 result = doFind(leftIndex, ray, tMin, tMax, result, info, reciprocalDirection, tNear, tFar);
640 }
641 if (s >= node.rightBound())
642 {
643 result = doFind(rightIndex, ray, tMin, tMax, result, info, reciprocalDirection, tNear, tFar);
644 }
645 }
646 return result;
647}
648
649
650
651template <typename O, typename OT, typename SH>
652const typename AabpTree<O, OT, SH>::TObjectIterator
653AabpTree<O, OT, SH>::doIntersect(
654 TIndex index, const TRay& ray, TReference t, TParam tMin, const TInfo* info,
655 const TVector& reciprocalDirection, TParam tNear, TParam tFar) const
656{
657 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
658 LASS_ASSERT(index < nodes_.size());
659 LASS_ASSERT(tFar >= tNear * (1 - 1e-6f));
660 const Node& node = nodes_[index];
661
662 if (node.isLeaf())
663 {
664 TValue tBest = 0;
665 TObjectIterator best = *end_;
666 for (TIndex i = node.first(); i != node.last(); ++i)
667 {
668 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
669 TValue tCandidate = 0;
670 if (TObjectTraits::objectIntersect(objects_[i], ray, tCandidate, tMin, info))
671 {
672 LASS_ASSERT(tCandidate > tMin);
673 if (best == *end_ || tCandidate < tBest)
674 {
675 LASS_ASSERT(tCandidate > tMin && tCandidate >= tNear * (1 - 1e-6f) && tCandidate <= tFar * (1 + 1e-6f));
676 best = objects_[i];
677 tBest = tCandidate;
678 }
679 }
680 }
681 if (best != *end_)
682 {
683 t = tBest;
684 }
685 return best;
686 }
687
688 // check children
689 const TIndex leftIndex = index + 1;
690 const TIndex rightIndex = node.right();
691 const TValue s = TObjectTraits::coord(TObjectTraits::raySupport(ray), node.axis());
692 const TValue d = TObjectTraits::coord(TObjectTraits::rayDirection(ray), node.axis());
693 const TValue invD = TObjectTraits::coord(reciprocalDirection, node.axis());
694 const TValue tLeftBound = (node.leftBound() - s) * invD;
695 const TValue tRightBound = (node.rightBound() - s) * invD;
696
697 TValue tLeft = 0;
698 TValue tRight = 0;
699 TObjectIterator objectLeft = *end_;
700 TObjectIterator objectRight = *end_;
701 if (d > 0)
702 {
703 if (tLeftBound >= tNear * (1 - 1e-6f))
704 {
705 objectLeft = doIntersect(leftIndex, ray, tLeft, tMin, info, reciprocalDirection,
706 tNear, std::min(tLeftBound, tFar));
707 }
708 if (tRightBound <= tFar * (1 + 1e-6f))
709 {
710 objectRight = doIntersect(rightIndex, ray, tRight, tMin, info, reciprocalDirection,
711 std::max(tRightBound, tNear), tFar);
712 }
713 }
714 else if (d < 0)
715 {
716 if (tLeftBound <= tFar * (1 + 1e-6f))
717 {
718 objectLeft = doIntersect(leftIndex, ray, tLeft, tMin, info, reciprocalDirection,
719 std::max(tLeftBound, tNear), tFar);
720 }
721 if (tRightBound >= tNear * (1 - 1e-6f))
722 {
723 objectRight = doIntersect(rightIndex, ray, tRight, tMin, info, reciprocalDirection,
724 tNear, std::min(tRightBound, tFar));
725 }
726 }
727 else // if (d == TNumTraits::zero)
728 {
729 if (s <= node.leftBound())
730 {
731 objectLeft = doIntersect(leftIndex, ray, tLeft, tMin, info, reciprocalDirection,
732 tNear, tFar);
733 }
734 if (s >= node.rightBound())
735 {
736 objectRight = doIntersect(rightIndex, ray, tRight, tMin, info, reciprocalDirection,
737 tNear, tFar);
738 }
739 }
740
741 // determine result
742 if (objectLeft != *end_ && (objectRight == *end_ || tLeft < tRight))
743 {
744 LASS_ASSERT(tLeft > tMin);
745 t = tLeft;
746 return objectLeft;
747 }
748 if (objectRight != *end_)
749 {
750 LASS_ASSERT(objectLeft == *end_ || !(tLeft < tRight));
751 LASS_ASSERT(tRight > tMin);
752 t = tRight;
753 return objectRight;
754 }
755 return *end_;
756}
757
758
759
760template <typename O, typename OT, typename SH>
761bool AabpTree<O, OT, SH>::doIntersects(
762 TIndex index, const TRay& ray, TParam tMin, TParam tMax, const TInfo* info,
763 const TVector& reciprocalDirection, TParam tNear, TParam tFar) const
764{
765 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
766 LASS_ASSERT(index < nodes_.size());
767 LASS_ASSERT(tMax > tMin);
768 const Node& node = nodes_[index];
769
770 if (node.isLeaf())
771 {
772 for (TIndex i = node.first(); i != node.last(); ++i)
773 {
774 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
775 if (TObjectTraits::objectIntersects(objects_[i], ray, tMin, tMax, info))
776 {
777 return true;
778 }
779 }
780 return false;
781 }
782
783 // check children
784 const TIndex leftIndex = index + 1;
785 const TIndex rightIndex = node.right();
786 const TValue s = TObjectTraits::coord(TObjectTraits::raySupport(ray), node.axis());
787 const TValue d = TObjectTraits::coord(TObjectTraits::rayDirection(ray), node.axis());
788 const TValue invD = TObjectTraits::coord(reciprocalDirection, node.axis());
789 const TValue tLeftBound = (node.leftBound() - s) * invD;
790 const TValue tRightBound = (node.rightBound() - s) * invD;
791
792 if (d > 0)
793 {
794 if ((tLeftBound > tNear) && doIntersects(leftIndex, ray, tMin, tMax, info,
795 reciprocalDirection, tNear, std::min(tLeftBound, tFar)))
796 {
797 return true;
798 }
799 if ((tRightBound < tFar) && doIntersects(rightIndex, ray, tMin, tMax, info,
800 reciprocalDirection, std::max(tRightBound, tNear), tFar))
801 {
802 return true;
803 }
804 }
805 else if (d < 0)
806 {
807 if ((tLeftBound < tFar) && doIntersects(leftIndex, ray, tMin, tMax, info,
808 reciprocalDirection, std::max(tLeftBound, tNear), tFar))
809 {
810 return true;
811 }
812 if ((tRightBound > tNear) && doIntersects(rightIndex, ray, tMin, tMax, info,
813 reciprocalDirection, tNear, std::min(tRightBound, tFar)))
814 {
815 return true;
816 }
817 }
818 else // if (d == TNumTraits::zero)
819 {
820 if ((s <= node.leftBound()) && doIntersects(rightIndex, ray, tMin, tMax, info,
821 reciprocalDirection, tNear, tFar))
822 {
823 return true;
824 }
825 if ((s >= node.rightBound()) && doIntersects(rightIndex, ray, tMin, tMax, info,
826 reciprocalDirection, tNear, tFar))
827 {
828 return true;
829 }
830 }
831 return false;
832}
833
834
835
836template <typename O, typename OT, typename SH>
837void AabpTree<O, OT, SH>::doNearestNeighbour(
838 TIndex index, const TPoint& target, const TInfo* info, Neighbour& best) const
839{
840 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
841 LASS_ASSERT(index < nodes_.size());
842 LASS_ASSERT(best.squaredDistance() >= 0);
843
844 const Node& node = nodes_[index];
845
846 if (node.isLeaf())
847 {
848 for (TIndex i = node.first(); i != node.last(); ++i)
849 {
850 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
851 const TValue squaredDistance =
852 TObjectTraits::objectSquaredDistance(objects_[i], target, info);
853 if (squaredDistance < best.squaredDistance())
854 {
855 best = Neighbour(objects_[i], squaredDistance);
856 }
857 }
858 }
859 else
860 {
861 TIndex children[2];
862 TValue signedDist[2];
863 getChildren(index, target, children, signedDist);
864 if (signedDist[0] <= 0 || (signedDist[0] * signedDist[0]) < best.squaredDistance())
865 {
866 doNearestNeighbour(children[0], target, info, best);
867 if (signedDist[1] <= 0 || (signedDist[1] * signedDist[1]) < best.squaredDistance())
868 {
869 doNearestNeighbour(children[1], target, info, best);
870 }
871 }
872 }
873}
874
875
876
877template <typename O, typename OT, typename SH>
878template <typename RandomIterator>
879RandomIterator AabpTree<O, OT, SH>::doRangeSearch(
880 TIndex index, const TPoint& target, TReference squaredRadius, size_t maxCount,
881 RandomIterator first, RandomIterator last, const TInfo* info) const
882{
883 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
884 LASS_ASSERT(index < nodes_.size());
885 LASS_ASSERT(squaredRadius >= 0);
886
887 const Node& node = nodes_[index];
888
889 if (node.isLeaf())
890 {
891 for (TIndex i = node.first(); i != node.last(); ++i)
892 {
893 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
894 const TValue sqrDist = TObjectTraits::objectSquaredDistance(objects_[i], target, info);
895 if (sqrDist < squaredRadius)
896 {
897 *last++ = Neighbour(objects_[i], sqrDist);
898 std::push_heap(first, last);
899 LASS_ASSERT(last >= first);
900 if (static_cast<size_t>(last - first) > maxCount)
901 {
902 std::pop_heap(first, last);
903 --last;
904 squaredRadius = first->squaredDistance();
905 }
906 }
907 }
908 return last;
909 }
910
911 TIndex children[2];
912 TValue signedDist[2];
913 getChildren(index, target, children, signedDist);
914 if (signedDist[0] <= 0 || (signedDist[0] * signedDist[0]) < squaredRadius)
915 {
916 last = doRangeSearch(children[0], target, squaredRadius, maxCount, first, last, info);
917 if (signedDist[1] <= 0 || (signedDist[1] * signedDist[1]) < squaredRadius)
918 {
919 last = doRangeSearch(children[1], target, squaredRadius, maxCount, first, last, info);
920 }
921 }
922 return last;
923}
924
925
926
927template <typename O, typename OT, typename SH>
928void AabpTree<O, OT, SH>::getChildren(
929 TIndex index, const TPoint& target, TIndex indices[2], TValue signedDistances[2]) const
930{
931 LASS_ASSERT(index < nodes_.size());
932 const Node& node = nodes_[index];
933 indices[0] = index + 1;
934 indices[1] = node.right();
935 const TValue x = TObjectTraits::coord(target, node.axis());
936 signedDistances[0] = x - node.leftBound();
937 signedDistances[1] = node.rightBound() - x;
938
939 if (signedDistances[1] < signedDistances[0])
940 {
941 std::swap(signedDistances[0], signedDistances[1]);
942 std::swap(indices[0], indices[1]);
943 }
944}
945
946
947
948// --- free ----------------------------------------------------------------------------------------
949
950}
951
952}
953
954#endif
955
956// EOF
aabp_tree.h
lass::num::lass::stde::split
std::vector< std::basic_string< Char, Traits, Alloc > > split(const std::basic_string< Char, Traits, Alloc > &to_be_split)
Reflects the Python function split without seperator argument.
Definition basic_ops.h:211
lass::spat
spatial subdivisions, quadtrees, octrees, meshes in 2D and 3D, triangulators, ...
Definition aabb8_tree.h:80
lass
Library for Assembled Shared Sources.
Definition config.h:53
spat_common.h