octree.h

Go to the documentation of this file.

/*
    Sheep - A Rigid Body Dynamics Engine
    Copyright (C) 2001-2004 Francois Beaune
    Contact: http://toxicengine.sourceforge.net/

    This file is part of Sheep.

    Sheep is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    Sheep is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with Sheep; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#ifndef SHEEP_MATH_OCTREE_H
#define SHEEP_MATH_OCTREE_H

#include "common/misc/minmax.h"
#include "aabb3.h"
#include "point3.h"
#include "real.h"
#include "vector3.h"

#include <cassert>
#include <vector>

namespace sheep {

    inline
    unsigned int choose_next(Real x, Real y, Real z,
                             unsigned int a, unsigned int b, unsigned int c)
    {
        if(x < y) {
            if(x < z) return a;
            else return c;
        } else {
            if(y < z) return b;
            else return c;
        }
    }

    template<typename Item>
    struct OctreeNode {
        AABB3 m_voxel;              //!< The volume occupied by the node.
        Point3 m_center;            //!< The center of this volume.

        bool m_is_leaf;             //!< Indicates whether this node is a leaf or not.
        OctreeNode *m_children[8];  //!< The children of this node.

        typedef std::vector<Item> ItemVector;
        typedef typename ItemVector::const_iterator ItemVectorConstIt;

        ItemVector m_items;         //!< Items owned by this node, if this node is a leaf.

        OctreeNode();
        OctreeNode(const AABB3 &voxel);
        ~OctreeNode();

        template<typename ItemVoxelIntersector>
        void Subdivide(
            int pop_threshold,
            int remaining_subdiv,
            ItemVoxelIntersector &intersector)
        {
            assert(pop_threshold > 0);
            assert(remaining_subdiv > 0);

            if(m_items.size() <= static_cast<typename ItemVector::size_type>(pop_threshold))
                return;

            m_is_leaf = false;

            m_children[0] = new OctreeNode<Item>(
                AABB3(m_voxel.m_min, m_center));

            m_children[1] = new OctreeNode<Item>(
                AABB3(
                    Point3(m_voxel.m_min.m_x, m_voxel.m_min.m_y, m_center.m_z),
                    Point3(m_center.m_x, m_center.m_y, m_voxel.m_max.m_z)));

            m_children[2] = new OctreeNode<Item>(
                AABB3(
                    Point3(m_voxel.m_min.m_x, m_center.m_y, m_voxel.m_min.m_z),
                    Point3(m_center.m_x, m_voxel.m_max.m_y, m_center.m_z)));

            m_children[3] = new OctreeNode<Item>(
                AABB3(
                    Point3(m_voxel.m_min.m_x, m_center.m_y, m_center.m_z),
                    Point3(m_center.m_x, m_voxel.m_max.m_y, m_voxel.m_max.m_z)));

            m_children[4] = new OctreeNode<Item>(
                AABB3(
                    Point3(m_center.m_x, m_voxel.m_min.m_y, m_voxel.m_min.m_z),
                    Point3(m_voxel.m_max.m_x, m_center.m_y, m_center.m_z)));

            m_children[5] = new OctreeNode<Item>(
                AABB3(
                    Point3(m_center.m_x, m_voxel.m_min.m_y, m_center.m_z),
                    Point3(m_voxel.m_max.m_x, m_center.m_y, m_voxel.m_max.m_z)));

            m_children[6] = new OctreeNode<Item>(
                AABB3(
                    Point3(m_center.m_x, m_center.m_y, m_voxel.m_min.m_z),
                    Point3(m_voxel.m_max.m_x, m_voxel.m_max.m_y, m_center.m_z)));

            m_children[7] = new OctreeNode<Item>(
                AABB3(m_center, m_voxel.m_max));

            for(ItemVectorConstIt i = m_items.begin(), e = m_items.end(); i != e; ++i) {
                const Item &item = *i;

                for(int j = 0; j < 8; ++j) {
                    if(intersector.Intersect(item, m_children[j]->m_voxel))
                        m_children[j]->m_items.push_back(item);
                }
            }

            m_items.clear();

            if(--remaining_subdiv == 0)
                return;

            for(int i = 0; i < 8; ++i)
                m_children[i]->Subdivide(pop_threshold, remaining_subdiv, intersector);
        }

        template<typename Visitor>
        bool Trace(
            const Point3 &origin,
            Real tx0, Real ty0, Real tz0,
            Real tx1, Real ty1, Real tz1,
            unsigned int a,
            unsigned int b,
            Visitor &visitor) const
        {
            // Exit if the ray does not intersect the voxel.
            if(tx1 < 0.0 || ty1 < 0.0 || tz1 < 0.0)
                return false;   // continue octree traversal

            // If it is a leaf, visit it.
            if(m_is_leaf) {
                Real t0 = max<Real>(tx0, ty0, tz0);
                Real t1 = min<Real>(tx1, ty1, tz1);

                return visitor.Trace(this, t0, t1);
            }

            // Compute the intersection between the ray and the three middle planes.

            const Real &x0 = m_voxel.m_min.m_x;
            const Real &y0 = m_voxel.m_min.m_y;
            const Real &z0 = m_voxel.m_min.m_z;
            const Real &x1 = m_voxel.m_max.m_x;
            const Real &y1 = m_voxel.m_max.m_y;
            const Real &z1 = m_voxel.m_max.m_z;

            const Real INF = 1e9;

            Real txm, tym, tzm;

            switch(b) {
            case 0:
                txm = 0.5 * (tx0 + tx1);
                tym = 0.5 * (ty0 + ty1);
                tzm = 0.5 * (tz0 + tz1);
                break;
            case 1:
                txm = 0.5 * (tx0 + tx1);
                tym = 0.5 * (ty0 + ty1);
                tzm = origin.m_z < 0.5 * (z0 + z1) ? +INF : -INF;
                break;
            case 2:
                txm = 0.5 * (tx0 + tx1);
                tym = origin.m_y < 0.5 * (y0 + y1) ? +INF : -INF;
                tzm = 0.5 * (tz0 + tz1);
                break;
            case 3:
                txm = 0.5 * (tx0 + tx1);
                tym = origin.m_y < 0.5 * (y0 + y1) ? +INF : -INF;
                tzm = origin.m_z < 0.5 * (z0 + z1) ? +INF : -INF;
                break;
            case 4:
                txm = origin.m_x < 0.5 * (x0 + x1) ? +INF : -INF;
                tym = 0.5 * (ty0 + ty1);
                tzm = 0.5 * (tz0 + tz1);
                break;
            case 5:
                txm = origin.m_x < 0.5 * (x0 + x1) ? +INF : -INF;
                tym = 0.5 * (ty0 + ty1);
                tzm = origin.m_z < 0.5 * (z0 + z1) ? +INF : -INF;
                break;
            case 6:
                txm = origin.m_x < 0.5 * (x0 + x1) ? +INF : -INF;
                tym = origin.m_y < 0.5 * (y0 + y1) ? +INF : -INF;
                tzm = 0.5 * (tz0 + tz1);
                break;
            case 7:
                txm = origin.m_x < 0.5 * (x0 + x1) ? +INF : -INF;
                tym = origin.m_y < 0.5 * (y0 + y1) ? +INF : -INF;
                tzm = origin.m_z < 0.5 * (z0 + z1) ? +INF : -INF;
                break;
            default:
                assert(!"Internal failure.");
            }

            unsigned int current_octant = 0;

            if(tx0 > ty0) {
                if(tx0 > tz0) {
                    // max(tx0, ty0, tz0) is tx0. Entry plane is YZ.
                    if(tym < tx0) current_octant |= 2;
                    if(tzm < tx0) current_octant |= 1;
                } else {
                    // max(tx0, ty0, tz0) is tz0. Entry plane is XY.
                    if(txm < tz0) current_octant |= 4;
                    if(tym < tz0) current_octant |= 2;
                }
            } else {
                if(ty0 > tz0) {
                    // max(tx0, ty0, tz0) is ty0. Entry plane is XZ.
                    if(txm < ty0) current_octant |= 4;
                    if(tzm < ty0) current_octant |= 1;
                } else {
                    // max(tx0, ty0, tz0) is tz0. Entry plane is XY.
                    if(txm < tz0) current_octant |= 4;
                    if(tym < tz0) current_octant |= 2;
                }
            }

            // This special state indicates algorithm termination.
            const unsigned int END = 8;

            while(true) {
                switch(current_octant) {
                case 0:
                    if(m_children[a]->Trace(origin, tx0, ty0, tz0, txm, tym, tzm, a, b, visitor))
                        return true;
                    current_octant = choose_next(txm, tym, tzm, 4, 2, 1);
                    break;
                case 1:
                    if(m_children[1 ^ a]->Trace(origin, tx0, ty0, tzm, txm, tym, tz1, a, b, visitor))
                        return true;
                    current_octant = choose_next(txm, tym, tz1, 5, 3, END);
                    break;
                case 2:
                    if(m_children[2 ^ a]->Trace(origin, tx0, tym, tz0, txm, ty1, tzm, a, b, visitor))
                        return true;
                    current_octant = choose_next(txm, ty1, tzm, 6, END, 3);
                    break;
                case 3:
                    if(m_children[3 ^ a]->Trace(origin, tx0, tym, tzm, txm, ty1, tz1, a, b, visitor))
                        return true;
                    current_octant = choose_next(txm, ty1, tz1, 7, END, END);
                    break;
                case 4:
                    if(m_children[4 ^ a]->Trace(origin, txm, ty0, tz0, tx1, tym, tzm, a, b, visitor))
                        return true;
                    current_octant = choose_next(tx1, tym, tzm, END, 6, 5);
                    break;
                case 5:
                    if(m_children[5 ^ a]->Trace(origin, txm, ty0, tzm, tx1, tym, tz1, a, b, visitor))
                        return true;
                    current_octant = choose_next(tx1, tym, tz1, END, 7, END);
                    break;
                case 6:
                    if(m_children[6 ^ a]->Trace(origin, txm, tym, tz0, tx1, ty1, tzm, a, b, visitor))
                        return true;
                    current_octant = choose_next(tx1, ty1, tzm, END, END, 7);
                    break;
                case 7:
                    if(m_children[7 ^ a]->Trace(origin, txm, tym, tzm, tx1, ty1, tz1, a, b, visitor))
                        return true;
                case END:
                    return false;
                }
            }
        }

        template<typename Visitor>
        void Visit(Visitor &visitor) const {
            visitor.Visit(this);

            if(!m_is_leaf) {
                for(int i = 0; i < 8; ++i)
                    m_children[i]->Visit(visitor);
            }
        }
    };

    template<typename Item>
    class Octree {
    public:
        Octree();
        ~Octree();

        void Insert(const Item &item, const AABB3 &aabb);

        template<typename ItemBoxIntersector>
        void Subdivide(int pop_threshold, int max_subdiv, ItemBoxIntersector &intersector) {
            assert(m_root);
            assert(pop_threshold > 0);
            assert(max_subdiv >= 0);

            if(max_subdiv > 0) {
                m_root->m_center = m_root->m_voxel.GetCenter();
                m_root->Subdivide(pop_threshold, max_subdiv, intersector);
            }
        }

        const AABB3 &GetAABB() const;

        //! Invokes the visitor on the leaf containing a given point.
        template<typename Visitor>
        void Hit(const Point3 &point, Visitor &visitor) const {
            assert(m_root);

            Real x = point.m_x;
            Real y = point.m_y;
            Real z = point.m_z;

            const Point3 &box_min = m_root->m_voxel.m_min;
            const Point3 &box_max = m_root->m_voxel.m_max;

            if( x < box_min.m_x || x > box_max.m_x ||
                y < box_min.m_y || y > box_max.m_y ||
                z < box_min.m_z || z > box_max.m_z)
                return;

            OctreeNode<Item> *node = m_root;

            while(!node->m_is_leaf) {
                unsigned int octant = 0;

                if(x >= node->m_center.m_x) octant |= 4;
                if(y >= node->m_center.m_y) octant |= 2;
                if(z >= node->m_center.m_z) octant |= 1;

                node = node->m_children[octant];
            }

            visitor.Hit(node);
        }

        //! The Trace() method implements the algorithm described in the paper
        //! entitled "An Efficient Parametric Algorithm for Octree Traversal" by
        //! J. Revelles, C. Urena and M. Lastra. This paper can be found at the
        //! following address: http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf.
        template<typename Visitor>
        void Trace(Point3 origin, Vector3 direction, Visitor &visitor) const {
            assert(m_root);

            const Point3 &box_min = m_root->m_voxel.m_min;
            const Point3 &box_max = m_root->m_voxel.m_max;

            const Real EPSILON = 1e-9;

            unsigned int a = 0;
            unsigned int b = 0;

            if(direction.m_x == 0.0) {
                direction.m_x = EPSILON;
                b |= 4;
            } else if(direction.m_x < 0.0) {
                origin.m_x = (box_min.m_x + box_max.m_x) - origin.m_x;
                direction.m_x = -direction.m_x;
                a |= 4;
            }

            if(direction.m_y == 0.0) {
                direction.m_y = EPSILON;
                b |= 2;
            } else if(direction.m_y < 0.0) {
                origin.m_y = (box_min.m_y + box_max.m_y) - origin.m_y;
                direction.m_y = -direction.m_y;
                a |= 2;
            }

            if(direction.m_z == 0.0) {
                direction.m_z = EPSILON;
                b |= 1;
            } else if(direction.m_z < 0.0) {
                origin.m_z = (box_min.m_z + box_max.m_z) - origin.m_z;
                direction.m_z = -direction.m_z;
                a |= 1;
            }

            assert(
                direction.m_x != 0.0 &&
                direction.m_y != 0.0 &&
                direction.m_z != 0.0);

            Real tx0 = (box_min.m_x - origin.m_x) / direction.m_x;
            Real tx1 = (box_max.m_x - origin.m_x) / direction.m_x;
            Real ty0 = (box_min.m_y - origin.m_y) / direction.m_y;
            Real ty1 = (box_max.m_y - origin.m_y) / direction.m_y;
            Real tz0 = (box_min.m_z - origin.m_z) / direction.m_z;
            Real tz1 = (box_max.m_z - origin.m_z) / direction.m_z;

            if(max(tx0, ty0, tz0) < min(tx1, ty1, tz1))
                m_root->Trace(origin, tx0, ty0, tz0, tx1, ty1, tz1, a, b, visitor);
        }

        //! Go through all the nodes and call the Visit() method on each of them.
        template<typename Visitor>
        void Visit(Visitor &visitor) const {
            assert(m_root);
            m_root->Visit(visitor);
        }

    private:
        OctreeNode<Item> *m_root;
    };

#include "octree.inl"

}

#endif  // !SHEEP_MATH_OCTREE_H

---