AnimaL

Tutorial

Documentation

---

Documentation

• Overview
• Modules
• Namespace List
• Class Hierarchy
• Alphabetical List
• Compound List
• File List
• Namespace Members
• Compound Members
• File Members
• Examples

---

fastoctreedeformableconstrained.h

Go to the documentation of this file.

#ifndef FASTOCTREE_H
#define FASTOCTREE_H

#include <stack>
#include <deque>
#include <set>
#include <vector>
#include <algorithm>
#include <stdexcept>
#include <iostream>

#include "assertions.h"
#include "global.h"

namespace animal
{
namespace octree
{

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Interface of FastOctree<T,S,U>
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
template<class T,class S,class U>
class FastOctree
{
public:
    class Cell;
    class Face;
    typedef T Vertex;
    typedef Vertex* VertexPtr;

    class dfs_iterator;
    class dfs_leaf_iterator;
    class dbs_iterator;
    typedef struct
    {
        bool _entered;
        Cell* _cell;
    }
    dfs_iterator_data;





public:
    typedef S Data;

    FastOctree(Cell* root);
    //  FastOctree(const T& min,const FloatingPointType& size,const S& defaultData = S());
    //FastOctree(const T& min,const FloatingPointType& ,const FloatingPointType& ,const FloatingPointType&,
    //       const S& defaultData = S());
    FastOctree(const U& min,const FloatingPointType& size,const S& defaultData = S());
    FastOctree(const U& min,const FloatingPointType& ,const FloatingPointType& ,const FloatingPointType&,
               const S& defaultData = S());

    virtual ~FastOctree();
    inline Cell* root() const;

    static inline unsigned short vertexIndex(unsigned short i,
            unsigned short j,
            unsigned short k);
    static inline unsigned short childIndex(unsigned short i,
                                            unsigned short j,
                                            unsigned short k);
public:
    //protected:
    static const bool hasBrotherAlongFace[8][6];
    static const bool hasBrotherAlongEdge[8][12];
    static const unsigned short alongFace[8][6];
    static const unsigned short alongEdge[8][12];
    static const unsigned short supportingFace[8][12];
    static const bool hasOnFatherEdge[8][12];
    static const unsigned short childrenSharingFace[6][4];
    static const unsigned short alterEgoFace[6];
    static const unsigned short faceVertices[6][4];
    static const unsigned short verticesConnectedFaces[8][3];
    static const unsigned short directionForVertexOnEdge[8][8];
    static const unsigned short directionForVertexOnFace[8][8];


public:
    //private:
    Cell* root_;
    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Interface of FastOctree<T,S,U>::Cell
    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    /*template<class T,class S,class U>
    class FastOctree<T,S,U>::Cell*/

    class Cell
    {

    public:
        inline Data& operator*()
        {
            return data_;
        }
        inline Data* operator->()
        {
            return &data_;
        }
        inline const Data& operator*() const
        {
            return data_;
        }
        inline const Data* operator->() const
        {
            return &data_;
        }

        inline void setData( const Data d )
        {
            data_ = d;
        }
        inline Data& getData( )
        {
            return data_;
        }

        void subdivide(const S& defaultData = S());
        void simplify( );

        Cell* locate(const U& p);

        inline bool isLeaf() const;
        inline Vertex* vertex(unsigned short i) const;
        inline Cell* father() const;
        inline unsigned short fatherPos() const;
        inline Cell* child(unsigned short i) const;

        inline Vertex center() const;
        inline Vertex diagonal() const;
        inline FloatingPointType size(unsigned short i) const;

        inline Face face(unsigned short pos) const;

        Cell* cellSharingFace(unsigned short i) const;
        template<class O>
        O copyChildrenTouchingFace(unsigned short face,O output) const;
        template<class O>
        O copyCellsTouchingFace(unsigned short face,O output) const;
        Cell* cellSharingEdge(unsigned short i) const;

        bool hasUncleSharingFace( unsigned short i ) const;
        bool hasUncleSharingEdge( unsigned short edge ) const;

        Cell* uncleSharingFace( unsigned short i ) const;

        bool edgeOnRootFace( unsigned short edge ) const;
        bool edgeOnRootEdge( unsigned short edge ) const;
        bool faceOnRootFace( unsigned short face ) const;

        bool contains(const U& p) const;

        inline int memorySize() const;


        inline bool isRoot() const
        {
            return father_ == NULL;
        };

    
    inline Cell* getNeighbour( const unsigned int f ) const { Require(f<6); return _faceNeighbours[f]; };
    
        class vertex_width_iterator;
        class vertex_width_neighbours_iterator;
        class vertex_connected_iterator;

        //friend inline std::ostream& operator<<(std::ostream& s, const Cell &c);

    protected:
        friend class FastOctree<T,S,U>;

    Cell *_faceNeighbours[6];
    
        Cell(Cell& f,unsigned short p,VertexPtr vertices[27],const S& defaultData);
        Cell( const U& min,const FloatingPointType& size,const S& defaultData = S());
        Cell( const U& min,const FloatingPointType& ,const FloatingPointType& ,const FloatingPointType&,
              const S& defaultData = S());
        ~Cell();

        inline VertexPtr createRootVertex( const FloatingPointType x, const FloatingPointType y, const FloatingPointType z );
        inline VertexPtr createFreeVertex(const U& v);
        inline VertexPtr createFreeVertex(const FloatingPointType x,const FloatingPointType y,const FloatingPointType z);
        inline VertexPtr createFreeVertex( VertexPtr v1, VertexPtr v2 );
        inline VertexPtr createFreeVertex( VertexPtr v1, VertexPtr v2, VertexPtr v3, VertexPtr v4 );
        inline VertexPtr createFreeVertex( VertexPtr v1, VertexPtr v2, VertexPtr v3, VertexPtr v4, VertexPtr v5, VertexPtr v6, VertexPtr v7, VertexPtr v8 );

        inline VertexPtr createLinkedVertex( VertexPtr v1, VertexPtr v2 );
        inline VertexPtr createLinkedVertex( VertexPtr v1, VertexPtr v2, VertexPtr v3, VertexPtr v4 );


        inline VertexPtr& vertexPtrAtCenterOfFace(unsigned short face);
        inline VertexPtr& vertexPtrAtCenterOfEdge(unsigned short edge);

        inline void removeVertex( VertexPtr vp );

        static const unsigned short faceAlongEdge[12][2];


    public:
        // private:
        Data           data_;
        Cell*          children_[8];
        Cell*          father_;
        unsigned short pos_;
        VertexPtr      vertices_[8];
        std::deque<VertexPtr> toDelete_;

        void printPos( std::ostream& out ) const;
        static const unsigned short connectedVertices[8][3];
        


        //         /** added by FF for more flexibility than the FastOctree::dfs_iterator  (?)*/
        //         class dfs_iterator
        //         {
        //         public:
        //             dfs_iterator( Cell* c ):_cell(c){}
        //             virtual ~dfs_iterator(){}
        //
        //             dfs_iterator& operator++();
        //
        //             Cell& operator*(){ return *_cell; }
        //
        //         private:
        //             Cell* _cell;
        //             std::stack<Cell*> _stack;
        //
        //         };


    class vertex_iterator: public dfs_iterator
        {
        public:
            vertex_iterator( Cell *cell );
            ~vertex_iterator();

            Vertex* operator++();
            bool empty();

        private:
            Vertex* next_vertex();
            Cell *_currentCell;
            int _currentVertexNumber;
            Vertex* _nextVertex;

        };

        class vertex_width_iterator
        {
        public:
            vertex_width_iterator( Cell *cell );
            ~vertex_width_iterator();

            Vertex* operator++();
            bool empty();

        private:
            /*
             * The lists of vertices ordered by depth,
             * There are only 2 different depths at the same time
             * So we have 2 lists and one ID to get the current top list
             */
            std::deque<Vertex*> _vLists[2];
            unsigned int _topListId;

            /*
             * Now, in order not to add only Vertices that are
             * not already in the list, keep a trace of the stored vertices
             */
            std::set
                <Vertex*> _insertedList;

            Cell *_cell;

        };


        /*
         * Now, for each vertex of this cell,
         * Also get the vertices that are connected to it in the neighbourhood of the cell
         */

        class vertex_width_neighbours_iterator
        {
        public:
            vertex_width_neighbours_iterator( Cell *cell );

            /*
             * Only use this vertex at this depth to catch connected vertices
             */
            vertex_width_neighbours_iterator( Vertex* vertex, Cell *cell, unsigned int vPos );
            ~vertex_width_neighbours_iterator();

            Vertex* operator++();
            bool empty();

        private:
            std::deque<Vertex*> _vLists[2];
            unsigned int _topListId;
            std::set
                <Vertex*> _insertedList;

            Cell *_cell;

        };
    };
    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Interface of FastOctree<T,S,U>::Face
    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    /*template<class T,class S,class U>
    class FastOctree<T,S,U>::Face*/
    class Face
    {
    public:
        inline Vertex* vertex(unsigned short i) const;
        inline unsigned short pos() const;
        inline Vertex* vertexAtCenter() const;
        inline const FloatingPointType* normal() const;
    protected:
        friend class Cell;
        Face(const Cell* cell,unsigned short pos);
    private:
        const Cell*    cell_;
        unsigned short pos_;
    };


    class dfs_iterator
    {
    public:
        dfs_iterator( Cell*, bool (*selectF)(Cell*) = NULL, void (*enterF)(Cell*)=NULL, void (*leaveF)(Cell*)=NULL );
        virtual ~dfs_iterator();

        Cell* operator++();
        bool empty();

        Cell* operator*();

    protected:
        virtual bool select(Cell*)
        {
            return true;
        };
        // What to do when entering a node
        virtual void enter(Cell*)
        {}
        ;
        // What to do when leaving a node
        virtual void leave(Cell*)
        {}
        ;


    private:
        bool (*_selectF)(Cell*);
        void (*_enterF)(Cell*);
        void (*_leaveF)(Cell*);

        std::deque<dfs_iterator_data> _deque;
    };


    class dfs_leaf_iterator
    {
    public:
        dfs_leaf_iterator( Cell*, void (*workF)(Cell*)=NULL );
        virtual ~dfs_leaf_iterator();

        Cell* operator++();
        bool empty();

        Cell* operator*();

    protected:
        // What to do with a node
        virtual void work(Cell*)
        {}
        ;


    private:
        void (*_workF)(Cell*);

