aabb8_tree.inl

Go to the documentation of this file.

/** @file
    *   @author Bram de Greve (bram@cocamware.com)
    *   @author Tom De Muer (tom@cocamware.com)
    *
    *   *** BEGIN LICENSE INFORMATION ***
    *   
    *   The contents of this file are subject to the Common Public Attribution License 
    *   Version 1.0 (the "License"); you may not use this file except in compliance with 
    *   the License. You may obtain a copy of the License at 
    *   http://lass.sourceforge.net/cpal-license. The License is based on the 
    *   Mozilla Public License Version 1.1 but Sections 14 and 15 have been added to cover 
    *   use of software over a computer network and provide for limited attribution for 
    *   the Original Developer. In addition, Exhibit A has been modified to be consistent 
    *   with Exhibit B.
    *   
    *   Software distributed under the License is distributed on an "AS IS" basis, WITHOUT 
    *   WARRANTY OF ANY KIND, either express or implied. See the License for the specific 
    *   language governing rights and limitations under the License.
    *   
    *   The Original Code is LASS - Library of Assembled Shared Sources.
    *   
    *   The Initial Developer of the Original Code is Bram de Greve and Tom De Muer.
    *   The Original Developer is the Initial Developer.
    *   
    *   All portions of the code written by the Initial Developer are:
    *   Copyright (C) 2024 the Initial Developer.
    *   All Rights Reserved.
    *   
    *   Contributor(s):
    *
    *   Alternatively, the contents of this file may be used under the terms of the 
    *   GNU General Public License Version 2 or later (the GPL), in which case the 
    *   provisions of GPL are applicable instead of those above.  If you wish to allow use
    *   of your version of this file only under the terms of the GPL and not to allow 
    *   others to use your version of this file under the CPAL, indicate your decision by 
    *   deleting the provisions above and replace them with the notice and other 
    *   provisions required by the GPL License. If you do not delete the provisions above,
    *   a recipient may use your version of this file under either the CPAL or the GPL.
    *   
    *   *** END LICENSE INFORMATION ***
    */
 
#pragma once
 
#include "spat_common.h"
#include "aabb8_tree.h"
 
#include <cstddef>
#include <cstring>
 
