Library of Assembled Shared Sources
 
Loading...
Searching...
No Matches
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-2026 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 aabb_(std::move(other.aabb_)),
108 objects_(std::move(other.objects_)),
109 nodes_(std::move(other.nodes_)),
110 end_(std::move(other.end_))
111{
112}
113
114
115template <typename O, typename OT, typename SH>
116AabpTree<O, OT, SH>& AabpTree<O, OT, SH>::operator=(TSelf&& other) noexcept
117{
118 TSplitHeuristics::operator=(std::forward<TSplitHeuristics>(other));
119 aabb_ = std::move(other.aabb_);
120 objects_ = std::move(other.objects_);
121 nodes_ = std::move(other.nodes_);
122 end_ = std::move(other.end_);
123 return *this;
124}
125
126
127template <typename O, typename OT, typename SH>
128void AabpTree<O, OT, SH>::reset()
129{
130 TSelf temp(static_cast<const SH&>(*this));
131 swap(temp);
132}
133
134
135
136template <typename O, typename OT, typename SH>
137void AabpTree<O, OT, SH>::reset(TObjectIterator first, TObjectIterator last)
138{
139 TSelf temp(first, last, static_cast<const SH&>(*this));
140 swap(temp);
141}
142
143
144
145template <typename O, typename OT, typename SH>
146const typename AabpTree<O, OT, SH>::TAabb&
147AabpTree<O, OT, SH>::aabb() const
148{
149 return aabb_;
150}
151
152
153
154template <typename O, typename OT, typename SH>
155bool AabpTree<O, OT, SH>::contains(const TPoint& point, const TInfo* info) const
156{
157 if (isEmpty() || !TObjectTraits::aabbContains(aabb_, point))
158 {
159 return false;
160 }
161 return doContains(0, point, info);
162}
163
164
165
166template <typename O, typename OT, typename SH>
167template <typename OutputIterator>
168OutputIterator AabpTree<O, OT, SH>::find(const TPoint& point, OutputIterator result, const TInfo* info) const
169{
170 if (isEmpty() || !TObjectTraits::aabbContains(aabb_, point))
171 {
172 return result;
173 }
174 return doFind(0, point, result, info);
175}
176
177
178
179template <typename O, typename OT, typename SH>
180template <typename OutputIterator>
181OutputIterator AabpTree<O, OT, SH>::find(const TAabb& box, OutputIterator result, const TInfo* info) const
182{
183 if (isEmpty() || !TObjectTraits::aabbIntersects(aabb_, box))
184 {
185 return result;
186 }
187 return doFind(0, box, result, info);
188}
189
190
191
192template <typename O, typename OT, typename SH>
193template <typename OutputIterator>
194OutputIterator AabpTree<O, OT, SH>::find(const TRay& ray, TParam tMin, TParam tMax, OutputIterator result, const TInfo* info) const
195{
196 TValue tNear;
197 if (isEmpty() || !TObjectTraits::aabbIntersect(aabb_, ray, tNear, tMin))
198 {
199 return result;
200 }
201 TValue tFar;
202 if (!TObjectTraits::aabbIntersect(aabb_, ray, tFar, tNear))
203 {
204 tFar = tNear;
205 tNear = tMin;
206 }
207 if (tNear > tMax || tFar < tMin)
208 {
209 return result;
210 }
211 const TVector reciprocalDirection = TObjectTraits::vectorReciprocal(TObjectTraits::rayDirection(ray));
212 return doFind(0, ray, tMin, tMax, result, info, reciprocalDirection, tNear, tFar);
213}
214
215
216
217template <typename O, typename OT, typename SH>
218const typename AabpTree<O, OT, SH>::TObjectIterator
219AabpTree<O, OT, SH>::intersect(const TRay& ray, TReference t, TParam tMin, const TInfo* info) const
220{
221 TValue tNear;
222 if (isEmpty() || !TObjectTraits::aabbIntersect(aabb_, ray, tNear, tMin))
223 {
224 return *end_;
225 }
226 TValue tFar;
227 if (!TObjectTraits::aabbIntersect(aabb_, ray, tFar, tNear))
228 {
229 tFar = tNear;
230 tNear = tMin;
231 }
232 const TVector reciprocalDirection = TObjectTraits::vectorReciprocal(TObjectTraits::rayDirection(ray));
233 TObjectIterator hit = doIntersect(0, ray, t, tMin, info, reciprocalDirection, tNear, tFar);
234 LASS_ASSERT((t > tMin && t >= tNear * (1 - 1e-7f) && t <= tFar * (1 + 1e-7f)) || hit == *end_);
235 return hit;
236}
237
238
239
240template <typename O, typename OT, typename SH>
241bool AabpTree<O, OT, SH>::intersects(
242 const TRay& ray, TParam tMin, TParam tMax, const TInfo* info) const
243{
244 LASS_ASSERT(tMax > tMin || (num::isInf(tMin) && num::isInf(tMax)));
245 TValue tNear;
246 if (isEmpty() || !TObjectTraits::aabbIntersect(aabb_, ray, tNear, tMin))
247 {
248 return false;
249 }
250 TValue tFar;
251 if (!TObjectTraits::aabbIntersect(aabb_, ray, tFar, tNear))
252 {
253 tFar = tNear;
254 tNear = tMin;
255 }
256 if (tNear > tMax || tFar < tMin)
257 {
258 return false;
259 }
260 const TPoint support = TObjectTraits::raySupport(ray);
261 const TVector direction = TObjectTraits::rayDirection(ray);
262 const TVector reciprocalDirection = TObjectTraits::vectorReciprocal(direction);
263#if 1
264 struct Visit
265 {
266 TIndex index;
267 TValue tNear;
268 TValue tFar;
269 Visit(TIndex index = 0, TValue tNear = 0, TValue tFar = 0) : index(index), tNear(tNear), tFar(tFar) {}
270 };
271 Visit stack[32];
272 TIndex stackSize = 0;
273 stack[stackSize++] = Visit(0, tNear, tFar);
274 while (stackSize > 0)
275 {
276 const Visit visit = stack[--stackSize];
277 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
278 LASS_ASSERT(visit.index < nodes_.size());
279 const Node& node = nodes_[visit.index];
280
281 if (node.isLeaf())
282 {
283 for (TIndex i = node.first(); i != node.last(); ++i)
284 {
285 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
286 if (TObjectTraits::objectIntersects(objects_[i], ray, tMin, tMax, info))
287 {
288 return true;
289 }
290 }
291 continue;
292 }
293
294 const TIndex leftIndex = visit.index + 1;
295 const TIndex rightIndex = node.right();
296 const TValue s = TObjectTraits::coord(support, node.axis());
297 const TValue d = TObjectTraits::coord(direction, node.axis());
298 const TValue invD = TObjectTraits::coord(reciprocalDirection, node.axis());
299 const TValue tLeftBound = (node.leftBound() - s) * invD;
300 const TValue tRightBound = (node.rightBound() - s) * invD;
301 if (d > 0)
302 {
303 if (tRightBound < tFar)
304 {
305 stack[stackSize++] = Visit(rightIndex, std::max(tRightBound, visit.tNear), visit.tFar);
306 }
307 if (tLeftBound > tNear)
308 {
309 stack[stackSize++] = Visit(leftIndex, visit.tNear, std::min(tLeftBound, visit.tFar));
310 }
311 }
312 else if (d < 0)
313 {
314 if (tRightBound > tNear)
315 {
316 stack[stackSize++] = Visit(rightIndex, visit.tNear, std::min(tRightBound, visit.tFar));
317 }
318 if (tLeftBound < tFar)
319 {
320 stack[stackSize++] = Visit(leftIndex, std::max(tLeftBound, visit.tNear), visit.tFar);
321 }
322 }
323 else // if (d == TNumTraits::zero)
324 {
325 if (s >= node.rightBound())
326 {
327 stack[stackSize++] = Visit(rightIndex, visit.tNear, visit.tFar);
328 }
329 if (s <= node.leftBound())
330 {
331 stack[stackSize++] = Visit(leftIndex, visit.tNear, visit.tFar);
332 }
333 }
334 }
335 return false;
336#else
337 return doIntersects(0, ray, tMin, tMax, info, reciprocalDirection, tNear, tFar);
338#endif
339}
340
341
342
343template <typename O, typename OT, typename SH>
344const typename AabpTree<O, OT, SH>::Neighbour
345AabpTree<O, OT, SH>::nearestNeighbour(const TPoint& target, const TInfo* info) const
346{
347 Neighbour nearest(*end_, std::numeric_limits<TValue>::infinity());
348 if (!isEmpty())
349 {
350 doNearestNeighbour(0, target, info, nearest);
351 }
352 return nearest;
353}
354
355
356
357template <class O, class OT, typename SH>
358template <typename RandomAccessIterator>
359RandomAccessIterator
360AabpTree<O, OT, SH>::rangeSearch(
361 const TPoint& target, TParam maxRadius, size_t maxCount, RandomAccessIterator first,
362 const TInfo* info) const
363{
364 if (isEmpty() || maxRadius == 0)
365 {
366 return first;
367 }
368 TValue squaredRadius = maxRadius * maxRadius;
369 return doRangeSearch(0, target, squaredRadius, maxCount, first, first, info);
370}
371
372
373
374template <typename O, typename OT, typename SH>
375void AabpTree<O, OT, SH>::swap(TSelf& other)
376{
377 SH::swap(other);
378 std::swap(aabb_, other.aabb_);
379 nodes_.swap(other.nodes_);
380 objects_.swap(other.objects_);
381 end_.swap(other.end_);
382}
383
384
385
386template <typename O, typename OT, typename SH>
387bool AabpTree<O, OT, SH>::isEmpty() const
388{
389 return objects_.empty();
390}
391
392
393
394template <typename O, typename OT, typename SH>
395const typename AabpTree<O, OT, SH>::TObjectIterator
396AabpTree<O, OT, SH>::end() const
397{
398 return *end_;
399}
400
401
402
403// --- protected -----------------------------------------------------------------------------------
404
405
406
407// --- private -------------------------------------------------------------------------------------
408
409template <typename O, typename OT, typename SH>
410const typename AabpTree<O, OT, SH>::BalanceResult
411AabpTree<O, OT, SH>::balance(TInputIterator first, TInputIterator last)
412{
413 const SplitInfo<OT> split = TSplitHeuristics::template split<OT>(first, last);
414 if (split.isLeaf())
415 {
416 return BalanceResult(split.aabb, addLeafNode(first, last));
417 }
418
419 TInputIterator middle = std::partition(first, last, impl::Splitter<TObjectTraits>(split));
420 if (middle == first || middle == last)
421 {
422 const std::ptrdiff_t halfSize = (last - first) / 2;
423 LASS_ASSERT(halfSize > 0);
424 middle = first + halfSize;
425 std::nth_element(first, middle, last, impl::LessAxis<TObjectTraits>(split.axis));
426 }
427 LASS_ASSERT(middle != first && middle != last);
428
429 const TIndex index = addInternalNode(split.axis);
430 const BalanceResult left = balance(first, middle);
431 const BalanceResult right = balance(middle, last);
432
433 Node& node = nodes_[index];
434 node.setLeftBound(TObjectTraits::coord(TObjectTraits::aabbMax(left.aabb), split.axis));
435 node.setRightBound(TObjectTraits::coord(TObjectTraits::aabbMin(right.aabb), split.axis));
436 LASS_ASSERT(left.index == index + 1);
437 node.setRight(right.index);
438
439 return BalanceResult(split.aabb, index);
440}
441
442
443
444template <typename O, typename OT, typename SH>
445typename AabpTree<O, OT, SH>::TIndex
446AabpTree<O, OT, SH>::addLeafNode(TInputIterator first, TInputIterator last)
447{
448 LASS_ASSERT(objects_.size() <= maxSize);
449 const TIndex begin = static_cast<TIndex>(objects_.size());
450 while (first != last)
451 {
452 objects_.push_back((first++)->object);
453 }
454
455 LASS_ASSERT(objects_.size() <= maxSize);
456 const TIndex end = static_cast<TIndex>(objects_.size());
457
458 nodes_.push_back(Node(begin, end));
459
460 LASS_ASSERT(nodes_.size() > 0);
461 return static_cast<TIndex>(nodes_.size() - 1);
462}
463
464
465
466template <typename O, typename OT, typename SH>
467typename AabpTree<O, OT, SH>::TIndex
468AabpTree<O, OT, SH>::addInternalNode(size_t axis)
469{
470 nodes_.push_back(Node(axis));
471 LASS_ASSERT(nodes_.size() > 0);
472 return static_cast<TIndex>(nodes_.size() - 1);
473}
474
475
476
477template <typename O, typename OT, typename SH>
478bool AabpTree<O, OT, SH>::doContains(TIndex index, const TPoint& point, const TInfo* info) const
479{
480 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
481 LASS_ASSERT(index < nodes_.size());
482 const Node& node = nodes_[index];
483
484 if (node.isLeaf())
485 {
486 for (TIndex i = node.first(); i != node.last(); ++i)
487 {
488 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
489 if (TObjectTraits::objectContains(objects_[i], point, info))
490 {
491 return true;
492 }
493 }
494 return false;
495 }
496
497 const TValue x = TObjectTraits::coord(point, node.axis());
498 if (x <= node.leftBound() && doContains(index + 1, point, info))
499 {
500 return true;
501 }
502 if (x >= node.rightBound() && doContains(node.right(), point, info))
503 {
504 return true;
505 }
506 return false;
507}
508
509
510
511template <typename O, typename OT, typename SH>
512template <typename OutputIterator>
513OutputIterator AabpTree<O, OT, SH>::doFind(
514 TIndex index, const TPoint& point, OutputIterator result, const TInfo* info) const
515{
516 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
517 LASS_ASSERT(index < nodes_.size());
518 const Node& node = nodes_[index];
519
520 if (node.isLeaf())
521 {
522 for (TIndex i = node.first(); i != node.last(); ++i)
523 {
524 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
525 if (TObjectTraits::objectContains(objects_[i], point, info))
526 {
527 *result++ = objects_[i];
528 }
529 }
530 return result;
531 }
532
533 const TValue x = TObjectTraits::coord(point, node.axis());
534 if (x <= node.leftBound())
535 {
536 result = doFind(index + 1, point, result, info);
537 }
538 if (x >= node.rightBound())
539 {
540 result = doFind(node.right(), point, result, info);
541 }
542 return result;
543}
544
545
546
547template <typename O, typename OT, typename SH>
548template <typename OutputIterator>
549OutputIterator AabpTree<O, OT, SH>::doFind(
550 TIndex index, const TAabb& box, OutputIterator result, const TInfo* info) const
551{
552 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
553 LASS_ASSERT(index < nodes_.size());
554 const Node& node = nodes_[index];
555
556 if (node.isLeaf())
557 {
558 for (TIndex i = node.first(); i != node.last(); ++i)
559 {
560 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
561 if (TObjectTraits::objectIntersects(objects_[i], box, info))
562 {
563 *result++ = objects_[i];
564 }
565 }
566 return result;
567 }
568
569 if (TObjectTraits::coord(TObjectTraits::aabbMin(box), node.axis()) <= node.leftBound())
570 {
571 result = doFind(index + 1, box, result, info);
572 }
573 if (TObjectTraits::coord(TObjectTraits::aabbMax(box), node.axis()) >= node.rightBound())
574 {
575 result = doFind(node.right(), box, result, info);
576 }
577 return result;
578}
579
580
581
582template <typename O, typename OT, typename SH>
583template <typename OutputIterator>
584OutputIterator AabpTree<O, OT, SH>::doFind(
585 TIndex index, const TRay& ray, TParam tMin, TParam tMax, OutputIterator result, const TInfo* info,
586 const TVector& reciprocalDirection, TParam tNear, TParam tFar) const
587{
588 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
589 LASS_ASSERT(index < nodes_.size());
590 LASS_ASSERT(tFar >= tNear * (1 - 1e-6f));
591 const Node& node = nodes_[index];
592
593 if (node.isLeaf())
594 {
595 for (TIndex i = node.first(); i != node.last(); ++i)
596 {
597 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
598 if (TObjectTraits::objectIntersects(objects_[i], ray, tMin, tMax, info))
599 {
600 *result++ = objects_[i];
601 }
602 }
603 return result;
604 }
605
606 // check children
607 const TIndex leftIndex = index + 1;
608 const TIndex rightIndex = node.right();
609 const TValue s = TObjectTraits::coord(TObjectTraits::raySupport(ray), node.axis());
610 const TValue d = TObjectTraits::coord(TObjectTraits::rayDirection(ray), node.axis());
611 const TValue invD = TObjectTraits::coord(reciprocalDirection, node.axis());
612 const TValue tLeftBound = (node.leftBound() - s) * invD;
613 const TValue tRightBound = (node.rightBound() - s) * invD;
614
615 if (d > 0)
616 {
617 if (tLeftBound > tNear)
618 {
619 result = doFind(leftIndex, ray, tMin, tMax, result, info, reciprocalDirection, tNear, std::min(tLeftBound, tFar));
620 }
621 if (tRightBound < tFar)
622 {
623 result = doFind(rightIndex, ray, tMin, tMax, result, info, reciprocalDirection, std::max(tRightBound, tNear), tFar);
624 }
625 }
626 else if (d < 0)
627 {
628 if (tLeftBound < tFar)
629 {
630 result = doFind(leftIndex, ray, tMin, tMax, result, info, reciprocalDirection, std::max(tLeftBound, tNear), tFar);
631 }
632 if (tRightBound > tNear)
633 {
634 result = doFind(rightIndex, ray, tMin, tMax, result, info, reciprocalDirection, tNear, std::min(tRightBound, tFar));
635 }
636 }
637 else // if (d == TNumTraits::zero)
638 {
639 if (s <= node.leftBound())
640 {
641 result = doFind(leftIndex, ray, tMin, tMax, result, info, reciprocalDirection, tNear, tFar);
642 }
643 if (s >= node.rightBound())
644 {
645 result = doFind(rightIndex, ray, tMin, tMax, result, info, reciprocalDirection, tNear, tFar);
646 }
647 }
648 return result;
649}
650
651
652
653template <typename O, typename OT, typename SH>
654const typename AabpTree<O, OT, SH>::TObjectIterator
655AabpTree<O, OT, SH>::doIntersect(
656 TIndex index, const TRay& ray, TReference t, TParam tMin, const TInfo* info,
657 const TVector& reciprocalDirection, TParam tNear, TParam tFar) const
658{
659 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
660 LASS_ASSERT(index < nodes_.size());
661 LASS_ASSERT(tFar >= tNear * (1 - 1e-6f));
662 const Node& node = nodes_[index];
663
664 if (node.isLeaf())
665 {
666 TValue tBest = 0;
667 TObjectIterator best = *end_;
668 for (TIndex i = node.first(); i != node.last(); ++i)
669 {
670 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
671 TValue tCandidate = 0;
672 if (TObjectTraits::objectIntersect(objects_[i], ray, tCandidate, tMin, info))
673 {
674 LASS_ASSERT(tCandidate > tMin);
675 if (best == *end_ || tCandidate < tBest)
676 {
677 LASS_ASSERT(tCandidate > tMin && tCandidate >= tNear * (1 - 1e-6f) && tCandidate <= tFar * (1 + 1e-6f));
678 best = objects_[i];
679 tBest = tCandidate;
680 }
681 }
682 }
683 if (best != *end_)
684 {
685 t = tBest;
686 }
687 return best;
688 }
689
690 // check children
691 const TIndex leftIndex = index + 1;
692 const TIndex rightIndex = node.right();
693 const TValue s = TObjectTraits::coord(TObjectTraits::raySupport(ray), node.axis());
694 const TValue d = TObjectTraits::coord(TObjectTraits::rayDirection(ray), node.axis());
695 const TValue invD = TObjectTraits::coord(reciprocalDirection, node.axis());
696 const TValue tLeftBound = (node.leftBound() - s) * invD;
697 const TValue tRightBound = (node.rightBound() - s) * invD;
698
699 TValue tLeft = 0;
700 TValue tRight = 0;
701 TObjectIterator objectLeft = *end_;
702 TObjectIterator objectRight = *end_;
703 if (d > 0)
704 {
705 if (tLeftBound >= tNear * (1 - 1e-6f))
706 {
707 objectLeft = doIntersect(leftIndex, ray, tLeft, tMin, info, reciprocalDirection,
708 tNear, std::min(tLeftBound, tFar));
709 }
710 if (tRightBound <= tFar * (1 + 1e-6f))
711 {
712 objectRight = doIntersect(rightIndex, ray, tRight, tMin, info, reciprocalDirection,
713 std::max(tRightBound, tNear), tFar);
714 }
715 }
716 else if (d < 0)
717 {
718 if (tLeftBound <= tFar * (1 + 1e-6f))
719 {
720 objectLeft = doIntersect(leftIndex, ray, tLeft, tMin, info, reciprocalDirection,
721 std::max(tLeftBound, tNear), tFar);
722 }
723 if (tRightBound >= tNear * (1 - 1e-6f))
724 {
725 objectRight = doIntersect(rightIndex, ray, tRight, tMin, info, reciprocalDirection,
726 tNear, std::min(tRightBound, tFar));
727 }
728 }
729 else // if (d == TNumTraits::zero)
730 {
731 if (s <= node.leftBound())
732 {
733 objectLeft = doIntersect(leftIndex, ray, tLeft, tMin, info, reciprocalDirection,
734 tNear, tFar);
735 }
736 if (s >= node.rightBound())
737 {
738 objectRight = doIntersect(rightIndex, ray, tRight, tMin, info, reciprocalDirection,
739 tNear, tFar);
740 }
741 }
742
743 // determine result
744 if (objectLeft != *end_ && (objectRight == *end_ || tLeft < tRight))
745 {
746 LASS_ASSERT(tLeft > tMin);
747 t = tLeft;
748 return objectLeft;
749 }
750 if (objectRight != *end_)
751 {
752 LASS_ASSERT(objectLeft == *end_ || !(tLeft < tRight));
753 LASS_ASSERT(tRight > tMin);
754 t = tRight;
755 return objectRight;
756 }
757 return *end_;
758}
759
760
761
762template <typename O, typename OT, typename SH>
763bool AabpTree<O, OT, SH>::doIntersects(
764 TIndex index, const TRay& ray, TParam tMin, TParam tMax, const TInfo* info,
765 const TVector& reciprocalDirection, TParam tNear, TParam tFar) const
766{
767 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
768 LASS_ASSERT(index < nodes_.size());
769 LASS_ASSERT(tMax > tMin);
770 const Node& node = nodes_[index];
771
772 if (node.isLeaf())
773 {
774 for (TIndex i = node.first(); i != node.last(); ++i)
775 {
776 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
777 if (TObjectTraits::objectIntersects(objects_[i], ray, tMin, tMax, info))
778 {
779 return true;
780 }
781 }
782 return false;
783 }
784
785 // check children
786 const TIndex leftIndex = index + 1;
787 const TIndex rightIndex = node.right();
788 const TValue s = TObjectTraits::coord(TObjectTraits::raySupport(ray), node.axis());
789 const TValue d = TObjectTraits::coord(TObjectTraits::rayDirection(ray), node.axis());
790 const TValue invD = TObjectTraits::coord(reciprocalDirection, node.axis());
791 const TValue tLeftBound = (node.leftBound() - s) * invD;
792 const TValue tRightBound = (node.rightBound() - s) * invD;
793
794 if (d > 0)
795 {
796 if ((tLeftBound > tNear) && doIntersects(leftIndex, ray, tMin, tMax, info,
797 reciprocalDirection, tNear, std::min(tLeftBound, tFar)))
798 {
799 return true;
800 }
801 if ((tRightBound < tFar) && doIntersects(rightIndex, ray, tMin, tMax, info,
802 reciprocalDirection, std::max(tRightBound, tNear), tFar))
803 {
804 return true;
805 }
806 }
807 else if (d < 0)
808 {
809 if ((tLeftBound < tFar) && doIntersects(leftIndex, ray, tMin, tMax, info,
810 reciprocalDirection, std::max(tLeftBound, tNear), tFar))
811 {
812 return true;
813 }
814 if ((tRightBound > tNear) && doIntersects(rightIndex, ray, tMin, tMax, info,
815 reciprocalDirection, tNear, std::min(tRightBound, tFar)))
816 {
817 return true;
818 }
819 }
820 else // if (d == TNumTraits::zero)
821 {
822 if ((s <= node.leftBound()) && doIntersects(rightIndex, ray, tMin, tMax, info,
823 reciprocalDirection, tNear, tFar))
824 {
825 return true;
826 }
827 if ((s >= node.rightBound()) && doIntersects(rightIndex, ray, tMin, tMax, info,
828 reciprocalDirection, tNear, tFar))
829 {
830 return true;
831 }
832 }
833 return false;
834}
835
836
837
838template <typename O, typename OT, typename SH>
839void AabpTree<O, OT, SH>::doNearestNeighbour(
840 TIndex index, const TPoint& target, const TInfo* info, Neighbour& best) const
841{
842 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
843 LASS_ASSERT(index < nodes_.size());
844 LASS_ASSERT(best.squaredDistance() >= 0);
845
846 const Node& node = nodes_[index];
847
848 if (node.isLeaf())
849 {
850 for (TIndex i = node.first(); i != node.last(); ++i)
851 {
852 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
853 const TValue squaredDistance =
854 TObjectTraits::objectSquaredDistance(objects_[i], target, info);
855 if (squaredDistance < best.squaredDistance())
856 {
857 best = Neighbour(objects_[i], squaredDistance);
858 }
859 }
860 }
861 else
862 {
863 TIndex children[2];
864 TValue signedDist[2];
865 getChildren(index, target, children, signedDist);
866 if (signedDist[0] <= 0 || (signedDist[0] * signedDist[0]) < best.squaredDistance())
867 {
868 doNearestNeighbour(children[0], target, info, best);
869 if (signedDist[1] <= 0 || (signedDist[1] * signedDist[1]) < best.squaredDistance())
870 {
871 doNearestNeighbour(children[1], target, info, best);
872 }
873 }
874 }
875}
876
877
878
879template <typename O, typename OT, typename SH>
880template <typename RandomIterator>
881RandomIterator AabpTree<O, OT, SH>::doRangeSearch(
882 TIndex index, const TPoint& target, TReference squaredRadius, size_t maxCount,
883 RandomIterator first, RandomIterator last, const TInfo* info) const
884{
885 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
886 LASS_ASSERT(index < nodes_.size());
887 LASS_ASSERT(squaredRadius >= 0);
888
889 const Node& node = nodes_[index];
890
891 if (node.isLeaf())
892 {
893 for (TIndex i = node.first(); i != node.last(); ++i)
894 {
895 LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
896 const TValue sqrDist = TObjectTraits::objectSquaredDistance(objects_[i], target, info);
897 if (sqrDist < squaredRadius)
898 {
899 *last++ = Neighbour(objects_[i], sqrDist);
900 std::push_heap(first, last);
901 LASS_ASSERT(last >= first);
902 if (static_cast<size_t>(last - first) > maxCount)
903 {
904 std::pop_heap(first, last);
905 --last;
906 squaredRadius = first->squaredDistance();
907 }
908 }
909 }
910 return last;
911 }
912
913 TIndex children[2];
914 TValue signedDist[2];
915 getChildren(index, target, children, signedDist);
916 if (signedDist[0] <= 0 || (signedDist[0] * signedDist[0]) < squaredRadius)
917 {
918 last = doRangeSearch(children[0], target, squaredRadius, maxCount, first, last, info);
919 if (signedDist[1] <= 0 || (signedDist[1] * signedDist[1]) < squaredRadius)
920 {
921 last = doRangeSearch(children[1], target, squaredRadius, maxCount, first, last, info);
922 }
923 }
924 return last;
925}
926
927
928
929template <typename O, typename OT, typename SH>
930void AabpTree<O, OT, SH>::getChildren(
931 TIndex index, const TPoint& target, TIndex indices[2], TValue signedDistances[2]) const
932{
933 LASS_ASSERT(index < nodes_.size());
934 const Node& node = nodes_[index];
935 indices[0] = index + 1;
936 indices[1] = node.right();
937 const TValue x = TObjectTraits::coord(target, node.axis());
938 signedDistances[0] = x - node.leftBound();
939 signedDistances[1] = node.rightBound() - x;
940
941 if (signedDistances[1] < signedDistances[0])
942 {
943 std::swap(signedDistances[0], signedDistances[1]);
944 std::swap(indices[0], indices[1]);
945 }
946}
947
948
949
950// --- free ----------------------------------------------------------------------------------------
951
952}
953
954}
955
956#endif
957
958// 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