        std::deque<dfs_iterator_data> _deque;
    };

}
;//class FastOctree

template<class T,class S,class U>
void
FastOctree<T,S,U>::Cell::printPos( std::ostream& out ) const { if( father_ ) father_->printPos(out); else out<<'r'; out<<pos_; }
/*
template<class T,class S,class U>
std::ostream&
FastOctree<T,S,U>::Cell::operator<<(std::ostream& s)
{
    if( father() == NULL )
        s << "root->";
    else
    {
        s << *father() << fatherPos() << "->";
    }
    return s;
}
*/
/*
template<class T,class S,class U>
std::ostream& operator<<(std::ostream& s,typename const Cell& c)
{
    if( c->father() == NULL )
        s << "root->";
    else
    {
        s << *(c->father()) << c->fatherPos() << "->";
    }
    return s;
}
*/

//************************************************************
// Implementation of FastOctree<T,S,U>::Cell
//************************************************************
template<class T,class S,class U>
void
FastOctree<T,S,U>::Cell::subdivide(const S& defaultData)
{
    // If it is already subdivided, exit!
    if (!isLeaf())
    {
        return;
    }
    // center_edge[e] = the index of vertex at the center of edge e.
    // Table is checked.
    static const unsigned short centerEdge[12] =
        {
            1,7,19,25,3,21,5,23,9,11,15,17
        };

    // center_face[f] = the index of point at the center of face f.
    // Table is checked.
    static const unsigned short centerFace[6] =
        {
            12,14,10,16,4,22
        };

    // face_edge[f] are the 4 edges who surround face f.
    // Table is checked.
    /*
    static const unsigned short faceEdge[6][4] =
    {
      { 5,10, 4, 8 },
      { 6,11, 7, 9 },
      { 0, 8, 2, 9 },
      { 3,11, 1,10 },
      { 0, 4, 1, 6 },
      { 3, 7, 2, 5 }  
    };
    */
    // alter_ego_edge_face[e][f] is the number of the edge equivalent to
    // edge \p e of a cell for cell sharing face \p f.
    // Table is checked.
    static const short alterEgoEdgeFace[12][6] =
        {
            {
                -1,-1, 1,-1, 2,-1
            }
            ,             //{ 0,0,1,1,2,2 },
            { -1,-1,-1, 0, 3,-1 },       //{ 1,1,0,0,3,3 },
            { -1,-1, 3,-1,-1, 0 },       //{ 2,2,3,3,0,0 },
            { -1,-1,-1, 2,-1, 1 },       //{ 3,3,2,2,1,1 },
            {  6,-1,-1,-1, 5,-1 },       //{ 6,6,4,4,5,5 },
            {  7,-1,-1,-1,-1, 4 },       //{ 7,7,5,5,4,4 },
            { -1, 4,-1,-1, 7,-1 },       //{ 4,4,6,6,7,7 },
            { -1, 5,-1,-1,-1, 6 },       //{ 5,5,7,7,6,6 },
            {  9,-1,10,-1,-1,-1 },       //{ 9,9,10,10,8,8 },
            { -1, 8,11,-1,-1,-1 },       //{ 8,8,11,11,9,9 },
            { 11,-1,-1, 8,-1,-1 },       //{ 11,11,8,8,10,10 },
            { -1,10,-1, 9,-1,-1 }        //{ 10,10,9,9,11,11 }
        }
        ;
    // alter_ego_edge[e] is the index of the edge number \p e of a cell C
    // in the cell sharing this edge with cell C.
    // Table is checked.
    static const unsigned short alterEgoEdge[12] =
        {
            3,2,1,0,7,6,5,4,11,10,9,8
        };

    /* From Mathieu :
    * depending_points_edge_subdivided[e] gives you the 2 vertices
    * which are needed to create the new point at the middle of the edge \p e
    * They're part of the original points, we give them with new indices
    * Not checked
    */
    static const unsigned short depending_points_edge_subdivided[12][2] =
        {
            {
                0,  2
            }
            , // vertex[1] is 1/2 ( vertex[0] + vertex[2] )
            {  6,  8 },
            { 18, 20 },
            { 24, 26 },
            {  0,  6 },
            { 18, 24 },
            {  2,  8 },
            { 20, 26 },
            {  0, 18 },
            {  2, 20 },
            {  6, 24 },
            {  8, 26 }
        };

    /*
     * vertexChildPos[v][0] gives our associated father's child position for the vertex v
     * vertexChildPos[v][1] gives the ID of the vertex in vertexChildPos[v][0] father's child
     */
    /*
    static const unsigned short vertexChildPos[27][2] =
    {
    {0,0}, {0,1}, {1,1}, {0,2}, {0,3}, {1,3}, {2,2}, {2,3}, {3,3},
    {0,4}, {0,5}, {1,5}, {0,6}, {0,7}, {1,7}, {2,6}, {2,7}, {3,7},
    {4,4}, {4,5}, {5,5}, {4,6}, {4,7}, {5,7}, {6,6}, {6,7}, {7,7}
    };
    */

    /* From Mathieu :
    * depending_points_face_subdivided[f] gives you the 4 vertices on which depends
    * the vertex at the "center" of face f.
    * We give the 4 vertices which are at the center of the 4 edges of the face
    * See later the "centerFace" table :
    * centerFace[f] = ( sum(i=0..3) depending_points_face_subdivided[f][i] ) / 4;
    */
    /*
    static const unsigned short depending_points_face_subdivided[6][4] =
    {
    {  3,  9, 21, 15 },     // For face 0 : vertex 12
    {  5, 11, 23, 17 },     // For face 1 : vertex 14
    {  1, 11, 19,  9 },     // For face 2 : vertex 10
    {  7, 15, 25, 17 },     // For face 3 : vertex 16
    {  1,  3,  5,  7 },     // For face 4 : vertex  4
    { 19, 23, 25, 21 },     // For face 5 : vertex 22
    };
    */


    //************************************************************
    // We create/get from neighbours the 27 vertices that
    // are necessary to subdivide the cell.
    //************************************************************
    VertexPtr vertices[27];
    std::fill(vertices,vertices+27,static_cast<VertexPtr>(NULL));
    // 8 are easily find: these are the vertices of the cell
    vertices[ 0] = vertices_[0];
    vertices[ 2] = vertices_[1];
    vertices[ 6] = vertices_[2];
    vertices[ 8] = vertices_[3];
    vertices[18] = vertices_[4];
    vertices[20] = vertices_[5];
    vertices[24] = vertices_[6];
    vertices[26] = vertices_[7];

    // The center always has to be created
    // From Mathieu : Removed this, see this below, it's created from the centers of the faces

    // Search along the 6 faces and 12 edges the already existing vertices,
    // that is the vertex at center of faces and edges.
    // ATTENTION: this remark is to signal a subtil bug that was hard to
    // correct!! The subtle vertex is that calls to centerOfFace or
    // centerOfEdge silently returns NULL if vertex does not exist.  But some
    // vertices amongst the 27 we are trying to share can be "shared" in
    // different manner.  For example the middle vertex of an edge (e.g.
    // number 19) can be shared with 3 cells. It is eventually the same vertex
    // that is shared, but it can be "retrieved" from 3 neighbouring cells.
    // So once a retrieval has given a non NULL vertex, we must ensure that the
    // 2 other retrieval (which might return NULL) do not override the
    // result.  Actually, once we have find a way to share, we must skip other
    // retrievals (which speeds up by the way).

    /* From Mathieu :
    * We need to first deal the vertices at the center of edges
    * So we can use them to make the vertices at the center of faces
    * (We could also have used the "this" cell's 8 main vertices
    * But next, for constrained vertices, we would be able to parametrize
    * them directly from the 2 edges they depend on (given by the 2 pairs of points)
    */
    for (unsigned short edge=0;edge<12;++edge)
    {
        Vertex** edgeVertex = vertices + centerEdge[edge];

        std::cerr << "Making edge " << edge << " : vertex " << centerEdge[edge] << "\n";
        VertexPtr sharedPoint = NULL;
        unsigned short nNeighbours = 0;

        Cell *neighbourEdge, *neighbourFace0, *neighbourFace1;
        // First neighbour : on the edge
        neighbourEdge = cellSharingEdge( edge );
        if( neighbourEdge != NULL )
        {
            nNeighbours++;
            if( !neighbourEdge->isLeaf() )
            {
                sharedPoint = neighbourEdge->vertexPtrAtCenterOfEdge(alterEgoEdge[edge]);
            }
        }

        // Second neighbour : on the first face
        neighbourFace0 = cellSharingFace( faceAlongEdge[edge][0] );
        if( neighbourFace0 != NULL )
        {
            nNeighbours++;
            if( (sharedPoint==NULL) && (!neighbourFace0->isLeaf()) )
            {
                sharedPoint = neighbourFace0->vertexPtrAtCenterOfEdge(alterEgoEdgeFace[edge][faceAlongEdge[edge][0]]);
            }
        }

        // Third neighbour : on the second face
        neighbourFace1 = cellSharingFace( faceAlongEdge[edge][1] );
        if( neighbourFace1 != NULL )
        {
            nNeighbours++;
            if( (sharedPoint==NULL) && (!neighbourFace1->isLeaf()) )
                sharedPoint = neighbourFace1->vertexPtrAtCenterOfEdge(alterEgoEdgeFace[edge][faceAlongEdge[edge][1]]);
        }


        if( sharedPoint != NULL )
        {
        std::cerr << "sharePoint != NULL\n";
            // There's a sharedPoint
            *edgeVertex = sharedPoint;

            if( nNeighbours == 3 )
            {
            std::cerr << "\tnNeighbours is 3\n";
                if( !neighbourEdge->isLeaf() && !neighbourFace0->isLeaf() && !neighbourFace1->isLeaf() )
                {
                    (*edgeVertex)->freeIt();
                    //                      std::cerr << "nNeighbours == 3 ->FREED\n";
                }
                else
                {
                    //                      std::cerr << "Existe SharedPoint, 3 neighbours avec au moins 1 feuille : share it\n";
                }
            }
            else if( nNeighbours == 2 )
            {
            std::cerr << "\tnNeighbours is 2\n";
                //                  std::cerr << "nNeighbours == 2 : share it\n";
            }
            else if( nNeighbours == 1 )
            {
            std::cerr << "\tnNeighbours is 1\n";
        
        /*
                if( neighbourEdge == NULL )
        //if( edgeOnRootFace(edge) || edgeOnRootEdge(edge) )
                {
            
            std::cerr << "\t\tNeighbour == NULL\n";
            if( hasUncleSharingFace(faceAlongEdge[edge][0]) )
            {
                std::cerr << "hasUncleSharingFace(faceAlongEdge[edge][0]" << faceAlongEdge[edge][0] << "\n";
            }
            if( hasUncleSharingFace(faceAlongEdge[edge][1]) )
            {
                std::cerr << "hasUncleSharingFace(faceAlongEdge[edge][1]" << faceAlongEdge[edge][1] << "\n";
            }
            
            
            if( ! ((neighbourFace0 || hasUncleSharingFace(faceAlongEdge[edge][0])) && (neighbourFace1||hasUncleSharingFace(faceAlongEdge[edge][1])) ) )
            {
                std::cerr << "\t\tfreeIt !\n";
                            (*edgeVertex)->freeIt();
            }
        */
        if( !( this->getNeighbour( faceAlongEdge[edge][0] ) && this->getNeighbour( faceAlongEdge[edge][1] ) ) )
        {
            std::cerr << "\t\tfreeIt !\n";
            (*edgeVertex)->freeIt();
        }
                    //                      std::cerr << "nNeighbours == 1 -> FREED\n";
                else
                {
                    //                      std::cerr << "nNeighbours == 1 : share it\n";
                }
            }
            else // 0 neighbours
            {
                //                  std::cerr << "Impossible : 0 neighbours et sharedPoint existe\n";
            }

        } // end sharedPoint != NULL

        else // sharedPoint == NULL
        {
            // Create it, warning, it could be freed just after

            if(edgeOnRootEdge(edge) )
            {
                *edgeVertex = createFreeVertex(
                                  vertices[ depending_points_edge_subdivided[edge][0] ],
                                  vertices[ depending_points_edge_subdivided[edge][1] ] );
                //              std::cerr << "Creer : Free\n";
            }
            else
            {
                *edgeVertex = createLinkedVertex(
                                  vertices[ depending_points_edge_subdivided[edge][0] ],
                                  vertices[ depending_points_edge_subdivided[edge][1] ] );
                //              std::cerr << "Creer : Linked\n";
            }
        } // sharedPoint == NULL

    }

    //              std::cerr << "\n";
    for (unsigned short face=0;face<6;++face)
    {
        // With a cell sharing face, we can have 5 vertices in common:
        // - the center of the face
        // - the center of the 4 edges defining the shared face

        Cell* neighbour = cellSharingFace(face);

        if( neighbour != NULL )
        {
            // We have a neighbour, see if it already have this Vertex
            if( !neighbour->isLeaf() )
            {
                vertices[centerFace[face]] =
                    neighbour->vertexPtrAtCenterOfFace(alterEgoFace[face]);
                vertices[centerFace[face]]->freeIt();
            }
            else
            {
                vertices[centerFace[face]] = createLinkedVertex(
                                                 vertices_[faceVertices[face][0]],
                                                 vertices_[faceVertices[face][1]],
                                                 vertices_[faceVertices[face][2]],
                                                 vertices_[faceVertices[face][3]] );
            }
        }
        else
        {
            if( faceOnRootFace(face) )
            {
                // this face is on the root's borders, it's free
                vertices[centerFace[face]] = createFreeVertex(
                                                 vertices_[faceVertices[face][0]],
                                                 vertices_[faceVertices[face][1]],
                                                 vertices_[faceVertices[face][2]],
                                                 vertices_[faceVertices[face][3]] );
            }
            else
            {
                vertices[centerFace[face]] = createLinkedVertex(
                                                 vertices_[faceVertices[face][0]],
                                                 vertices_[faceVertices[face][1]],
                                                 vertices_[faceVertices[face][2]],
                                                 vertices_[faceVertices[face][3]] );
            }
        }
    }

    /* From Mathieu :
    * Now create the "center" of the cell (it must always be created),
    * it's given by
    * (sum(f=0..5) vertices[ centerFace[f] ] ) / 6
    */
    vertices[13] = createFreeVertex(
                       vertices_[ 0 ],
                       vertices_[ 1 ],
                       vertices_[ 2 ],
                       vertices_[ 3 ],
                       vertices_[ 4 ],
                       vertices_[ 5 ],
                       vertices_[ 6 ],
                       vertices_[ 7 ]
                   );

    // Create the children
    Cell** c = children_;
    *c++ = new Cell(*this,0,vertices,defaultData);
    *c++ = new Cell(*this,1,vertices,defaultData);
    *c++ = new Cell(*this,2,vertices,defaultData);
    *c++ = new Cell(*this,3,vertices,defaultData);
    *c++ = new Cell(*this,4,vertices,defaultData);
    *c++ = new Cell(*this,5,vertices,defaultData);
    *c++ = new Cell(*this,6,vertices,defaultData);
    *c++ = new Cell(*this,7,vertices,defaultData);

    // Update the neighbour informations
    for( unsigned short face=0 ; face<6 ; ++face )
    {
        Cell *neighbour = this->getNeighbour(face);
    
    // For each child cell containing this face
    if( !neighbour || neighbour->isLeaf() )
    {
        // It's on a root cell or
        // this neighbour is of size >= 
        // Simply tell them that it's their neighbour
        for( unsigned short i = 0 ; i<4 ; ++i )
        {
            unsigned int childId = childrenSharingFace[face][i] ;
            // The face's id correspond in our children
            this->child( childId )->_faceNeighbours[ face ] = neighbour;
        }
    }
    else
    {
        // Find the neighbour child corresponding
        for( unsigned short i = 0 ; i<4 ; ++i )
        {
            unsigned int childId = childrenSharingFace[face][i];
            unsigned int alterEgoChildId = childrenSharingFace[alterEgoFace[face]][i];
            // The face's id correspond in our children
            this->child( childId )->_faceNeighbours[ face ] = neighbour->child( alterEgoChildId );
            
            // don't forget to tell this neighbour that we're its new neighbour along this face
            this->child( childId )->_faceNeighbours[ face ]->_faceNeighbours[ alterEgoFace[face] ] = this->child( childId );
        }
    }
    
    // Now make the information for brothers
    for( unsigned int i=0 ; i<4 ; ++i )
    {
        unsigned int childId = childrenSharingFace[face][i];
        unsigned int alterEgoChildId = childrenSharingFace[alterEgoFace[face]][i];
        
        this->child( alterEgoChildId )->_faceNeighbours[ face ] = this->child( childId );
        this->child( childId )->_faceNeighbours[ alterEgoFace[face] ] = this->child( alterEgoChildId );
    }
    }

}



//************************************************************
// Implementation of FastOctree<T,S,U>::Cell
//************************************************************
// The simplification is not OK !!
template<class T,class S,class U>
void FastOctree<T,S,U>::Cell::simplify( )
{
    //std::cerr << "simplify() of depth " << this->getData()._depth << " and cell is " << *this << "\n";

    if( !isLeaf() )
    {
        // Simplify the children

        for( unsigned short i=0 ; i<8 ; i++ )
        {
            //          std::cerr << "Simplify child " << i << "\n";
            children_[i]->simplify();
        }

        //std::cerr << "Now simplify us\n";

        //      std::cerr << "simplify() of depth " << this->getData()._depth << " and cell is " << *this << " end children\n";

        // This table associate to every vertex of the 27 vertices
        // one of the child that contains it and it's inner vertex id associated
        // Example :
        //   Vertex 25 is cell 6's 7th vertex (but also cell 7's 6th vertex)
        //   cellsChildrenVertices[25] = { 6, 7 };
        //
        static const unsigned short cellsChildrenVertices[27][2] =
            {
                {
                    0,0
                }
                , {0,1}, {1,1}, {0,2}, {0,3}, {1,3}, {2,2}, {2,3}, {3,3},
                {0,4}, {0,5}, {1,5}, {0,6}, {0,7}, {1,7}, {2,6}, {2,7}, {3,7},
                {4,4}, {4,5}, {5,5}, {4,6}, {4,7}, {5,7}, {6,6}, {6,7}, {7,7}
            };


        // Get pointers to vertices
        VertexPtr vertices[27];
        for( unsigned short v=0 ; v<27 ; v++ )
        {
            vertices[v] = this->child( cellsChildrenVertices[v][0] )->vertex( cellsChildrenVertices[v][1] );
            //std::cerr << "Vertex[" << v << "] = " << vertices[v] << "\n";
        }


        // Unregister the cells
        for( unsigned short c=0 ; c<8 ; ++c )
        {
            for( unsigned short v=0 ; v<8 ; ++v )
            {
                Require( this->child(c)->vertex(v)->isConnected( this->child(c) ) );

                //std::cerr << "Child(" << c <<") -> Vertex[" << v << "] = " << child(c)->vertex(v) << "\n";
                for( unsigned short cellId=0 ; cellId<this->child(c)->vertex(v)->nConnectedCells() ; ++cellId )
                {
                    unsigned int i;
                    for( i=0 ; i<8 ; ++i )
                    {
                        if( this->child(c)->vertex(v)->connectedCell(cellId)->vertex(i) == this->child(c)->vertex(v) )
                            break;
                    }

                    Require( i != 8);
                    //std::cerr << "\t" << *this->child(c)->vertex(v)->connectedCell(cellId) << "\n";
                }

                this->child(c)->vertex(v)->unregisterCell( this->child(c) );
            }
        }

        // Cells have been unregistered
        for( unsigned short v=0 ; v<27 ; v++ )
        {
            // Find if it's part of one of our children
            //std::cerr << "I'm vertex " << v << "\n";

            if( vertices[v]->nConnectedCells() == 0 )
            {
                //std::cerr << "\tDeleting vertex\n";
                // Ok to delete !
                delete vertices[v];
                vertices[v] = NULL;
            }
            else
            {
                // See if we need to change the mainCell
                unsigned int c;
                for( c=0 ; c<8 ; ++c )
                {
                    if( vertices[v]->getMainCell() == this->child(c) )
                    {
                        //std::cerr << "\tMaincell is child " << c << "\n";
                        // This means that I could have created it
                        Cell *otherCell = NULL;
                        for( unsigned short cellId=0 ; cellId<vertices[v]->nConnectedCells() ; ++cellId )
                        {
                            Require( vertices[v]->connectedCell(cellId)->getData()._depth >= this->child(c)->getData()._depth );
                            //std::cerr << "\t\tConencted to : " << *(vertices[v]->connectedCell(cellId)) << "\n";
                        }

                        // Find the first cell of same depth
                        for( unsigned short cellId=0 ; cellId<vertices[v]->nConnectedCells() ; ++cellId )
                        {
                            if( vertices[v]->connectedCell(cellId)->getData()._depth == vertices[v]->getDepth() )
                                otherCell = vertices[v]->connectedCell(cellId);
                        }

                        // Find it's position in the other cell
                        unsigned short vPos = 8;
                        for( unsigned int j=0 ; j<8 ; ++j )
                        {
                            if( otherCell->vertex(j) == vertices[v] )
                            {
                                vPos = j;
                                break;
                            }
                        }
                        Require( vPos != 8 );

                        vertices[v]->setMainCell( otherCell, vPos );

                        if( vertices[v]->isFree() )
                        {
                            //std::cerr << "\tConstrain it\n";
                            if( vertices[v]->getGeoLink()->getNGeoParents() == 4 )
                            {
                                //std::cerr << "Hardlink for 4\n";
                                vertices[v]->hardLinkIt( vertices[v]->getGeoLink()->getGeoParent(vertices[v]->getGeoLink()->getGeoParentId(0)), vertices[v]->getGeoLink()->getGeoParent(vertices[v]->getGeoLink()->getGeoParentId(1)), vertices[v]->getGeoLink()->getGeoParent(vertices[v]->getGeoLink()->getGeoParentId(2)), vertices[v]->getGeoLink()->getGeoParent(vertices[v]->getGeoLink()->getGeoParentId(3)) );
                            }
                            else if( vertices[v]->getGeoLink()->getNGeoParents() == 2 )
                            {
                                //std::cerr << "Hardlink for 2\n";
                                vertices[v]->hardLinkIt( vertices[v]->getGeoLink()->getGeoParent(vertices[v]->getGeoLink()->getGeoParentId(0)), vertices[v]->getGeoLink()->getGeoParent(vertices[v]->getGeoLink()->getGeoParentId(1)) );
                            }
                            else
                            {
                                //std::cerr << "8 parents so free\n";
                            }
                        }


                        break;
                    }
                }

                if( c == 8 )
                {
                    //std::cerr << "\tThis vertex' maincell is not one of our child\n";
                    // This vertex' maincell is not one of our child
                    if( (vertices[v]->isFree()) && (vertices[v]->getDepth() == this->getData()._depth+1) )
                    {
                        //std::cerr << "\tConstrain it\n";
                        if( vertices[v]->getGeoLink()->getNGeoParents() == 4 )
                        {
                            //std::cerr << "Hardlink for 4\n";
                            vertices[v]->hardLinkIt( vertices[v]->getGeoLink()->getGeoParent(vertices[v]->getGeoLink()->getGeoParentId(0)), vertices[v]->getGeoLink()->getGeoParent(vertices[v]->getGeoLink()->getGeoParentId(1)), vertices[v]->getGeoLink()->getGeoParent(vertices[v]->getGeoLink()->getGeoParentId(2)), vertices[v]->getGeoLink()->getGeoParent(vertices[v]->getGeoLink()->getGeoParentId(3)) );
                        }
                        else if( vertices[v]->getGeoLink()->getNGeoParents() == 2 )
                        {
                            //std::cerr << "Hardlink for 2\n";
                            vertices[v]->hardLinkIt( vertices[v]->getGeoLink()->getGeoParent(vertices[v]->getGeoLink()->getGeoParentId(0)), vertices[v]->getGeoLink()->getGeoParent(vertices[v]->getGeoLink()->getGeoParentId(1)) );
                        }
                        else
                        {
                            //std::cerr << "8 parents so free\n";
                        }
                    }

                }
            }

        }



        for(unsigned short i=0 ; i<8 ; ++i )
        {
            delete children_[i];
            children_[i] = NULL;
        }

    }

    // Update the neighbour informations
    // Tell all the connected neighbours to one of our child that we're they're new neighbour
    // for the concerned face
    for( unsigned short face=0 ; face<6 ; ++face )
    {
        if( this->getNeighbour(face) )
    {
        unsigned short aeFace = alterEgoFace[ face ];
        
            std::deque<Cell*> connectedCells;
        connectedCells.push_back( this->getNeighbour(face) );
        
        while( !connectedCells.empty() )
        {
            Cell *cell = connectedCells.front();
            connectedCells.pop_front();
            cell->_faceNeighbours[ aeFace ] = this;
            if( !cell->isLeaf() )
            {
                for( unsigned short i=0 ; i<4 ; ++i )
                {
                    connectedCells.push_back( cell->child( childrenSharingFace[aeFace][i] ) );
                }
            }
        }
    }
    // else nothing to do
    }
    /*
        // Update the neighbour informations
    for( unsigned short face=0 ; face<6 ; ++face )
    {
        Cell *neighbour = this->getNeighbour(face);
    
    // For each child cell containing this face
    if( !neighbour || neighbour->isLeaf() )
    {
        // It's on a root cell or
        // this neighbour is of size >= 
        // Simply tell them that it's their neighbour
        for( unsigned short i = 0 ; i<4 ; ++i )
        {
            unsigned int childId = childrenSharingFace[face][i] ;
            // The face's id correspond in our children
            this->child( childId )->_faceNeighbours[ face ] = neighbour;
        }
    }
    else
    {
        // Find the neighbour child corresponding
        for( unsigned short i = 0 ; i<4 ; ++i )
        {
            unsigned int childId = childrenSharingFace[face][i];
            unsigned int alterEgoChildId = childrenSharingFace[alterEgoFace[face]][i];
            // The face's id correspond in our children
            this->child( childId )->_faceNeighbours[ face ] = neighbour->child( alterEgoChildId );
            
            // don't forget to tell this neighbour that we're its new neighbour along this face
            this->child( childId )->_faceNeighbours[ face ]->_faceNeighbours[ alterEgoFace[face] ] = this->child( childId );
        }
    }
    
    // Now make the information for brothers
    for( unsigned int i=0 ; i<4 ; ++i )
    {
        unsigned int childId = childrenSharingFace[face][i];
        unsigned int alterEgoChildId = childrenSharingFace[alterEgoFace[face]][i];
        
        this->child( alterEgoChildId )->_faceNeighbours[ face ] = this->child( childId );
        this->child( childId )->_faceNeighbours[ alterEgoFace[face] ] = this->child( alterEgoChildId );
    }
    }
    */    
    //std::cerr << "simplify() : end  of depth " << this->getData()._depth << " and cel " << *this << "\n";

}


//************************************************************
// Implementation of FastOctree<T,S,U>::Cell
//************************************************************
template<class T,class S,class U>
typename FastOctree<T,S,U>::Cell*
FastOctree<T,S,U>::Cell::locate(const U& p)
{
    const Vertex& O = *vertices_[0];
    const Vertex  I = (*(vertices_[1])-O);
    const Vertex  J = (*(vertices_[2])-O);
    const Vertex  K = (*(vertices_[4])-O);
    const Vertex  M = Vertex(p[0],p[1],p[2])-O;
    FloatingPointType ps;
    ps = (M*I)/(I*I);
    if (ps < 0.0f || ps > 1.0f)
    {
        return NULL;
    }
    const unsigned short x = ps<0.5f?0:1;
    ps = (M*J)/(J*J);
    if (ps < 0.0f || ps > 1.0f)
    {
        return NULL;
    }
    const unsigned short y = ps<0.5f?0:1;
    ps = (M*K)/(K*K);
    if (ps < 0.0f || ps > 1.0f)
    {
        return NULL;
    }
    const unsigned short z = ps<0.5f?0:1;
    //
    Cell* c = child(x + 2*y + 4*z);
    return c != NULL ? c : this;
}
template<class T,class S,class U>
inline bool
FastOctree<T,S,U>::Cell::isLeaf() const
{
    return child(0) == NULL;
}
template<class T,class S,class U>
inline typename FastOctree<T,S,U>::Vertex*
FastOctree<T,S,U>::Cell::vertex(unsigned short i) const
{
    return static_cast<Vertex*>(vertices_[i]);
}
template<class T,class S,class U>
inline typename FastOctree<T,S,U>::Cell*
FastOctree<T,S,U>::Cell::father() const
{
    return father_;
}
template<class T,class S,class U>
inline unsigned short
FastOctree<T,S,U>::Cell::fatherPos() const
{
    return pos_;
}
template<class T,class S,class U>
inline typename FastOctree<T,S,U>::Cell*
FastOctree<T,S,U>::Cell::child(unsigned short i) const
{
    return children_[i];
}
template<class T,class S,class U>
inline typename FastOctree<T,S,U>::Vertex
FastOctree<T,S,U>::Cell::center() const
{
    const Vertex& A = *vertices_[7];
    const Vertex& B = *vertices_[0];
    return Vertex(0.5f*(A[0]+B[0]),0.5f*(A[1]+B[1]),0.5f*(A[2]+B[2]));
}
template<class T,class S,class U>
inline typename FastOctree<T,S,U>::Vertex
FastOctree<T,S,U>::Cell::diagonal() const
{
    const Vertex& A = *vertices_[7];
    const Vertex& B = *vertices_[0];
    return Vertex(A[0]-B[0],A[1]-B[1],A[2]-B[2]);
}
template<class T,class S,class U>
inline FloatingPointType
FastOctree<T,S,U>::Cell::size(unsigned short i) const
{
    return (*vertices_[7])[i]-(*vertices_[0])[i];
}
template<class T,class S,class U>
inline typename FastOctree<T,S,U>::Face
FastOctree<T,S,U>::Cell::face(unsigned short pos) const
{
    return Face(this,pos);
}


template<class T,class S,class U>
inline typename FastOctree<T,S,U>::VertexPtr
FastOctree<T,S,U>::Cell::createRootVertex( const FloatingPointType x, const FloatingPointType y, const FloatingPointType z )
{
    //std::cerr << "createRootVertex\n";
    VertexPtr u = new Vertex( x,y,z, NULL );
    Ensure( u->isFree() );
    return u;
}
template<class T,class S,class U>
inline typename FastOctree<T,S,U>::VertexPtr
FastOctree<T,S,U>::Cell::createLinkedVertex( VertexPtr v1, VertexPtr v2 )
{
    //std::cerr << "createLinkedVertex3 : " << v1->getPosition() << " and " << v2->getPosition() << "\n";
    VertexPtr u = createFreeVertex( v1, v2 );
    u->hardLinkIt( v1, v2 );
    Ensure( !u->isFree() );
    return u;
}
template<class T,class S,class U>
inline typename FastOctree<T,S,U>::VertexPtr
FastOctree<T,S,U>::Cell::createLinkedVertex( VertexPtr v1, VertexPtr v2, VertexPtr v3, VertexPtr v4 )
{
    //std::cerr << "createLinkedVertex2\n";
    VertexPtr u = createFreeVertex( v1, v2, v3, v4 );
    u->hardLinkIt(v1,v2,v3,v4);
    Ensure( !u->isFree() );
    return u;
}



template<class T,class S,class U>
inline typename FastOctree<T,S,U>::VertexPtr
FastOctree<T,S,U>::Cell::createFreeVertex(const U& v)
{
    //std::cerr << "createFreeVertex with Vec " << v << "\n";
    VertexPtr u;
    u = new Vertex( v,
                    vertices_[0], vertices_[1], vertices_[2], vertices_[3],
                    vertices_[4], vertices_[5], vertices_[6], vertices_[7], this );
    Ensure( u->isFree() );
    return u;
}
template<class T,class S,class U>
inline typename FastOctree<T,S,U>::VertexPtr
FastOctree<T,S,U>::Cell::createFreeVertex(const FloatingPointType x,
        const FloatingPointType y,
        const FloatingPointType z)
{
    //std::cerr << "createFreeVertex2\n";
    return createFreeVertex(U(x,y,z));
}
template<class T,class S,class U>
inline typename FastOctree<T,S,U>::VertexPtr
FastOctree<T,S,U>::Cell::createFreeVertex( VertexPtr v1, VertexPtr v2 )
{
    //  std::cerr << "createFreeVertex 2 parents : " << v1->getPosition() << " and " << v2->getPosition() << "\n";
    VertexPtr u = createFreeVertex( (v1->getPosition() + v2->getPosition() )/2.0 );
    u->softLinkIt(v1,v2);
    return u;
}
template<class T,class S,class U>
inline typename FastOctree<T,S,U>::VertexPtr
FastOctree<T,S,U>::Cell::createFreeVertex( VertexPtr v1, VertexPtr v2, VertexPtr v3, VertexPtr v4 )
{
    //  std::cerr << "createFreeVertex 4 parents\n";
    VertexPtr u = createFreeVertex( (v1->getPosition()+v2->getPosition()+v3->getPosition()+v4->getPosition())/4.0  );
    u->softLinkIt(v1,v2,v3,v4);
    return u;
}
template<class T,class S,class U>
inline typename FastOctree<T,S,U>::VertexPtr
FastOctree<T,S,U>::Cell::createFreeVertex( VertexPtr v1, VertexPtr v2, VertexPtr v3, VertexPtr v4, VertexPtr v5, VertexPtr v6, VertexPtr v7, VertexPtr v8 )
{
    //  std::cerr << "createFreeVertex 8 parents\n";
    VertexPtr u = createFreeVertex( (v1->getPosition()+v2->getPosition()+v3->getPosition()+v4->getPosition()+v5->getPosition()+v6->getPosition()+v7->getPosition()+v8->getPosition())/8.0  );
    u->softLinkIt(v1,v2,v3,v4,v5,v6,v7,v8);
    return u;
}


template<class T,class S,class U>
inline void FastOctree<T,S,U>::Cell::removeVertex( typename FastOctree<T,S,U>::VertexPtr vp )
{
    // This stuff is done in the destrcutor of Vertex
    //vp->unregisterCell(this);
    //delete vp;
    toDelete_.push_back(vp);
}

template<class T,class S,class U>
inline typename FastOctree<T,S,U>::VertexPtr&
FastOctree<T,S,U>::Cell::vertexPtrAtCenterOfFace(unsigned short face)
{
    // Considering the face f of a cell, its center point is vertex
    // center_of_face[f][1] of its child center_of_face[f][0]. Four such pairs
    // exist, the table just gives one because since all 4 children of a cell
    // exist at the same time, they are equivalent.  The sum of first and
    // second elements of such pairs is a constant (indicated below in
    // comments).
    static const unsigned short centerOfFace[6][2] =
        {
            {
                0, 6
            }
            , // or { 2,4 } or opposed (sum = 6)
            { 1, 7 }, // or { 3,5 } or opposed (sum = 8)
            { 0, 5 }, // or { 1,4 } or opposed (sum = 5)
            { 2, 7 }, // or { 3,6 } or opposed (sum = 9)
            { 0, 3 }, // or { 1,2 } or opposed (sum = 3)
            { 4, 7 }  // or { 5,6 } or opposed (sum = 11)
        };
    const unsigned short* i = centerOfFace[face];
    return child(i[0])->vertices_[i[1]];
}
template<class T,class S,class U>
inline typename FastOctree<T,S,U>::VertexPtr&
FastOctree<T,S,U>::Cell::vertexPtrAtCenterOfEdge(unsigned short edge)
{
    // Considering the edge e of a cell, its center point is vertex
    // center_of_edge[e][1] of its child center_of_edge[f][0]. Two such pairs
    // exist, the table just gives one because since all 4 children of a cell
    // exist at the same time, they are equivalent.  Such pairs are symmetric.
    // Table is checked.
    static const unsigned short centerOfEdge[12][2] =
        {
            {
                0, 1
            },
            { 2, 3 },
            { 4, 5 },
            { 6, 7 },
            { 0, 2 },
            { 4, 6 },
            { 1, 3 },
            { 5, 7 },
            { 0, 4 },
            { 1, 5 },
            { 2, 6 },
            { 3, 7 }
        };
    const unsigned short* i = centerOfEdge[edge];
    return child(i[0])->vertices_[i[1]];
}
template<class T,class S,class U>
typename FastOctree<T,S,U>::Cell*
FastOctree<T,S,U>::Cell::cellSharingFace(unsigned short face) const
{
    // If the cell sharing FACE would be a brother (which we can say
    // from pos and FACE's index) then it always exist and we
    // query it from the father.
    // Else we consider the father's face that contains FACE (it
    // has the same index!) and ask the father to find a cell that would
    // share it. It would be an "uncle" of (*this).
    // We ask the uncle for the child who shares FACE. If exists, such a
    // child would be a cousin for (*this).
    if (father() == NULL)
    {
        //      std::cerr << "father is NULL for face " << face << "\n";
        return NULL;
    }
    if (hasBrotherAlongFace[pos_][face])
    {
        //      std::cerr << "Looking for my father's child for face " << face << " child (" <<alongFace[pos_][face]<< ")\n";
        return father()->child(alongFace[pos_][face]);
    }
    const Cell* uncle = father()->cellSharingFace(face);
    //  std::cerr << "Uncle is " << uncle << "\n";
    return uncle == NULL ? NULL : uncle->child(alongFace[pos_][face]);
}

template<class T,class S,class U>
bool FastOctree<T,S,U>::Cell::faceOnRootFace( unsigned short face ) const
{
    if( father() == NULL )
        return true;
    else if( hasBrotherAlongFace[pos_][face] )
        return false;
    else
        return father()->faceOnRootFace(face);
}
template<class T,class S,class U>
bool FastOctree<T,S,U>::Cell::edgeOnRootFace( unsigned short edge ) const
{
    if( father() == NULL )
    {
        //      std::cerr << "father is NULL\n";
        return false;
    //return true;
    }
    //  std::cerr << "Face0 : " << faceAlongEdge[edge][0] << " Face1 : " <<  faceAlongEdge[edge][1] << "\n";
    // For each edge, it has at maximum 1 face not shared by a brother for this edge
    if( !hasBrotherAlongFace[pos_][faceAlongEdge[edge][0]] )
    {
        //      std::cerr << "I don't have a brother on face0 " << faceAlongEdge[edge][0] << "\n";
        // Then if this face is on a root's face
        return father()->faceOnRootFace(faceAlongEdge[edge][0]);
    }
    if( !hasBrotherAlongFace[pos_][faceAlongEdge[edge][1]] )
    {
        //      std::cerr << "I don't have a brother on face1 " << faceAlongEdge[edge][1] << "\n";
        // Then if this face is on a root's face
        return father()->faceOnRootFace(faceAlongEdge[edge][1]);
    }

    //  std::cerr << "This is an inside edge\n";
    // This is an inside edge
    return false;
}

template<class T,class S,class U>
bool FastOctree<T,S,U>::Cell::edgeOnRootEdge( unsigned short edge ) const
{
    if( father() == NULL )
        return true;
    if( hasOnFatherEdge[pos_][edge] )
        // We're sharing an edge with our father
        return father()->edgeOnRootEdge(edge);
    else
        // This is an inside edge
        return false;
}

template<class T,class S,class U>
bool FastOctree<T,S,U>::Cell::hasUncleSharingFace(unsigned short face) const
{
    if (father() == NULL)
    {
        return false;
    }
    if( hasBrotherAlongFace[pos_][face] )
        return false;
    if( father()->cellSharingFace(face) == NULL )
        return false;
    else
        return true;
}

/*
 * Get the brother (has the same father), cousin (at the same depth) or uncle (no children and deepest)
 * sharing face
 */
template<class T,class S,class U>
typename FastOctree<T,S,U>::Cell*
FastOctree<T,S,U>::Cell::uncleSharingFace(unsigned short face) const
{
    /* First, go up and find our parent that is a brother of
     * our "uncle", if no such brother exists, return null
     */
    const Cell* parent = this;
    std::deque<unsigned int> posList;

    //std::cerr << "Face is " << face << "\n";
    //std::cerr << "Pos is " << parent->pos_ << "\n";
    while( (parent->father() != NULL) && !hasBrotherAlongFace[parent->pos_][face] )
    {
        //std::cerr << "First it\n";
        posList.push_front(parent->pos_);
        parent = parent->father();
        //std::cerr << "Have pushed " << posList.front() << "\n";
        // Memorize our position
    }

    if( parent->father() == NULL )
    {
        //std::cerr << "Parent is NULL\n";
        // No neighbour
        return NULL;
    }

    // Now descend back to find the touching cell
    //std::cerr << "parent pos is " << parent->pos_ << "\n";
    //std::cerr << "Uncle id on father : " << alongFace[parent->pos_][face] << "\n";
    Cell *uncle = parent->father()->child(alongFace[parent->pos_][face]);

    while( !uncle->isLeaf() && (uncle->data_._depth < this->data_._depth) )
    {
        //std::cerr << "Have poped " << posList.front() << "\n";
        uncle = uncle->child( alongFace[posList.front()][face] );
        //std::cerr << "\tchild ID is " << alongFace[posList.front()][face] << "\n";
        posList.pop_front();
    }

    //std::cerr << "Uncle pos is " << uncle->pos_ << "\n";
    return uncle;
}

template<class T,class S,class U>
bool FastOctree<T,S,U>::Cell::hasUncleSharingEdge(unsigned short edge) const
{
    if (father() == NULL)
    {
        return false;
    }
    if ( !hasOnFatherEdge[pos_][edge] )
    {
        // We're not on an edge of our father, so we can't have an uncle ALONG this edge
        return false;
    }
    if( father()->cellSharingEdge(edge) != NULL )
        return true;
    else
        return false;
}
template<class T,class S,class U>
template<class O>
O
FastOctree<T,S,U>::Cell::copyChildrenTouchingFace(unsigned short face,
        O output) const
{
    for (unsigned short i=0;i<4;++i)
    {
        Cell* c = child(childrenSharingFace[face][i]);
        if (c != NULL)
        {
            *output++ = c;
            output = c->copyChildrenTouchingFace(face,output);
        }
    }
    return output;
}
template<class T,class S,class U>
template<class O>
O
FastOctree<T,S,U>::Cell::copyCellsTouchingFace(unsigned short face,
        O output) const
{
    // We search for the biggest cell who has a face that contains "face"
    // We remember all uncles crossed along the way
    std::deque<Cell*> uncles;
    const Cell* C = this;
    while (C != NULL && C->father() != NULL &&
            !hasBrotherAlongFace[C->fatherPos()][face])
    {
        Cell* uncle = C->father()->cellSharingFace(face);
        if (uncle != NULL)
        {
            uncles.push_front(uncle);
        }
        C = C->father();
    }
    output = std::copy(uncles.begin(),uncles.end(),output);
    // We now search for cell sharing face if it exists and if so, we
    // add all its children touching the alter ego face
    Cell* D = cellSharingFace(face);
    if (D != NULL)
    {
        *output++ = D;
        output = D->copyChildrenTouchingFace(alterEgoFace[face],output);
    }
    return output;
}
template<class T,class S,class U>
typename FastOctree<T,S,U>::Cell*
FastOctree<T,S,U>::Cell::cellSharingEdge(unsigned short edge) const
{
    // If the cell sharing EDGE would be a brother (which we can say
    // from pos and EDGE's index) then it always exist and we
    // query it from the father.

    // Else we must request it to a brother of the father (that is an
    // uncle for (*this)). There can be 2 different cases: either the
    // edge lies inside a father's face and then we must query the cell
    // that shares this face. Or it lies on a father's edge and we must
    // query the cell that shares this supporting edge.
    if (father() == NULL)
    {
        return NULL;
    }
    if (hasBrotherAlongEdge[pos_][edge])
    {
        return father()->child(alongEdge[pos_][edge]);
    }
    const Cell* uncle;
    if (hasOnFatherEdge[pos_][edge])
    {
        uncle = father()->cellSharingEdge(edge);
    }
    else
    {
        uncle = father()->cellSharingFace(supportingFace[pos_][edge]);
    }
    return uncle == NULL ? NULL : uncle->child(alongEdge[pos_][edge]);
}
template<class T,class S,class U>
bool
FastOctree<T,S,U>::Cell::contains(const U& p) const
{

    U pos = p;
    U vPos[8];
    for( unsigned int v = 0 ; v<8 ; ++v )
        vPos[v] = vertex(v)->getPosition();

    // For each face
    for( unsigned int f=0 ; f<6 ; ++f )
    {
        //std::cerr << "For face " << f << "\n";
        // For each possibility, it must at least be in one
        bool isGoodForFace = false;
        for( unsigned int p=0 ; p<4 ; ++p )
            //for( unsigned int p=0 ; p<1 ; ++p )
        {
            //std::cerr << "p : " << p << "next : " << (p+1)%4 << " prev : " << abs((p-1)%4) << "\n";
            //std::cerr << "point 0 : " << faceVertices[f][p] << ", 1 : " << faceVertices[f][(p+1)%4] <<
            //  ", 2 : " << faceVertices[f][abs((p-1)%4)] << "\n";
            //  getchar();
            U v1 = vPos[faceVertices[f][(p+1)%4]] - vPos[faceVertices[f][p]];
            U v2 = vPos[faceVertices[f][abs((p-1)%4)]] - vPos[faceVertices[f][p]];
            U v3 = v1 ^ v2;
            FloatingPointType sp = (pos-vPos[faceVertices[f][p]]) * v3;
            //std::cerr << "sp is " << sp << "\n";
            if( sp <= 0.000000001 )
            {
                isGoodForFace = true;
                break;
            }
        }
        if( !isGoodForFace )
            return false;
    }

    // Every face is containing it
    return true;
    /*
      const U O = *vertices_[0];
      const U  I = (*(vertices_[1])-O);
      const U  J = (*(vertices_[2])-O);
      const U  K = (*(vertices_[4])-O);
      const U  M = U(p[0],p[1],p[2])-O;
      FloatingPointType ps;
      ps = (M*I)/(I*I);
      if (ps < 0.0f || ps > 1.0f)
      {
        return false;
      }
      ps = (M*J)/(J*J);
      if (ps < 0.0f || ps > 1.0f)
      {
        return false;
      }
      ps = (M*K)/(K*K);
      if (ps < 0.0f || ps > 1.0f)
      {
        return false;
      }
      return true;
        */
}
template<class T,class S,class U>
FastOctree<T,S,U>::Cell::Cell( const U& minP,const FloatingPointType& size,
                               const S& defaultData)
        : data_(defaultData),father_(NULL),pos_(0)
{
    Cell** c = children_;
    *c++ = NULL;
    *c++ = NULL;
    *c++ = NULL;
    *c++ = NULL;
    *c++ = NULL;
    *c++ = NULL;
    *c++ = NULL;
    *c++ = NULL;
    // Creates the eight vertex vertices
    VertexPtr* p = vertices_;

    FloatingPointType *minPValues = minP.get_FloatingPointTypePointerCopy();
    (*p++) = createRootVertex(minPValues[0]+0.0f,minPValues[1]+0.0f,minPValues[2]+0.0f);
    (*p++) = createRootVertex(minPValues[0]+size,minPValues[1]+0.0f,minPValues[2]+0.0f);
    (*p++) = createRootVertex(minPValues[0]+0.0f,minPValues[1]+size,minPValues[2]+0.0f);
    (*p++) = createRootVertex(minPValues[0]+size,minPValues[1]+size,minPValues[2]+0.0f);
    (*p++) = createRootVertex(minPValues[0]+0.0f,minPValues[1]+0.0f,minPValues[2]+size);
    (*p++) = createRootVertex(minPValues[0]+size,minPValues[1]+0.0f,minPValues[2]+size);
    (*p++) = createRootVertex(minPValues[0]+0.0f,minPValues[1]+size,minPValues[2]+size);
    (*p++) = createRootVertex(minPValues[0]+size,minPValues[1]+size,minPValues[2]+size);
    free( minPValues );

    p = vertices_;
    (p++)->registerCell( this );
    (p++)->registerCell( this );
    (p++)->registerCell( this );
    (p++)->registerCell( this );
    (p++)->registerCell( this );
    (p++)->registerCell( this );
    (p++)->registerCell( this );
    (p++)->registerCell( this );

    for( unsigned short v=0 ; v<8 ; ++v )
    {
        Require( !vertices_[v]->hasMainCell() );
        vertices_[v]->setMainCell( this, v );
    }
    
    // Root cell has no neighbour
    for( unsigned short i=0 ; i<6 ; ++i )
    {
        _faceNeighbours[i] = NULL;
    }

}
template<class T,class S,class U>
FastOctree<T,S,U>::Cell::Cell( const U& minP,
                               const FloatingPointType& xsize,
                               const FloatingPointType& ysize,
                               const FloatingPointType& zsize,
                               const S& defaultData)
        : data_(defaultData),father_(NULL),pos_(0)
{
    Cell** c = children_;
    *c++ = NULL;
    *c++ = NULL;
    *c++ = NULL;
    *c++ = NULL;
    *c++ = NULL;
    *c++ = NULL;
    *c++ = NULL;
    *c++ = NULL;

    // Creates the eight vertex points
    VertexPtr* p = vertices_;
    const FloatingPointType *minPValues = minP;
    (*p++) = createRootVertex(minPValues[0]+ 0.0f,minPValues[1]+ 0.0f,minPValues[2]+ 0.0f);
    (*p++) = createRootVertex(minPValues[0]+xsize,minPValues[1]+ 0.0f,minPValues[2]+ 0.0f);
    (*p++) = createRootVertex(minPValues[0]+ 0.0f,minPValues[1]+ysize,minPValues[2]+ 0.0f);
    (*p++) = createRootVertex(minPValues[0]+xsize,minPValues[1]+ysize,minPValues[2]+ 0.0f);
    (*p++) = createRootVertex(minPValues[0]+ 0.0f,minPValues[1]+ 0.0f,minPValues[2]+zsize);
    (*p++) = createRootVertex(minPValues[0]+xsize,minPValues[1]+ 0.0f,minPValues[2]+zsize);
    (*p++) = createRootVertex(minPValues[0]+ 0.0f,minPValues[1]+ysize,minPValues[2]+zsize);
    (*p++) = createRootVertex(minPValues[0]+xsize,minPValues[1]+ysize,minPValues[2]+zsize);

    p = vertices_;
    (*p++)->registerCell( this );
    (*p++)->registerCell( this );
    (*p++)->registerCell( this );
    (*p++)->registerCell( this );
    (*p++)->registerCell( this );
    (*p++)->registerCell( this );
    (*p++)->registerCell( this );
    (*p++)->registerCell( this );

    for( unsigned short v=0 ; v<8 ; ++v )
    {
        Require( !vertices_[v]->hasMainCell() );
        vertices_[v]->setMainCell( this, v );
    }
    
    for( unsigned short i=0 ; i<6 ; ++i )
    {
        _faceNeighbours[i] = NULL;
    }    

}
template<class T,class S,class U>
FastOctree<T,S,U>::Cell::Cell(Cell& f,unsigned short pos,
                              FastOctree<T,S,U>::VertexPtr vertices[27],
                              const S& defaultData)
        : data_(defaultData),father_(&f),pos_(pos)
{
    Cell** c = children_;
    *c++ = NULL;
    *c++ = NULL;
    *c++ = NULL;
    *c++ = NULL;
    *c++ = NULL;
    *c++ = NULL;
    *c++ = NULL;
    *c++ = NULL;
    // Sets the 8 points from the father
    const unsigned short I = pos_&0x1;
    const unsigned short J = (pos_>>1)&0x1;
    const unsigned short K = (pos_>>2)&0x1;
    const unsigned short I0 =   I,I1 =    1+I;
    const unsigned short J0 = 3*J,J1 = 3*(1+J);
    const unsigned short K0 = 9*K,K1 = 9*(1+K);
    VertexPtr* p = vertices_;
    *p++ = vertices[I0+J0+K0];
    *p++ = vertices[I1+J0+K0];
    *p++ = vertices[I0+J1+K0];
    *p++ = vertices[I1+J1+K0];
    *p++ = vertices[I0+J0+K1];
    *p++ = vertices[I1+J0+K1];
    *p++ = vertices[I0+J1+K1];
    *p++ = vertices[I1+J1+K1];

    // Do not register THIS vertex as it's already registerd by our father
    // It's equivalent to our father's position for our vertices
    //  const unsigned short vertexOnBorder = pos_;

    /*
     for( unsigned short v = 0 ; v<8 ; ++v )
        if( v != vertexOnBorder )
        {
            //std::cerr << "Adding one cell " << v << "\n";
    //          if( !vertices_[v]->isConnected(this) )
                vertices_[v]->registerCell(this);
    //          else
    //              std::cerr <<"Constructor : Error register3\n";
    //          std::cerr << "Finished\n";
        }
        */

    for( unsigned short v = 0 ; v<8 ; ++v )
    {
        vertices_[v]->registerCell(this);
    }

    /*
        std::cerr << "Cell constructor : " << *this << "\n";
        std::cerr << "Registering vertices :\n";
      for( unsigned short v = 0 ; v<8 ; ++v )
        {
            vertices_[v]->registerCell(this);
            std::cerr << "Vertice[" <<v << "] is connected to :\n";
            for( unsigned short cellId=0 ; cellId<vertex(v)->nConnectedCells() ; ++cellId )
            {
                std::cerr << "\t" << *vertex(v)->connectedCell(cellId) << "\n";
            }
        }
        */

    for( unsigned short v=0 ; v<8 ; ++v )
    {
        if( !vertices_[v]->hasMainCell() )
            vertices_[v]->setMainCell( this, v );
    }
    
    for( unsigned short i=0 ; i<6 ; ++i )
    {
        _faceNeighbours[i] = NULL;
    }    

}
template<class T,class S,class U>
FastOctree<T,S,U>::Cell::~Cell()
{
    //std::cerr << "~Cell() of cell " << *this << "\n";

    Require( isLeaf() );

    /*
     * With simplify, every child is freed !
     */
    if( !isLeaf() )
        simplify();

    if( father() == NULL )
    {
        for( unsigned short v=0 ; v<8 ; ++v )
        {
            vertices_[v]->unregisterCell(this);
            delete vertices_[v];
            vertices_[v] = NULL;
        }
    }

    //std::cerr << "~Cell() end of cell " << *this << "\n";

}




template<class T,class S,class U>
inline int
FastOctree<T,S,U>::Cell::memorySize() const
{
    // We do not count the memory size of toDelete_ deque because we
    // intend o supress it (in todo list) but we use it to find the
    // number of "owned" vertices.
    return
        sizeof(S)+
        sizeof(Cell*)*8+
        sizeof(Cell*)+
        sizeof(unsigned short)+
        sizeof(VertexPtr)*8+
        sizeof(Vertex)*toDelete_.size();
}
//************************************************************
// Implementation of Face
//************************************************************
template<class T,class S,class U>
inline typename FastOctree<T,S,U>::Vertex*
FastOctree<T,S,U>::Face::vertex(unsigned short i) const
{
    return cell_->vertex(FastOctree<T,S,U>::faceVertices[pos_][i]);
}
template<class T,class S,class U>
inline unsigned short
FastOctree<T,S,U>::Face::pos() const
{
    return pos_;
}
template<class T,class S,class U>
inline typename FastOctree<T,S,U>::Vertex*
FastOctree<T,S,U>::Face::vertexAtCenter() const
{
    return cell_->vertexPtrAtCenterOfFace(i);
}
template<class T,class S,class U>
inline const FloatingPointType*
FastOctree<T,S,U>::Face::normal() const
{
    static const FloatingPointType normals[6][3] =
        {
            {
                -1.0f, 0.0f, 0.0f
            },
            {  1.0f, 0.0f, 0.0f },
            {  0.0f,-1.0f, 0.0f },
            {  0.0f, 1.0f, 0.0f },
            {  0.0f, 0.0f,-1.0f },
            {  0.0f, 0.0f, 1.0  }
        };
    return normals[pos_];
}
template<class T,class S,class U>
inline FastOctree<T,S,U>::Face::Face(const FastOctree<T,S,U>::Cell* cell,
                                     unsigned short pos)
        : cell_(cell),pos_(pos)
{}
//************************************************************
// Implementation of FastOctree<T,S,U>
//************************************************************
template<class T,class S,class U>
FastOctree<T,S,U>::FastOctree(FastOctree<T,S,U>::Cell* root)
        : root_(root)
{}
template<class T,class S,class U>
FastOctree<T,S,U>::FastOctree(const U& min,
                              const FloatingPointType& size,const S& defaultData)
{
    root_ = new Cell(min,size,defaultData);
}
template<class T,class S,class U>
FastOctree<T,S,U>::FastOctree(const U& min,
                              const FloatingPointType& sx,const FloatingPointType& sy,const FloatingPointType& sz,
                              const S& defaultData)
{
    root_ = new Cell(min,sx,sy,sz,defaultData);
}
template<class T,class S,class U>
FastOctree<T,S,U>::~FastOctree()
{
    delete root_;
}
template<class T,class S,class U>
inline typename FastOctree<T,S,U>::Cell*
FastOctree<T,S,U>::root() const
{
    return root_;
}
template<class T,class S,class U>
inline unsigned short
FastOctree<T,S,U>::vertexIndex(unsigned short i,unsigned short j,unsigned short k)
{
    return i+2*j+4*k;
}
template<class T,class S,class U>
inline unsigned short
FastOctree<T,S,U>::childIndex(unsigned short i,unsigned short j,unsigned short k)
{
    return i+2*j+4*k;
}










template<class T, class S, class U>
FastOctree<T,S,U>::dfs_iterator::dfs_iterator( Cell* cStart, bool (*selectF)(Cell*), void (*enterF)(Cell*), void (*leaveF)(Cell*) ) :
        _selectF(selectF), _enterF(enterF), _leaveF(leaveF)
{
    dfs_iterator_data data = {false,cStart};
    _deque.push_front(data);
}

template<class T, class S, class U>
FastOctree<T,S,U>::dfs_iterator::~dfs_iterator()
{}


/*
template<class T, class S, class U>
void FastOctree<T,S,U>::dfs_iterator::enter( Cell* cell )
{
}
template<class T, class S, class U>
void FastOctree<T,S,U>::dfs_iterator::leave( Cell* cell )
{
}
*/

template<class T, class S, class U>
bool FastOctree<T,S,U>::dfs_iterator::empty( )
{
    return _deque.empty();
}

template<class T, class S, class U>
typename FastOctree<T,S,U>::Cell*
FastOctree<T,S,U>::dfs_iterator::operator*()
{
    Require( !empty() );
    return _deque.front()._cell;
}

template<class T, class S, class U>
typename FastOctree<T,S,U>::Cell*
FastOctree<T,S,U>::dfs_iterator::operator++()
{
    Require( !empty() );
    // First get the last item of the list

    dfs_iterator_data lastIt = _deque.front();

    if( !lastIt._entered )
    {
        // Mark it entered
        _deque.front()._entered = true;

        //
        // First push its children
        //
        if( !lastIt._cell->isLeaf() )
        {
            // Then push our children
            for( short c=7 ; c>=0 ; --c )
            {
                dfs_iterator_data data = {false,lastIt._cell->child(c)};
                _deque.push_front( data );
            }
        }


        if( (_selectF != NULL && _selectF(lastIt._cell)) || (_selectF == NULL && select(lastIt._cell)) )
        {
            // Call the enter function
            if( _enterF != NULL )
            {
                _enterF(lastIt._cell);
            }
            else
            {
                enter( lastIt._cell );
            }
        }

    }
    else
    {
        // We have made all our children so call the leave function if needed
        if( (_selectF != NULL && _selectF(lastIt._cell)) || (_selectF == NULL && select(lastIt._cell)) )
        {
            if( _leaveF != NULL )
            {
                _leaveF( lastIt._cell );
            }
            else
            {
                leave( lastIt._cell );
            }
        }

        _deque.pop_front();
    }

    return lastIt._cell;
}






// template<class T, class S, class U>
// class FastOctree<T,S,U>::dfs_leaf_iterator
// {
//  public:
//      dfs_leaf_iterator( Cell*, void (*workF)(Cell*)=NULL );
//      virtual ~dfs_leaf_iterator();
//
//      Cell* operator++();
//      bool empty();
//
//      Cell* operator*();
//
//  protected:
//      // What to do with a node
//      virtual void work(Cell*){};
//
//
//  private:
//      void (*_workF)(Cell*);
//
//      std::deque<dfs_iterator_data> _deque;
// };

template<class T, class S, class U>
FastOctree<T,S,U>::dfs_leaf_iterator::dfs_leaf_iterator( Cell* cStart, void (*workF)(Cell*) ) :
        _workF(workF)
{
    dfs_iterator_data data = {false,cStart};
    _deque.push_front(data);
}

template<class T, class S, class U>
FastOctree<T,S,U>::dfs_leaf_iterator::~dfs_leaf_iterator()
{}


template<class T, class S, class U>
bool FastOctree<T,S,U>::dfs_leaf_iterator::empty( )
{
    return _deque.empty();
}

template<class T, class S, class U>
typename FastOctree<T,S,U>::Cell*
FastOctree<T,S,U>::dfs_leaf_iterator::operator*()
{
    Require( !empty() );
    return _deque.front()._cell;
}

template<class T, class S, class U>
typename FastOctree<T,S,U>::Cell*
FastOctree<T,S,U>::dfs_leaf_iterator::operator++()
{
    Require( !empty() );
    // First get the last item of the list

    dfs_iterator_data lastIt = _deque.front();

    if( !lastIt._entered )
    {
        // Mark it entered
        _deque.front()._entered = true;

        //
        // First push its children
        //
        if( !lastIt._cell->isLeaf() )
        {
            // Then push our children
            for( short c=7 ; c>=0 ; --c )
            {
                dfs_iterator_data data = {false,lastIt._cell->child(c)};
                _deque.push_front( data );
            }
        }
        else
        {
            // Call the enter function
            if( _workF != NULL )
            {
                _workF(lastIt._cell);
            }
            else
            {
                work( lastIt._cell );
            }
        }

    }
    else
    {
        _deque.pop_front();
    }

    return lastIt._cell;
}





//=============================================================================================
template<class T,class S,class U>
FastOctree<T,S,U>::Cell::vertex_iterator::vertex_iterator( FastOctree<T,S,U>::Cell *cell )
        : dfs_iterator(cell)
        , _currentCell(cell)
        , _currentVertexNumber(0)
{
    //cerr<<"FastOctree<T,S,U>::Cell::vertex_iterator::vertex_iterator, searching next vertex"<<endl;
    _nextVertex = next_vertex();
/*    if( !_nextVertex )
        cerr<<"FastOctree<T,S,U>::Cell::vertex_iterator::vertex_iterator, no vertex"<<endl;*/
}

template<class T,class S,class U>
FastOctree<T,S,U>::Cell::vertex_iterator::~vertex_iterator()
{
}

template<class T,class S,class U>
typename FastOctree<T,S,U>::Vertex* FastOctree<T,S,U>::Cell::vertex_iterator::operator++()
{
    FastOctree<T,S,U>::Vertex* result = _nextVertex;
    _nextVertex = next_vertex();
    return result;
}

template<class T,class S,class U>
typename FastOctree<T,S,U>::Vertex* FastOctree<T,S,U>::Cell::vertex_iterator::next_vertex()
{
    FastOctree<T,S,U>::Vertex* found = 0;
    while( !found && !dfs_iterator::empty() )
    {
        //cerr<<"FastOctree<T,S,U>::Vertex* FastOctree<T,S,U>::Cell::vertex_iterator::next_vertex() a"<<endl;
        while( _currentVertexNumber<8 && _currentCell->vertex(_currentVertexNumber)->getMainCell()!=_currentCell )
        {
            _currentVertexNumber++;
            //cerr<<"FastOctree<T,S,U>::Vertex* FastOctree<T,S,U>::Cell::vertex_iterator::next_vertex() b"<<endl;
        }
        if( _currentVertexNumber<8 ) // found a vertex
        {
            //cerr<<"FastOctree<T,S,U>::Vertex* FastOctree<T,S,U>::Cell::vertex_iterator::next_vertex() c"<<endl;
            found = _currentCell->vertex(_currentVertexNumber);
            //cerr<<"FastOctree<T,S,U>::Vertex* FastOctree<T,S,U>::Cell::vertex_iterator::next_vertex() found vertex number "<< _currentVertexNumber << endl;
            _currentVertexNumber++;
        }
        else // we must go to the next cell
        {
            _currentCell = dfs_iterator::operator ++();
            _currentCell = dfs_iterator::operator ++();
            _currentVertexNumber = 0;
            //cerr<<"FastOctree<T,S,U>::Vertex* FastOctree<T,S,U>::Cell::vertex_iterator::next_vertex() change cell ------- "<<endl;
        }
    }
    //cerr<<"------- FastOctree<T,S,U>::Vertex* FastOctree<T,S,U>::Cell::vertex_iterator::next_vertex() in cell "; _currentCell->printPos(cerr); cerr<<endl;
    return found;
}

template<class T,class S,class U>
bool FastOctree<T,S,U>::Cell::vertex_iterator::empty()
{
    return _nextVertex == 0;
}

//=============================================================================================
template<class T,class S,class U>
FastOctree<T,S,U>::Cell::vertex_width_iterator::vertex_width_iterator( FastOctree<T,S,U>::Cell *cell ) :
        _topListId(0), _cell(cell)
{
    for( unsigned short v=0 ; v<8 ; ++v )
        _vLists[_topListId].push_back( _cell->vertex(v) );

}
template<class T,class S,class U>
FastOctree<T,S,U>::Cell::vertex_width_iterator::~vertex_width_iterator()
{}
template<class T,class S,class U>
typename FastOctree<T,S,U>::Vertex* FastOctree<T,S,U>::Cell::vertex_width_iterator::operator++()
{
    Require( !empty() );

    Vertex* res = _vLists[_topListId].front();
    _vLists[_topListId].pop_front();

    for( unsigned int i=0 ; i<res->nChildren() ; ++i )
    {
        if( _insertedList.find(res->child(i)) == _insertedList.end() )
        {
            // Then add it to the other list and to the set
            _vLists[(_topListId+1)%2].push_back( res->child(i) );
            _insertedList.insert( res->child(i) );
        }
    }

    if( empty() )
    {
        // Switch to the (possibly empty) other list
        _topListId = (_topListId+1)%2;
    }

    return res;
}
template<class T,class S,class U>
bool FastOctree<T,S,U>::Cell::vertex_width_iterator::empty()
{
    return _vLists[_topListId].empty();
}










template<class T,class S,class U>
FastOctree<T,S,U>::Cell::vertex_width_neighbours_iterator::vertex_width_neighbours_iterator( FastOctree<T,S,U>::Cell *cell ) :
        _topListId(0), _cell(cell)
{
    for( unsigned short v=0 ; v<8 ; ++v )
    {
        _vLists[_topListId].push_back( _cell->vertex(v) );
    }

    for( unsigned short v=0 ; v<8 ; ++v )
    {
        ConstrainedVertex *vertex = _vLists[_topListId][v];
        for( unsigned short i=0 ; i < vertex->nConnectedCells() ; ++i )
        {
            Cell *connectedCell = vertex->connectedCell( i );
            if( connectedCell->getData()._depth == cell->getData()._depth )
            {
                // Find the corresponding vertex in this new cell
                unsigned short child;
                for( child=0 ; child<8 ; ++child )
                {
                    if( connectedCell->vertex(child) == _vLists[_topListId][v] )
                    {
                        _vLists[_topListId].push_back( connectedCell->vertex( Cell::connectedVertices[child][0] ) );
                        _vLists[_topListId].push_back( connectedCell->vertex( Cell::connectedVertices[child][1] ) );
                        _vLists[_topListId].push_back( connectedCell->vertex( Cell::connectedVertices[child][2] ) );
                        break;
                    }
                }
                Require( child != 8 );
            }
        }
    }
}

template<class T,class S,class U>
FastOctree<T,S,U>::Cell::vertex_width_neighbours_iterator::vertex_width_neighbours_iterator( typename FastOctree<T,S,U>::Vertex* vertex, FastOctree<T,S,U>::Cell *cell, unsigned int vPos ) :
        _topListId(0), _cell(NULL)
{
    _vLists[_topListId].push_back( vertex );

    Cell::vertex_connected_iterator it( vertex, cell, vPos );
    while( !it.empty() )
    {
        _vLists[_topListId].push_back( ++it );
    }

    /*
    for( unsigned short i=0 ; i < vertex->nConnectedCells() ; ++i )
    {
        Cell *connectedCell = vertex->connectedCell( i );
        if( connectedCell->getData()._depth == depth )
        {
            // Find the corresponding vertex in this new cell
            unsigned short child;
            for( child=0 ; child<8 ; ++child )
            {
                if( connectedCell->vertex(child) == vertex )
                {
                    _vLists[_topListId].push_back( connectedCell->vertex( Cell::connectedVertices[child][0] ) );
                    _vLists[_topListId].push_back( connectedCell->vertex( Cell::connectedVertices[child][1] ) );
                    _vLists[_topListId].push_back( connectedCell->vertex( Cell::connectedVertices[child][2] ) );
                    break;
                }
            }
            Require( child != 8 );
        }
    }
    */
}

template<class T,class S,class U>
FastOctree<T,S,U>::Cell::vertex_width_neighbours_iterator::~vertex_width_neighbours_iterator()
{}
template<class T,class S,class U>
typename FastOctree<T,S,U>::Vertex* FastOctree<T,S,U>::Cell::vertex_width_neighbours_iterator::operator++()
{
    Require( !empty() );

    Vertex* res = _vLists[_topListId].front();
    _vLists[_topListId].pop_front();

    for( unsigned int i=0 ; i<res->nChildren() ; ++i )
    {
        if( _insertedList.find(res->child(i)) == _insertedList.end() )
        {
            // Then add it to the other list and to the set
            _vLists[(_topListId+1)%2].push_back( res->child(i) );
            _insertedList.insert( res->child(i) );
        }
    }

    if( empty() )
    {
        // Switch to the (possibly empty) other list
        _topListId = (_topListId+1)%2;
    }

    return res;
}
template<class T,class S,class U>
bool FastOctree<T,S,U>::Cell::vertex_width_neighbours_iterator::empty()
{
    return _vLists[_topListId].empty();
}




































//************************************************************
// Lookup tables
//************************************************************
/*
 * hasBrotherAlongFace[b][f] is true if child number \p b of a cell
 * has a brother along its face number \p f. Use along_face to find the
 * index of such brother.
 * Table is checked.
 */
template<class T,class S,class U>
const bool
FastOctree<T,S,U>::hasBrotherAlongFace[8][6] =
    {
        {
            false,true ,false,true ,false,true
        },
        { true ,false,false,true ,false,true  },
        { false,true ,true ,false,false,true  },
        { true ,false,true ,false,false,true  },
        { false,true ,false,true ,true ,false },
        { true ,false,false,true ,true ,false },
        { false,true ,true ,false,true ,false },
        { true ,false,true ,false,true ,false }
    };
/*
 * hasBrotherAlongEdge[b][e] is true if child number \p b of a cell
 * has a brother along its edge number \p e.
 */
template<class T,class S,class U>
const bool
FastOctree<T,S,U>::hasBrotherAlongEdge[8][12] =
    {
        {
            false,false,false,true ,false,false,false,true ,false,false,false,true
        },
        { false,false,false,true ,false,true ,false,false,false,false,true ,false },
        { false,false,true ,false,false,false,false,true ,false,true ,false,false },
        { false,false,true ,false,false,true ,false,false,true ,false,false,false },
        { false,true ,false,false,false,false,true ,false,false,false,false,true  },
        { false,true ,false,false,true ,false,false,false,false,false,true ,false },
        { true ,false,false,false,false,false,true ,false,false,true ,false,false },
        { true ,false,false,false,true ,false,false,false,true ,false,false,false }
    };
/*
 * alongFace[b][f] returns the child number of the brother of child \p b
 * of a cell along face number \p f.  If such a brother would not exist
 * (indicated by hasBrotherAlongFace[b][f] being false), it has the value
 * of the brother along the face parallel to f for which a brother exists
 * (there is always one).
 * Table is checked.
 */
template<class T,class S,class U>
const unsigned short
FastOctree<T,S,U>::alongFace[8][6] =
    {
        {
            1,1,2,2,4,4
        },
        { 0,0,3,3,5,5 },
        { 3,3,0,0,6,6 },
        { 2,2,1,1,7,7 },
        { 5,5,6,6,0,0 },
        { 4,4,7,7,1,1 },
        { 7,7,4,4,2,2 },
        { 6,6,5,5,3,3 }
    };
/*
 * alongEdge[b][e] returns the child number of the brother of child \p b
 * of a cell along edge number \p e.  If such a brother would not exist
 * (indicated by hasBrotherAlongEdge[b][e] being false), it has the value
 * of the brother along the edge parallel to e for which a brother exists
 * (there is alwyas one).
 */
template<class T,class S,class U>
const unsigned short
FastOctree<T,S,U>::alongEdge[8][12] =
    {
        {
            6,6,6,6,5,5,5,5,3,3,3,3
        },
        { 7,7,7,7,4,4,4,4,2,2,2,2 },
        { 4,4,4,4,7,7,7,7,1,1,1,1 },
        { 5,5,5,5,6,6,6,6,0,0,0,0 },
        { 2,2,2,2,1,1,1,1,7,7,7,7 },
        { 3,3,3,3,0,0,0,0,6,6,6,6 },
        { 0,0,0,0,3,3,3,3,5,5,5,5 },
        { 1,1,1,1,2,2,2,2,4,4,4,4 }
    };
/*
 * alterEgoFace[f] is the index of the face number \p f of a cell C in the
 * cell sharing this face with cell C.
 */
template<class T,class S,class U>
const unsigned short
FastOctree<T,S,U>::alterEgoFace[6] =
    {
        1,0,3,2,5,4
    };
/*
 * supportingFace[c][e] is the number of the cell's face (if any)
 * that contains edge \p e of child number \p c.
 */
template<class T,class S,class U>
const unsigned short
FastOctree<T,S,U>::supportingFace[8][12] =
    {
        {
            6,4,2,1,4,0,4,1,2,2,0,1
        },
        { 6,4,2,1,4,1,4,1,2,2,1,1 },
        { 4,6,1,3,4,0,4,1,0,1,2,3 },
        { 4,6,1,3,4,1,4,1,1,1,3,3 },
        { 2,1,6,5,0,4,1,5,2,2,0,1 },
        { 2,1,6,5,1,5,1,5,2,2,1,1 },
        { 1,3,5,7,0,4,1,5,0,1,2,3 },
        { 1,3,5,7,1,5,1,5,1,1,3,3 }
    };
/*
 * hasOnFatherEdge[c][e] is true if child \p c of a cell shares
 * edge \p e with its father (actually half the one of its father).
 */
template<class T,class S,class U>
const bool
FastOctree<T,S,U>::hasOnFatherEdge[8][12] =
    {
        {
            true ,false,false,false,true ,false,false,false,true ,false,false,false
        },
        { true ,false,false,false,false,false,true ,false,false,true ,false,false },
        { false,true ,false,false,true ,false,false,false,false,false,true ,false },
        { false,true ,false,false,false,false,true ,false,false,false,false,true  },
        { false,false,true ,false,false,true ,false,false,true ,false,false,false },
        { false,false,true ,false,false,false,false,true ,false,true ,false,false },
        { false,false,false,true ,false,true ,false,false,false,false,true ,false },
        { false,false,false,true ,false,false,false,true ,false,false,false,true  }
    };
/*
 * childrenSharingFace[f] are the indexes of the 4 children
 * that lies along face \p f.
 * Warning : each cell id corresponds with an alterego face, meaning :
 * childrenSharingFace[0][i] <=> childrenSharingFace[1][i] in the neighbouring cell !
 */
template<class T,class S,class U>
const unsigned short
FastOctree<T,S,U>::childrenSharingFace[6][4] =
    {
        { 0,2,4,6 }, // 0
        { 1,3,5,7 }, // 1
        { 0,1,4,5 }, // 2
        { 2,3,6,7 }, // 3
        { 0,1,2,3 }, // 4
        { 4,5,6,7 }  // 5
    };
/*
 * faceVertices[f] are the 4 vertices that composes face \p f
 * (in such order as the normal points outward the cell.
 */
template<class T,class S,class U>
const unsigned short
FastOctree<T,S,U>::faceVertices[6][4] =
    {
        {
            0,4,6,2
        },
        { 5,1,3,7 },
        { 0,1,5,4 },
        { 3,2,6,7 },
        { 0,2,3,1 },
        { 6,4,5,7 }
    };

/* From Mathieu :
 * faceAlongEdge[e] are the 2 faces which are sharing the
 * edge \p e.
 */
template<class T,class S,class U>
const unsigned short
FastOctree<T,S,U>::Cell::faceAlongEdge[12][2] =
    {
        {2,4}, // 0
        {3,4}, // 1
        {2,5}, // 2
        {3,5}, // 3
        {0,4}, // 4
        {0,5}, // 5
        {1,4}, // 6
        {1,5}, // 7
        {0,2}, // 8
        {1,2}, // 9
        {0,3}, // 10
        {1,3}  // 11
    };

/* From Mathieu :
 * For each vertex, give us the id of the vertices with whom we're sharing an edge
 * This list is ordered so that the first vertex is the one that don't depend on x, the second on y, third on z
 * X is given by P1-P0, Y by P2-P0 and Z by P4-P0 
 */
template<class T,class S,class U>
const unsigned short
FastOctree<T,S,U>::Cell::connectedVertices[8][3] =
    {
        {
            1,2,4
        },
        {0,3,5},
        {3,0,6},
        {2,1,7},
        {5,6,0},
        {4,7,1},
        {7,4,2},
        {6,5,3}
    };

/* From Mathieu :
 * For each vertex, give us the id of the faces connected
 * This list is ordered so that the first face is the one that don't depend on x, the second on y, third on z
 * X is given by P1-P0, Y by P2-P0 and Z by P4-P0
 */
template<class T,class S,class U>
const unsigned short
FastOctree<T,S,U>::verticesConnectedFaces[8][3] =
    {
        {
            0,2,4
        },
        {1,2,4},
        {0,3,4},
        {1,3,4},
        {0,2,5},
        {1,2,5},
        {0,3,5},
        {1,3,5}
    };

template<class T,class S,class U>
const unsigned short
FastOctree<T,S,U>::directionForVertexOnEdge[8][8] =
    {
        {
            -1,0,1,-1, 2,-1,-1,-1
        },
        {0,-1,-1,1, -1,2,-1,-1},
        {1,-1,-1,0, -1,-1,2,-1},
        {-1,1,0,-1, -1,-1,-1,2},
        {2,-1,-1,-1, -1,0,1,-1},
        {-1,2,-1,-1, 0,-1,-1,1},
        {-1,-1,2,-1, 1,-1,-1,0},
        {-1,-1,-1,2, -1,1,0,-1}
    };


template<class T,class S,class U>
const unsigned short
FastOctree<T,S,U>::directionForVertexOnFace[8][8] =
    {
        {
            -1,-1,-1,2, -1,1,0,-1
        },
        {-1,-1,2,-1, 1,-1,-1,0},
        {-1,2,-1,-1, 0,-1,-1,1},
        {2,-1,-1,-1, -1,0,1,-1},

        {-1,1,0,-1, -1,-1,-1,2},
        {1,-1,-1,0, -1,-1,2,-1},
        {0,-1,-1,1, -1,2,-1,-1},
        {-1,0,1,-1, 2,-1,-1,-1}
    };

}
}

#endif // FASTOCTREE_H

---