namespace lass
{
namespace spat
{
namespace impl::aabb8
{
 
template <typename T, size_t D>
void Aabb8<T, D>::set(size_t k, const Aabb<T, D>& aabb)
{
    for (size_t i = 0; i < D; ++i)
    {
        corners[k][i][0] = aabb.corners[i][0];
        corners[k][i][1] = aabb.corners[i][1];
    }
}
 
template <typename T, size_t D>
int contains(const Aabb8<T, D>& bounds, const Point<T, D>& point, bool hits[4])
{
    int hitmask = 0;
    for (size_t k = 0; k < 8; ++k)
    {
        int hit = 1;
        for (size_t d = 0; d < D; ++d)
        {
            hit &= bounds.corners[k][d][0] <= point.x[d] && point.x[d] <= bounds.corners[k][d][1];
        }
        hitmask |= hit << k;
    }
    return hitmask;
}
 
template <typename T, size_t D>
void overlaps(const Aabb8<T, D>& bounds, const Aabb<T, D>& box, bool hits[4])
{
    int hitmask = 0;
    for (size_t k = 0; k < 8; ++k)
    {
        int hit = 1;
        for (size_t d = 0; d < D; ++d)
        {
            hit &= bounds.corners[k][d][0] <= box.corners[d][1] && box.corners[d][0] <= bounds.corners[k][d][1];
        }
        hitmask |= hit << k;
    }
    return hitmask;
}
 
template <typename T, size_t D>
int intersect(const Aabb8<T, D>& bounds, const Ray<T, D>& ray, T tMin, T tMax, T ts[8])
{
    int hitmask = 0;
    for (size_t k = 0; k < 8; ++k)
    {
        T tmin = tMin;
        T tmax = tMax;
        for (size_t d = 0; d < D; ++d)
        {
            const bool sign = ray.sign[d];
            const T bmin = bounds.corners[k][d][sign];
            const T bmax = bounds.corners[k][d][!sign];
            const T dmin = (bmin - ray.support[d]) * ray.invDir[d];
            const T dmax = (bmax - ray.support[d]) * ray.invDir[d];
            tmin = std::max(dmin, tmin);
            tmax = std::min(dmax, tmax);
        }
        tmax *= 1 + 2 * num::NumTraits<T>::gamma(3);
        hitmask |= tmin <= tmax ? 1 << k : 0;
        ts[k] = tmin;
    }
    return hitmask;
}
 
 
template <typename T, size_t D>
void squaredDistance(const Aabb8<T, D>& bounds, const Point<T, D>& point, T sds[8])
{
    for (size_t k = 0; k < 8; ++k)
    {
        T sd = 0;
        for (size_t i = 0; i < D; ++i)
        {
            const T bmin = bounds.corners[k][i][0];
            const T bmax = bounds.corners[k][i][1];
            const T dmin = bmin - point.x[i];
            const T dmax = point.x[i] - bmax;
            const T d = std::max(dmin, dmax);
            if (d > 0)
            {
                sd += d * d;
            }
        }
        sds[k] = sd;
    }
}
 
#if LASS_HAVE_AVX
 
template <size_t D>
void Aabb8<float, D>::set(size_t k, const Aabb<float, D>& aabb)
{
    for (size_t i = 0; i < D; ++i)
    {
#if LASS_COMPILER_TYPE == LASS_COMPILER_TYPE_MSVC
        corners[i][0].m256_f32[k] = aabb.corners[i][0];
        corners[i][1].m256_f32[k] = aabb.corners[i][1];
#else
        corners[i][0][k] = aabb.corners[i][0];
        corners[i][1][k] = aabb.corners[i][1];
#endif
    }
}
 
template <size_t D>
void Aabb8<double, D>::set(size_t k, const Aabb<double, D>& aabb)
{
    const size_t k0 = k / 4;
    const size_t dk = k % 4;
    for (size_t i = 0; i < D; ++i)
    {
#if LASS_COMPILER_TYPE == LASS_COMPILER_TYPE_MSVC
        corners[i][0][k0].m256d_f64[dk] = aabb.corners[i][0];
        corners[i][1][k0].m256d_f64[dk] = aabb.corners[i][1];
#else
        corners[i][0][k0][dk] = aabb.corners[i][0];
        corners[i][1][k0][dk] = aabb.corners[i][1];
#endif
    }
}
 
template <size_t D>
int contains(const Aabb8<float, D>& bounds, const Point<float, D>& point)
{
    __m256 h = _mm256_castsi256_ps(_mm256_set1_epi32(-1));
    for (size_t d = 0; d < D; ++d)
    {
        const __m256 p = _mm256_set1_ps(point.x[d]);
        const __m256 hmin = _mm256_cmp_ps(bounds.corners[d][0], p, _CMP_LE_OQ);
        const __m256 hmax = _mm256_cmp_ps(p, bounds.corners[d][1], _CMP_LE_OQ);
        h = _mm256_and_ps(h, hmin);
        h = _mm256_and_ps(h, hmax);
    }
    const int hitmask = _mm256_movemask_ps(h);
    return hitmask;
}
 
template <size_t D>
int contains(const Aabb8<double, D>& bounds, const Point<double, D>& point)
{
    __m256d h0 = _mm256_castsi256_pd(_mm256_set1_epi32(-1));
    __m256d h1 = h0;
    for (size_t d = 0; d < D; ++d)
    {
        const __m256d p = _mm256_set1_pd(point.x[d]);
        const __m256d hmin0 = _mm256_cmp_pd(bounds.corners[d][0][0], p, _CMP_LE_OQ);
        const __m256d hmin1 = _mm256_cmp_pd(bounds.corners[d][0][1], p, _CMP_LE_OQ);
        const __m256d hmax0 = _mm256_cmp_pd(p, bounds.corners[d][1][0], _CMP_LE_OQ);
        const __m256d hmax1 = _mm256_cmp_pd(p, bounds.corners[d][1][1], _CMP_LE_OQ);
        h0 = _mm256_and_pd(h0, hmin0);
        h1 = _mm256_and_pd(h1, hmin1);
        h0 = _mm256_and_pd(h0, hmax0);
        h1 = _mm256_and_pd(h1, hmax1);
    }
    const int hitmask = _mm256_movemask_pd(h0) | (_mm256_movemask_pd(h1) << 4);
    return hitmask;
}
 
 
template <size_t D>
int overlaps(const Aabb8<float, D>& bounds, const Aabb<float, D>& box)
{
    __m256 h = _mm256_castsi256_ps(_mm256_set1_epi32(-1));
    for (size_t d = 0; d < D; ++d)
    {
        const __m256 pmin = _mm256_set1_ps(box.corners[d][0]);
        const __m256 pmax = _mm256_set1_ps(box.corners[d][1]);
        const __m256 hmin = _mm256_cmp_ps(bounds.corners[d][0], pmax, _CMP_LE_OQ);
        const __m256 hmax = _mm256_cmp_ps(pmin, bounds.corners[d][1], _CMP_LE_OQ);
        h = _mm256_and_ps(h, hmin);
        h = _mm256_and_ps(h, hmax);
    }
    const int hitmask = _mm256_movemask_ps(h);
    return hitmask;
}
 
 
template <size_t D>
int overlaps(const Aabb8<double, D>& bounds, const Aabb<double, D>& box)
{
    __m256d h0 = _mm256_castsi256_pd(_mm256_set1_epi32(-1));
    __m256d h1 = h0;
    for (size_t d = 0; d < D; ++d)
    {
        const __m256d pmin = _mm256_set1_pd(box.corners[d][0]);
        const __m256d pmax = _mm256_set1_pd(box.corners[d][1]);
        const __m256d hmin0 = _mm256_cmp_pd(bounds.corners[d][0][0], pmax, _CMP_LE_OQ);
        const __m256d hmin1 = _mm256_cmp_pd(bounds.corners[d][0][1], pmax, _CMP_LE_OQ);
        const __m256d hmax0 = _mm256_cmp_pd(pmin, bounds.corners[d][1][0], _CMP_LE_OQ);
        const __m256d hmax1 = _mm256_cmp_pd(pmin, bounds.corners[d][1][1], _CMP_LE_OQ);
        h0 = _mm256_and_pd(h0, hmin0);
        h1 = _mm256_and_pd(h1, hmin1);
        h0 = _mm256_and_pd(h0, hmax0);
        h1 = _mm256_and_pd(h1, hmax1);
    }
    const int hitmask = _mm256_movemask_pd(h0) | (_mm256_movemask_pd(h1) << 4);
    return hitmask;
}
 
template <size_t D>
int intersect(const Aabb8<float, D>& bounds, const Ray<float, D>& ray, float tMin, float tMax, float ts[8])
{
    __m256 tmin = _mm256_set1_ps(tMin);
    __m256 tmax = _mm256_set1_ps(tMax);
    for (size_t d = 0; d < D; ++d)
    {
        const __m256 support = _mm256_set1_ps(ray.support[d]);
        const __m256 invDir = _mm256_set1_ps(ray.invDir[d]);
        const bool sign = ray.sign[d];
        const __m256 bmin = bounds.corners[d][sign];
        const __m256 bmax = bounds.corners[d][!sign];
        const __m256 dmin = _mm256_mul_ps(_mm256_sub_ps(bmin, support), invDir);
        const __m256 dmax = _mm256_mul_ps(_mm256_sub_ps(bmax, support), invDir);
        tmin = _mm256_max_ps(dmin, tmin);
        tmax = _mm256_min_ps(dmax, tmax);
    }
    tmax = _mm256_mul_ps(tmax, _mm256_set1_ps(1 + 2 * num::NumTraits<float>::gamma(3)));
    _mm256_storeu_ps(ts, tmin);
    __m256 h = _mm256_cmp_ps(tmin, tmax, _CMP_LE_OQ);
    const int hitmask = _mm256_movemask_ps(h);
    return hitmask;
}
 
template <size_t D>
int intersect(const Aabb8<double, D>& bounds, const Ray<double, D>& ray, double tMin, double tMax, double ts[8])
{
    __m256d tmin0 = _mm256_set1_pd(tMin);
    __m256d tmax0 = _mm256_set1_pd(tMax);
    __m256d tmin1 = tmin0;
    __m256d tmax1 = tmax0;
    for (size_t d = 0; d < D; ++d)
    {
        const __m256d support = _mm256_set1_pd(ray.support[d]);
        const __m256d invDir = _mm256_set1_pd(ray.invDir[d]);
        const bool sign = ray.sign[d];
        const __m256d bmin0 = bounds.corners[d][sign][0];
        const __m256d bmin1 = bounds.corners[d][sign][1];
        const __m256d bmax0 = bounds.corners[d][!sign][0];
        const __m256d bmax1 = bounds.corners[d][!sign][1];
        const __m256d dmin0 = _mm256_mul_pd(_mm256_sub_pd(bmin0, support), invDir);
        const __m256d dmin1 = _mm256_mul_pd(_mm256_sub_pd(bmin1, support), invDir);
        const __m256d dmax0 = _mm256_mul_pd(_mm256_sub_pd(bmax0, support), invDir);
        const __m256d dmax1 = _mm256_mul_pd(_mm256_sub_pd(bmax1, support), invDir);
        tmin0 = _mm256_max_pd(dmin0, tmin0);
        tmin1 = _mm256_max_pd(dmin1, tmin1);
        tmax0 = _mm256_min_pd(dmax0, tmax0);
        tmax1 = _mm256_min_pd(dmax1, tmax1);
    }
    _mm256_storeu_pd(&ts[0], tmin0);
    _mm256_storeu_pd(&ts[4], tmin1);
    const __m256d f = _mm256_set1_pd(1 + 2 * num::NumTraits<double>::gamma(3));
    tmax0 = _mm256_mul_pd(tmax0, f);
    tmax1 = _mm256_mul_pd(tmax1, f);
    const __m256d h0 = _mm256_cmp_pd(tmin0, tmax0, _CMP_LE_OQ);
    const __m256d h1 = _mm256_cmp_pd(tmin1, tmax1, _CMP_LE_OQ);
    const int hitmask = _mm256_movemask_pd(h0) | (_mm256_movemask_pd(h1) << 4);
    return hitmask;
}
 
 
template <size_t D>
void squaredDistance(const Aabb8<float, D>& bounds, const Point<float, D>& point, float sds[4])
{
    const __m256 zero = _mm256_setzero_ps();
    __m256 sd = _mm256_setzero_ps();
    for (size_t i = 0; i < D; ++i)
    {
        const __m256 p = _mm256_set1_ps(point.x[i]);
        const __m256 dmin = _mm256_sub_ps(bounds.corners[i][0], p);
        const __m256 dmax = _mm256_sub_ps(p, bounds.corners[i][1]);
        const __m256 d = _mm256_max_ps(_mm256_max_ps(dmin, dmax), zero);
        const __m256 d2 = _mm256_mul_ps(d, d);
        sd = _mm256_add_ps(sd, d2);
    }
    _mm256_storeu_ps(sds, sd);
}
 
template <size_t D>
void squaredDistance(const Aabb8<double, D>& bounds, const Point<double, D>& point, double sds[4])
{
    const __m256d zero = _mm256_setzero_pd();
    __m256d sd0 = _mm256_setzero_pd();
    __m256d sd1 = sd0;
    for (size_t i = 0; i < D; ++i)
    {
        const __m256d p = _mm256_set1_pd(point.x[i]);
        const __m256d dmin0 = _mm256_sub_pd(bounds.corners[i][0][0], p);
        const __m256d dmin1 = _mm256_sub_pd(bounds.corners[i][0][1], p);
        const __m256d dmax0 = _mm256_sub_pd(p, bounds.corners[i][1][0]);
        const __m256d dmax1 = _mm256_sub_pd(p, bounds.corners[i][1][1]);
        const __m256d d0 = _mm256_max_pd(_mm256_max_pd(dmin0, dmax0), zero);
        const __m256d d1 = _mm256_max_pd(_mm256_max_pd(dmin1, dmax1), zero);
        sd0 = _mm256_add_pd(sd0, _mm256_mul_pd(d0, d0));
        sd1 = _mm256_add_pd(sd1, _mm256_mul_pd(d1, d1));
    }
    _mm256_storeu_pd(&sds[0], sd0);
    _mm256_storeu_pd(&sds[4], sd1);
}
 
#endif
 
 
}
 
 
// --- public --------------------------------------------------------------------------------------
 
 
 
template <typename O, typename OT, typename SH>
Aabb8Tree<O, OT, SH>::Aabb8Tree(TSplitHeuristics heuristics) :
    SH(std::move(heuristics)),
    aabb_(TObjectTraits::aabbEmpty()),
    nodes_(),
    objects_(),
    end_(new TObjectIterator),
    root_()
{
}
 
 
 
template <typename O, typename OT, typename SH>
Aabb8Tree<O, OT, SH>::Aabb8Tree(TObjectIterator first, TObjectIterator last, TSplitHeuristics heuristics):
    SH(std::move(heuristics)),
    aabb_(TObjectTraits::aabbEmpty()),
    nodes_(),
    objects_(),
    end_(new TObjectIterator(last)),
    root_()
{
    if (first != last)
    {
        std::ptrdiff_t n = last - first;
        if (n < 0)
        {
            LASS_THROW("Aabb8Tree: invalid range");
        }
        if (static_cast<size_t>(n) > static_cast<size_t>(Child::maxNode) + 1)
        {
            LASS_THROW("Aabb8Tree: too many objects");
        }
        TInputs inputs;
        inputs.reserve(static_cast<size_t>(n));
        for (TObjectIterator i = first; i != last; ++i)
        {
            inputs.emplace_back(TObjectTraits::objectAabb(i), i);
        }
        root_ = balance(inputs.begin(), inputs.end(), aabb_);
    }
}
 
 
template <typename O, typename OT, typename SH>
Aabb8Tree<O, OT, SH>::Aabb8Tree(TSelf&& other) noexcept:
    SH(std::forward<TSplitHeuristics>(other)),
    aabb_(std::move(other.aabb_)),
    nodes_(std::move(other.nodes_)),
    objects_(std::move(other.objects_)),
    end_(std::move(other.end_)),
    root_(std::move(other.root_))
{
}
 
 
template <typename O, typename OT, typename SH>
Aabb8Tree<O, OT, SH>& Aabb8Tree<O, OT, SH>::operator=(TSelf&& other) noexcept
{
    TSplitHeuristics::operator=(std::forward<TSplitHeuristics>(other));
    aabb_ = std::move(other.aabb_);
    nodes_ = std::move(other.nodes_);
    objects_ = std::move(other.objects_);
    end_ = std::move(other.end_);
    root_ = std::move(other.root_);
    return *this;
}
 
 
 
/** Reset the tree to an empty one.
 * 
 *  Is equivalent to:
 *  @code
 *  *this = TSelf();
 *  @endcode
 */
template <typename O, typename OT, typename SH>
void Aabb8Tree<O, OT, SH>::reset()
{
    TSelf temp;
    swap(temp);
}
 
 
 
/** Reset the tree to a new one with objects in the range [@a first, @a last)
 * 
 *  Is equivalent to:
 *  @code
 *  *this = TSelf(first, last);
 *  @endcode
 */
template <typename O, typename OT, typename SH>
void Aabb8Tree<O, OT, SH>::reset(TObjectIterator first, TObjectIterator last)
{
    TSelf temp(first, last, static_cast<const SH&>(*this));
    swap(temp);
}
 
 
 
/** Return the total bounding box of all objecs in the tree 
 */
template <typename O, typename OT, typename SH> inline
const typename Aabb8Tree<O, OT, SH>::TAabb
Aabb8Tree<O, OT, SH>::aabb() const
{
    return aabb_;
}
 
 
 
/** Check whether there's any object in the tree that contains @a point.
 */
template <typename O, typename OT, typename SH>
bool Aabb8Tree<O, OT, SH>::contains(const TPoint& point, const TInfo* info) const
{
    if (isEmpty() || !TObjectTraits::aabbContains(aabb_, point))
    {
        return false;
    }
    LASS_ASSERT(!root_.isEmpty());
    return doContains(root_, point, info);
}
 
 
 
/** Find all objects that contain @a point.
    */
template <typename O, typename OT, typename SH>
template <typename OutputIterator> inline
OutputIterator Aabb8Tree<O, OT, SH>::find(const TPoint& point, OutputIterator result, const TInfo* info) const
{
    if (isEmpty() || !TObjectTraits::aabbContains(aabb_, point))
    {
        return result;
    }
    LASS_ASSERT(!root_.isEmpty());
    return doFind(root_, point, result, info);
}
 
 
 
/** Find all objects that intersect with @a box.
 */
template <typename O, typename OT, typename SH>
template <typename OutputIterator> inline
OutputIterator Aabb8Tree<O, OT, SH>::find(const TAabb& box, OutputIterator result, const TInfo* info) const
{
    if (isEmpty() || !TObjectTraits::aabbIntersects(aabb_, box))
    {
        return result;
    }
    LASS_ASSERT(!root_.isEmpty());
    return doFind(root_, box, result, info);
}
 
 
 
/** Find all objects that have an intersection with @a ray in the interval [tMin, tMax].
 */
template <typename O, typename OT, typename SH>
template <typename OutputIterator>
OutputIterator Aabb8Tree<O, OT, SH>::find(const TRay& ray, TParam tMin, TParam tMax, OutputIterator result, const TInfo* info) const
{
    const TVector invDir = TObjectTraits::vectorReciprocal(TObjectTraits::rayDirection(ray));
    if (isEmpty() || !volumeIntersects(aabb_, ray, invDir, tMin, tMax))
    {
        return result;
    }
    LASS_ASSERT(!root_.isEmpty());
    return doFind(root_, ray, invDir, tMin, tMax, result, info);
}
 
 
 
/** Find the first object that is intersected by @a ray, so that t >= tMin and t is minimized.
 */
template <typename O, typename OT, typename SH>
typename Aabb8Tree<O, OT, SH>::TObjectIterator
Aabb8Tree<O, OT, SH>::intersect(const TRay& ray, TReference t, TParam tMin, const TInfo* info) const
{
    const TVector dir = TObjectTraits::rayDirection(ray);
    const TVector invDir = TObjectTraits::vectorReciprocal(dir);
    TValue tRoot;
    if (isEmpty() || !volumeIntersect(aabb_, ray, invDir, tRoot, tMin))
    {
        return *end_;
    }
    LASS_ASSERT(!root_.isEmpty());
    return doIntersect(root_, ray, invDir, t, tMin, info);
}
 
 
 
/** Check whether there's any object in the tree that is intersected by @a ray in the interval [tMin, tMax].
 */
template <typename O, typename OT, typename SH>
bool Aabb8Tree<O, OT, SH>::intersects(
    const TRay& ray, TParam tMin, TParam tMax, const TInfo* info) const
{
    LASS_ASSERT(tMax > tMin || (num::isInf(tMin) && num::isInf(tMax)));
    const TVector dir = TObjectTraits::rayDirection(ray);
    const TVector invDir = TObjectTraits::vectorReciprocal(dir);
    if (isEmpty() || !volumeIntersects(aabb_, ray, invDir, tMin, tMax))
    {
        return false;
    }
    LASS_ASSERT(!root_.isEmpty());
    return doIntersects(root_, ray, invDir, tMin, tMax, info);
}
 
 
 
/** Find the object that is closest to @a point.
 */
template <typename O, typename OT, typename SH>
const typename Aabb8Tree<O, OT, SH>::Neighbour
Aabb8Tree<O, OT, SH>::nearestNeighbour(const TPoint& point, const TInfo* info) const
{
    Neighbour nearest(*end_, std::numeric_limits<TValue>::infinity());
    if (isEmpty())
    {
        return nearest;
    }
    LASS_ASSERT(!root_.isEmpty());
    doNearestNeighbour(root_, point, info, nearest);
    return nearest;
}
 
 
 
/** Find all objects that are within @a maxRadius from @a center, up to @a maxCount.
 * 
 *  If more than @a maxCount objects within @a maxRadius are found, then the closest
 *  @a maxCount objects are returned.
 * 
 *  @param center The center of the search sphere.
 *  @param maxRadius The maximum radius of the search sphere.
 *  @param maxCount The maximum number of objects to find.
 *  @param first An output iterator to a container of Neighbour objects that will be filled with the found objects.
 *  @param info Optional information to pass to the object traits.
 * 
 *  @return The output iterator @a last, pointing to the first element beyond the last found object.
 * 
 *  The range [@a first, @a last) will contain the found objects and form a heap, so that @a first points to the
 *  farthest object from @a center: its squared distance to @a center is the largest in the range, and gives you
 *  the effective maximum radius of the search sphere.
 * 
 *  @note The output iterator must be able to handle at least @a maxCount objects.
 */
template <class O, class OT, typename SH>
template<typename RandomIterator>
RandomIterator
Aabb8Tree<O, OT, SH>::rangeSearch(
    const TPoint& center, TParam maxRadius, size_t maxCount, RandomIterator first, const TInfo* info) const
{
    if (isEmpty() || maxRadius == 0)
    {
        return first;
    }
    TValue squaredRadius = maxRadius * maxRadius;
    LASS_ASSERT(!root_.isEmpty());
    return doRangeSearch(root_, center, squaredRadius, maxCount, first, first, info);
}
 
 
 
/** Returns true if there are no objects in the tree.
 */
template <typename O, typename OT, typename SH>
bool Aabb8Tree<O, OT, SH>::isEmpty() const
{
    return objects_.empty();
}
 
 
 
/** Returns an iterator not pointing to any object, used to indicate when no object is found.
 * 
 *  This iterator can be used as the return value of the find functions when no object is found.
 *  You can compare those return values to the end() function to check if an object was found.
 */
template <typename O, typename OT, typename SH>
const typename Aabb8Tree<O, OT, SH>::TObjectIterator
Aabb8Tree<O, OT, SH>::end() const
{
    return *end_;
}
 
 
 
/** Swap the contents of this tree with another.
 * 
 *  Is equivalent to:
 *  @code
 *  TSelf tmp = std::move(other);
 *  other = std::move(*this);
 *  *this = std::move(tmp);
 *  @endcode
 */
template <typename O, typename OT, typename SH>
void Aabb8Tree<O, OT, SH>::swap(TSelf& other)
{
    SH::swap(other);
    std::swap(aabb_, other.aabb_);
    nodes_.swap(other.nodes_);
    objects_.swap(other.objects_);
    end_.swap(other.end_);
    std::swap(root_, other.root_);
}
 
 
 
// --- private -------------------------------------------------------------------------------------
 
template <typename O, typename OT, typename SH>
typename Aabb8Tree<O, OT, SH>::Child
Aabb8Tree<O, OT, SH>::balance(TInputIterator first, TInputIterator last, TAabb& bounds)
{
    auto split = [this](TInputIterator first, TInputIterator last) -> TSplitInfo
    {
        auto s = TSplitHeuristics::template split<OT>(first, last);
        if (s.isLeaf() && (last - first) > Child::maxCount)
        {
            // we have to many objects, but SAH didn't split them.
            // force a median split
            s = forceSplit(s.aabb);
        }
        return s;
    };
 
    auto partition = [](TInputIterator first, TInputIterator last, const TSplitInfo& s) -> TInputIterator
    {
        TInputIterator middle = std::partition(first, last, impl::Splitter<TObjectTraits>(s));
        if (middle == first || middle == last)
        {
            const std::ptrdiff_t halfSize = (last - first) / 2;
            LASS_ASSERT(halfSize > 0);
            middle = first + halfSize;
            std::nth_element(first, middle, last, impl::LessAxis<TObjectTraits>(s.axis));
        }
        return middle;
    };
 
    auto makeLeaf = [this](TInputIterator first, TInputIterator last) -> Child
    {
        const auto count = last - first;
        LASS_ASSERT(count <= Child::maxCount);
        const TIndex frst = static_cast<TIndex>(objects_.size());
        LASS_ASSERT(frst >= 0 && frst <= Child::maxObject);
        for (TInputIterator i = first; i != last; ++i)
        {
            objects_.push_back(i->object);
        }
        LASS_ASSERT(objects_.size() <= static_cast<size_t>(Child::maxObject) + 1);
        return Child(frst, static_cast<TIndex>(count));
    };
 
    auto splitQuadrant = [this, makeLeaf, partition](TIndex index, size_t quadrant, TInputIterator first, TInputIterator last)
    {
        const size_t octant0 = 2 * quadrant;
        const size_t octant1 = 2 * quadrant + 1;
        auto s = TSplitHeuristics::template split<OT>(first, last);
        if (s.isLeaf())
        {
            Node& node = nodes_[index];
            node.children[octant0] = makeLeaf(first, last);
            node.children[octant1] = Child();
            node.bounds.set(octant0, makeAabb(s.aabb));
            node.bounds.set(octant1, makeAabb(TObjectTraits::aabbEmpty()));
            node.axis[quadrant + 3] = 0;
        }
        else
        {
            const TInputIterator middle = partition(first, last, s);
            TAabb bounds0, bounds1;
            const Child child0 = balance(first, middle, bounds0);
            const Child child1 = balance(middle, last, bounds1);
            Node& node = nodes_[index];
            node.children[octant0] = child0;
            node.children[octant1] = child1;
            node.bounds.set(octant0, makeAabb(bounds0));
            node.bounds.set(octant1, makeAabb(bounds1));
            node.axis[quadrant + 3] = static_cast<TAxis>(s.axis);
        }
    };
 
    auto splitHalf = [this, makeLeaf, partition, splitQuadrant](TIndex index, size_t half, TInputIterator first, TInputIterator last)
    {
        auto s = TSplitHeuristics::template split<OT>(first, last);
        if (s.isLeaf())
        {
            Node& node = nodes_[index];
            node.children[4 * half] = makeLeaf(first, last);
            node.children[4 * half + 1] = Child();
            node.children[4 * half + 2] = Child();
            node.children[4 * half + 3] = Child();
            node.bounds.set(4 * half, makeAabb(s.aabb));
            node.bounds.set(4 * half + 1, makeAabb(TObjectTraits::aabbEmpty()));
            node.bounds.set(4 * half + 2, makeAabb(TObjectTraits::aabbEmpty()));
            node.bounds.set(4 * half + 3, makeAabb(TObjectTraits::aabbEmpty()));
            node.axis[half + 1] = 0;
        }
        else
        {
            const TInputIterator middle = partition(first, last, s);
            splitQuadrant(index, 2 * half, first, middle);
            splitQuadrant(index, 2 * half + 1, middle, last);
            nodes_[index].axis[half + 1] = static_cast<TAxis>(s.axis);
        }
    };
 
    if (first == last)
    {
        bounds = TObjectTraits::aabbEmpty();
        return Child();
    }
 
    auto s = split(first, last);
    bounds = s.aabb;
    if (s.isLeaf())
    {
        return makeLeaf(first, last);
    }
    LASS_ASSERT(!s.isLeaf());
 
    LASS_ASSERT(nodes_.size() <= static_cast<size_t>(Child::maxNode));
    const TIndex index = static_cast<TIndex>(nodes_.size());
    nodes_.emplace_back();
 
    nodes_[index].axis[0] = static_cast<TAxis>(s.axis);
 
    const TInputIterator middle = partition(first, last, s);
    splitHalf(index, 0, first, middle);
    splitHalf(index, 1, middle, last);
    return Child(index);
}
 
 
 
template <typename O, typename OT, typename SH>
typename Aabb8Tree<O, OT, SH>::TSplitInfo
Aabb8Tree<O, OT, SH>::forceSplit(const TAabb& bounds)
{
    const TPoint min = TObjectTraits::aabbMin(bounds);
    const TPoint max = TObjectTraits::aabbMax(bounds);
    size_t axis = 0;
    TValue maxSize = TObjectTraits::coord(max, 0) - TObjectTraits::coord(min, 0);
    for (size_t k = 1; k < TObjectTraits::dimension; ++k)
    {
        const TValue size = TObjectTraits::coord(max, k) - TObjectTraits::coord(min, k);
        if (size > maxSize)
        {
            axis = k;
            maxSize = size;
        }
    }
    const TValue x = (TObjectTraits::coord(min, axis) + TObjectTraits::coord(max, axis)) / 2;
    return TSplitInfo(bounds, x, axis);
}
 
 
 
template <typename O, typename OT, typename SH>
bool Aabb8Tree<O, OT, SH>::doContains(Child root, const TPoint& point, const TInfo* info) const
{
    const auto pnt = makePoint(point);
 
    Child stack[stackSize_];
    TIndex stackSize = 0;
    stack[stackSize++] = root;
    while (stackSize > 0)
    {
        const Child index = stack[--stackSize];
        LASS_ASSERT(!index.isEmpty());
 
        if (index.isLeaf())
        {
            const TIndex first = index.first();
            const TIndex last = first + index.count();
            for (TIndex i = first; i != last; ++i)
            {
                LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
                if (TObjectTraits::objectContains(objects_[i], point, info))
                {
                    return true;
                }
            }
            continue;
        }
 
        LASS_ASSERT(index.isInternal());
        LASS_ASSERT(index.node() < nodes_.size());
        LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
        const Node& node = nodes_[index.node()];
 
        const int hits = impl::aabb8::contains(node.bounds, pnt);
        for (size_t k = 0; k < 8; ++k)
        {
            if (!(hits & (1 << k)))
            {
                continue;
            }
            const Child child = node.children[k];
            if (child.isEmpty())
            {
                continue;
            }
            if (stackSize < stackSize_)
            {
                stack[stackSize++] = child;
            }
            else if (doContains(child, point, info))
            {
                return true;
            }
        }
    }
    return false;
}
 
 
 
template <typename O, typename OT, typename SH>
template <typename OutputIterator>
OutputIterator Aabb8Tree<O, OT, SH>::doFind(Child root, const TPoint& point, OutputIterator result, const TInfo* info) const
{
    const auto pnt = makePoint(point);
 
    Child stack[stackSize_];
    TIndex stackSize = 0;
    stack[stackSize++] = root;
    while (stackSize > 0)
    {
        const Child index = stack[--stackSize];
        LASS_ASSERT(!index.isEmpty());
 
        if (index.isLeaf())
        {
            const TIndex first = index.first();
            const TIndex last = first + index.count();
            for (TIndex i = first; i != last; ++i)
            {
                LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
                if (TObjectTraits::objectContains(objects_[i], point, info))
                {
                    *result++ = objects_[i];
                }
            }
            continue;
        }
 
        LASS_ASSERT(index.isInternal());
        LASS_ASSERT(index.node() < nodes_.size());
        LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
        const Node& node = nodes_[index.node()];
 
        const int hits = impl::aabb8::contains(node.bounds, pnt);
        for (size_t k = 0; k < 8; ++k)
        {
            if (!(hits & (1 << k)))
            {
                continue;
            }
            const Child child = node.children[k];
            if (child.isEmpty())
            {
                continue;
            }
            if (stackSize < stackSize_)
            {
                stack[stackSize++] = child;
            }
            else
            {
                result = doFind(child, point, result, info);
            }
        }
    }
    return result;
}
 
 
template <typename O, typename OT, typename SH>
template <typename OutputIterator>
OutputIterator Aabb8Tree<O, OT, SH>::doFind(Child root, const TAabb& box, OutputIterator result, const TInfo* info) const
{
    const auto bx = makeAabb(box);
 
    Child stack[stackSize_];
    TIndex stackSize = 0;
    stack[stackSize++] = root;
    while (stackSize > 0)
    {
        const Child index = stack[--stackSize];
        LASS_ASSERT(!index.isEmpty());
 
        if (index.isLeaf())
        {
            const TIndex first = index.first();
            const TIndex last = first + index.count();
            for (TIndex i = first; i != last; ++i)
            {
                LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
                if (TObjectTraits::objectIntersects(objects_[i], box, info))
                {
                    *result++ = objects_[i];
                }
            }
            continue;
        }
 
        LASS_ASSERT(index.isInternal());
        LASS_ASSERT(index.node() < nodes_.size());
        LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
        const Node& node = nodes_[index.node()];
 
        const int hits = impl::aabb8::overlaps(node.bounds, bx);
        for (size_t k = 0; k < 8; ++k)
        {
            if (!(hits & (1 << k)))
            {
                continue;
            }
            const Child child = node.children[k];
            if (child.isEmpty())
            {
                continue;
            }
            if (stackSize < stackSize_)
            {
                stack[stackSize++] = child;
            }
            else
            {
                result = doFind(child, box, result, info);
            }
        }
    }
    return result;
}
 
 
 
template <typename O, typename OT, typename SH>
template <typename OutputIterator>
OutputIterator Aabb8Tree<O, OT, SH>::doFind(Child root, const TRay& ray, const TVector& invDir, TParam tMin, TParam tMax, OutputIterator result, const TInfo* info) const
{
    const auto r = makeRay(ray);
 
    Child stack[stackSize_];
    TIndex stackSize = 0;
    stack[stackSize++] = root;
    while (stackSize > 0)
    {
        const Child index = stack[--stackSize];
        LASS_ASSERT(!index.isEmpty());
 
        if (index.isLeaf())
        {
            const TIndex first = index.first();
            const TIndex last = first + index.count();
            for (TIndex i = first; i != last; ++i)
            {
                LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
                if (TObjectTraits::objectIntersects(objects_[i], ray, tMin, tMax, info))
                {
                    *result++ = objects_[i];
                }
            }
            continue;
        }
 
        LASS_ASSERT(index.isInternal());
        LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
        LASS_ASSERT(index.node() < nodes_.size());
        const Node& node = nodes_[index.node()];
 
        TValue ts[8];
        const int hits = impl::aabb8::intersect(node.bounds, r, tMin, tMax, ts);
        for (size_t k = 0; k < 8; ++k)
        {
            const Child child = node.children[k];
            if (!(hits & (child.isEmpty() ? 0 : 1) << k))
            {
                continue;
            }
            if (stackSize < stackSize_)
            {
                stack[stackSize++] = child;
            }
            else
            {
                result = doFind(child, ray, invDir, tMin, tMax, result, info);
            }
        }
    }
    return result;
}
 
 
 
 
 
template <typename O, typename OT, typename SH>
typename Aabb8Tree<O, OT, SH>::TObjectIterator
Aabb8Tree<O, OT, SH>::doIntersect(Child root, const TRay& ray, const TVector& invDir, TReference t, TParam tMin, const TInfo* info) const
{
    const auto r = makeRay(ray);
#if LASS_HAVE_AVX
    __m128i signs[dimension];
    for (size_t i = 0; i < dimension; ++i)
    {
        signs[i] = _mm_set1_epi32(r.sign[i] ? -1 : 0);
    }
#endif
 
    struct Visit
    {
        TValue tNear;
        Child index;
        Visit() = default;
        Visit(Child index, TValue tNear): tNear(tNear), index(index) {}
    };
    Visit stack[stackSize_];
    size_t stackSize = 0;
    stack[stackSize++] = Visit(root, tMin);
    TValue tBest = TNumTraits::infinity;
    TObjectIterator best = *end_;
    while (stackSize > 0)
    {
        const Visit visit = stack[--stackSize];
        if (tBest <= visit.tNear)
        {
            continue; // we already have a closer hit than what this node can offer
        }
 
        const Child index = visit.index;
        LASS_ASSERT(!index.isEmpty());
 
        if (index.isLeaf())
        {
            const TIndex first = index.first();
            const TIndex last = first + index.count();
            for (TIndex i = first; i != last; ++i)
            {
                LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
                TValue tCandidate = 0;
                if (TObjectTraits::objectIntersect(objects_[i], ray, tCandidate, tMin, info))
                {
                    if (best == *end_ || tCandidate < tBest)
                    {
                        tBest = tCandidate;
                        best = objects_[i];
                    }
                }
            }
            continue;
        }
 
        LASS_ASSERT(index.isInternal());
        LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
        LASS_ASSERT(index.node() < nodes_.size());
        const Node& node = nodes_[index.node()];
 
        TValue ts[8];
        const int hits = impl::aabb8::intersect(node.bounds, r, tMin, TNumTraits::infinity, ts);
 
#if 0 && LASS_HAVE_AVX
        alignas(16) int order[4];
        {
            auto order_ = _mm_set_epi32(3, 2, 1, 0);
            const auto m0 = _mm_set_epi32(-1, -1, -1, -1);
            const auto m1 = _mm_set_epi32(0, 0, -1, -1);
            const auto m2 = _mm_set_epi32(-1, -1, 0, 0);
            const auto mask0 = _mm_and_si128(m0, signs[node.axis[0]]);
            const auto mask1 = _mm_and_si128(m1, signs[node.axis[1]]);
            const auto mask2 = _mm_and_si128(m2, signs[node.axis[2]]);
            const auto mask21 = _mm_or_si128(mask2, mask1);
            const auto order21 = _mm_shuffle_epi32(order_, 0xB1);
            order_ = _mm_blendv_epi8(order_, order21, mask21);
            const auto order0 = _mm_shuffle_epi32(order_, 0x4E);
            order_ = _mm_blendv_epi8(order_, order0, mask0);
            LASS_ASSERT(sizeof(order) == sizeof(order_));
            memcpy(order, &order_, sizeof(order_));
        }
#else
        int order[8] = { 0, 1, 2, 3, 4, 5, 6, 7 };
        if (r.sign[node.axis[3]])
        {
            std::swap(order[0], order[1]);
        }
        if (r.sign[node.axis[4]])
        {
            std::swap(order[2], order[3]);
        }
        if (r.sign[node.axis[5]])
        {
            std::swap(order[4], order[5]);
        }
        if (r.sign[node.axis[6]])
        {
            std::swap(order[6], order[7]);
        }
        if (r.sign[node.axis[1]])
        {
            std::swap(order[0], order[2]);
            std::swap(order[1], order[3]);
        }
        if (r.sign[node.axis[2]])
        {
            std::swap(order[4], order[6]);
            std::swap(order[5], order[7]);
        }
        if (r.sign[node.axis[0]])
        {
            std::swap(order[0], order[4]);
            std::swap(order[1], order[5]);
            std::swap(order[2], order[6]);
            std::swap(order[3], order[7]);
        }
#endif
 
        // push in reverse order, so that the first child is on top of the stack
        for (int k = 7; k >= 0; --k)
        {
            const auto o = order[k];
            const Child child = node.children[o];
            if (!(hits & (child.isEmpty() ? 0 : 1) << o))
            {
                continue;
            }
            if (stackSize < stackSize_)
            {
                stack[stackSize++] = Visit(child, ts[o]);
            }
            else
            {
                TValue tb;
                TObjectIterator b = doIntersect(child, ray, invDir, tb, tMin, info);
                if (best != *end_ && tb < tBest)
                {
                    best = b;
                    tBest = tb;
                }
            }
        }
    }
    if (best != *end_)
    {
        t = tBest;
    }
    return best;
}
 
 
 
template <typename O, typename OT, typename SH>
bool Aabb8Tree<O, OT, SH>::doIntersects(Child root, const TRay& ray, const TVector& invDir, TParam tMin, TParam tMax, const TInfo* info) const
{
    const auto r = makeRay(ray);
 
    Child stack[stackSize_];
    TIndex stackSize = 0;
    stack[stackSize++] = root;
    while (stackSize > 0)
    {
        const Child index = stack[--stackSize];
        LASS_ASSERT(!index.isEmpty());
 
        if (index.isLeaf())
        {
            const TIndex first = index.first();
            const TIndex last = first + index.count();
            for (TIndex i = first; i != last; ++i)
            {
                LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
                if (TObjectTraits::objectIntersects(objects_[i], ray, tMin, tMax, info))
                {
                    return true;
                }
            }
            continue;
        }
 
        LASS_ASSERT(index.isInternal());
        LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
        LASS_ASSERT(index.node() < nodes_.size());
        const Node& node = nodes_[index.node()];
 
        TValue ts[8];
        const int hits = impl::aabb8::intersect(node.bounds, r, tMin, tMax, ts);
        for (size_t k = 0; k < 8; ++k)
        {
            const Child child = node.children[k];
            if (!(hits & (child.isEmpty() ? 0 : 1) << k))
            {
                continue;
            }
            if (stackSize < stackSize_)
            {
                stack[stackSize++] = child;
            }
            else if (doIntersects(child, ray, invDir, tMin, tMax, info))
            {
                return true;
            }
        }
    }
    return false;
}
 
 
namespace impl::aabb8
{
 
template <typename T>
void sort8(size_t order[8], const T values[8])
{
    std::sort(order, order + 8, [&values](size_t a, size_t b) { return values[a] < values[b]; });
}
 
}
 
 
 
template <typename O, typename OT, typename SH>
void Aabb8Tree<O, OT, SH>::doNearestNeighbour(
    Child root, const TPoint& target, const TInfo* info, Neighbour& nearest) const
{
    const auto pnt = makePoint(target);
 
    struct Visit
    {
        TValue squaredDistance;
        Child index;
        Visit() = default;
        Visit(Child index, TValue squaredDistance): squaredDistance(squaredDistance), index(index) {}
    };
    Visit stack[stackSize_];
    size_t stackSize = 0;
    stack[stackSize++] = Visit(root, 0);
    while (stackSize > 0)
    {
        LASS_ASSERT(nearest.squaredDistance() >= 0);
        const Visit visit = stack[--stackSize];
        if (nearest.squaredDistance() <= visit.squaredDistance)
        {
            continue; // we already have a closer neighbour than what this node can offer
        }
 
        const Child index = visit.index;
        LASS_ASSERT(!index.isEmpty());
 
        if (index.isLeaf())
        {
            const auto first = index.first();
            const auto last = first + index.count();
            for (auto i = first; i != last; ++i)
            {
                LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
                const TValue squaredDistance = TObjectTraits::objectSquaredDistance(objects_[i], target, info);
                if (squaredDistance < nearest.squaredDistance())
                {
                    nearest = Neighbour(objects_[i], squaredDistance);
                }
            }
            continue;
        }
 
        LASS_ASSERT(index.isInternal());
        LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
        LASS_ASSERT(index.node() < nodes_.size());
        const Node& node = nodes_[index.node()];
 
        TValue squaredDistances[8];
        impl::aabb8::squaredDistance(node.bounds, pnt, squaredDistances);
 
        size_t order[8] = { 0, 1, 2, 3, 4, 5, 6, 7 };
        impl::aabb8::sort8(order, squaredDistances);
 
        // push in reverse order, so that the first child is on top of the stack
        for (int k = 7; k >= 0; --k)
        {
            const size_t o = order[k];
            const Child child = node.children[o];
            if (child.isEmpty())
            {
                continue;
            }
            if (stackSize < stackSize_)
            {
                stack[stackSize++] = Visit(child, squaredDistances[o]);
            }
            else
            {
                doNearestNeighbour(child, target, info, nearest);
            }
        }
    }
}
 
 
 
template <typename O, typename OT, typename SH>
template <typename RandomIterator>
RandomIterator
Aabb8Tree<O, OT, SH>::doRangeSearch(
    Child root, const TPoint& center, TReference squaredRadius, size_t maxCount, RandomIterator first, RandomIterator last, const TInfo* info) const
{
    const auto pnt = makePoint(center);
 
    struct Visit
    {
        TValue squaredDistance;
        Child index;
        Visit() = default;
        Visit(Child index, TValue squaredDistance): squaredDistance(squaredDistance), index(index) {}
    };
    Visit stack[stackSize_];
    size_t stackSize = 0;
    stack[stackSize++] = Visit(root, 0);
    while (stackSize > 0)
    {
        LASS_ASSERT(squaredRadius >= 0);
        const Visit visit = stack[--stackSize];
        if (squaredRadius <= visit.squaredDistance)
        {
            continue; // node is entirely outside range
        }
 
        const Child index = visit.index;
        LASS_ASSERT(!index.isEmpty());
 
        if (index.isLeaf())
        {
            const auto frst = index.first();
            const auto lst = frst + index.count();
            for (auto i = frst; i != lst; ++i)
            {
                LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_VISIT_OBJECT;
                const TValue squaredDistance = TObjectTraits::objectSquaredDistance(objects_[i], center, info);
                if (squaredDistance < squaredRadius)
                {
                    *last++ = Neighbour(objects_[i], squaredDistance);
                    std::push_heap(first, last);
                    LASS_ASSERT(last >= first);
                    if (static_cast<size_t>(last - first) > maxCount)
                    {
                        std::pop_heap(first, last);
                        --last;
                        squaredRadius = first->squaredDistance();
                    }
                }
            }
            continue;
        }
 
        LASS_ASSERT(index.isInternal());
        LASS_SPAT_OBJECT_TREES_DIAGNOSTICS_INIT_NODE(TInfo, info);
        LASS_ASSERT(index.node() < nodes_.size());
        const Node& node = nodes_[index.node()];
 
        TValue squaredDistances[8];
        impl::aabb8::squaredDistance(node.bounds, pnt, squaredDistances);
 
        size_t order[8] = { 0, 1, 2, 3, 4, 5, 6, 7 };
        impl::aabb8::sort8(order, squaredDistances);
 
        // push in reverse order, so that the first child is on top of the stack
        for (int k = 7; k >= 0; --k)
        {
            size_t o = order[k];
            const Child child = node.children[o];
            if (child.isEmpty())
            {
                continue;
            }
            if (stackSize < stackSize_)
            {
                stack[stackSize++] = Visit(child, squaredDistances[o]);
            }
            else
            {
                last = doRangeSearch(child, center, squaredRadius, maxCount, first, last, info);
            }
        }
    }
 
    return last;
}
 
 
 
template <typename O, typename OT, typename SH>
bool Aabb8Tree<O, OT, SH>::volumeIntersect(const TAabb& box, const TRay& ray, const TVector& invDir, TReference t, TParam tMin) const
{
    if (TObjectTraits::aabbContains(box, TObjectTraits::rayPoint(ray, tMin)))
    {
        t = tMin;
        return true;
    }
    return TObjectTraits::aabbIntersect(box, ray, invDir, t, tMin);
}
 
 
 
template <typename O, typename OT, typename SH>
bool Aabb8Tree<O, OT, SH>::volumeIntersects(const TAabb& box, const TRay& ray, const TVector& invDir, TParam tMin, TParam tMax) const
{
    TValue t = 0;
    return volumeIntersect(box, ray, invDir, t, tMin) && t <= tMax;
}
 
 
 
template <typename O, typename OT, typename SH>
typename Aabb8Tree<O, OT, SH>::TPoint_
Aabb8Tree<O, OT, SH>::makePoint(const TPoint& point)
{
    TPoint_ result;
    for (size_t d = 0; d < dimension; ++d)
    {
        result.x[d] = OT::coord(point, d);
    }
    return result;
}
 
 
 
template <typename O, typename OT, typename SH>
typename Aabb8Tree<O, OT, SH>::TRay_
Aabb8Tree<O, OT, SH>::makeRay(const TRay& ray)
{
    TRay_ result;
    const auto support = OT::raySupport(ray);
    const auto direction = OT::rayDirection(ray);
    const auto invDir = TObjectTraits::vectorReciprocal(direction);
    for (size_t d = 0; d < dimension; ++d)
    {
        result.support[d] = OT::coord(support, d);
        result.invDir[d] = OT::coord(invDir, d);
        result.sign[d] = std::signbit(OT::coord(direction, d));
    }
    return result;
}
 
 
 
template <typename O, typename OT, typename SH>
typename Aabb8Tree<O, OT, SH>::TAabb_
Aabb8Tree<O, OT, SH>::makeAabb(const TAabb& aabb)
{
    TAabb_ result;
    const auto min = OT::aabbMin(aabb);
    const auto max = OT::aabbMax(aabb);
    for (size_t d = 0; d < dimension; ++d)
    {
        result.corners[d][0] = OT::coord(min, d);
        result.corners[d][1] = OT::coord(max, d);
    }
    return result;
}
 
 
 
}
 
}