Quadtree neighbor search in constant time with QTLCLD

I wish to implement this paper's algorithm for accessing quadtree node neighbors in constant time.

I am facing problems when attempting to access diagonal neighbors (for when the quad is one or more level smaller than the searched neighbor). Example: root->Child(SE)->Child(NE)->GetNeighbor(NW) should return root->Child(NE). However, I get a result of root->Child(NW).

The only problem is diagonal searches in different levels. The other stuff is working correctly; I can find the neighbors on the same level or from smaller level to bigger level without problems.

Here is the code:

#define QUAD_MAX_LEVEL 16
#define QUAD_MAX_UNITS 20

#define SOUTH_WEST 0
#define SOUTH_EAST 1
#define NORTH_WEST 2
#define NORTH_EAST 3

#define NORTH 4
#define WEST 5
#define SOUTH 6
#define EAST 7

// Precalculated QTLCLD direction increments for r = 16 = max level
#define EAST_NEIGHBOR 0x01
#define NORTH_EAST_NEIGHBOR 0x03
#define NORTH_NEIGHBOR 0x02
#define NORTH_WEST_NEIGHBOR 0x55555557
#define WEST_NEIGHBOR 0x55555555
#define SOUTH_WEST_NEIGHBOR 0xFFFFFFFF
#define SOUTH_NEIGHBOR 0xAAAAAAAA
#define SOUTH_EAST_NEIGHBOR 0xAAAAAAAB

#define tx 0x55555555
#define ty 0xAAAAAAAA


class Quad;
typedef std::shared_ptr< Quad > QuadPtr;
typedef std::weak_ptr< Quad > QuadWeakPtr;

class Quad {
public:
    static std::vector< QuadPtr > & s_GetLinearTree() {
        static std::vector< QuadPtr > linearTree( pow( QUAD_MAX_LEVEL, 4 ) );
        return linearTree;
    }

    enum Index { None = 0x00, North = 0x10, West = 0x20, South = 0x40, East = 0x80, NorthWest = 0x31, NorthEast = 0x92, SouthWest = 0x64, SouthEast = 0xC8  };

    Index index;
    int position;
    unsigned int level;
    int neighborSizes[8];

    Rectangle quadrant;
    bool hasChildren;

    QuadPtr parent;
    std::vector< QuadPtr > quads;
    std::list< UnitWeakPtr > units;

    Quad( Index p_index, const Rectangle &p_rect, unsigned int p_level, int p_position, QuadPtr p_parent = QuadPtr() ) : quadrant( p_rect ), quads( 4 ), parent( p_parent ) {
        index = p_index;
        position = p_position;
        hasChildren = false;
        level = p_level;

        // standard value zero
        for( int i = 0; i < 8; i++ )
            neighborSizes[i] = 0;

        if( parent.get() != NULL )
            calcNeighborsSizes( InxToI( p_index ) );
    }

    void Clear() {
        units.clear();

        for( auto quad : quads ) {
            if( quad.get() != NULL )
                quad->Clear();      
        }

        quads.clear();
    }

    int getIndex( const Rectangle &p_rect ) {
        if( !hasChildren ) {
            if( level < QUAD_MAX_LEVEL )
                Split();
            else 
                return 0;
        }

        int index = None;

        if( quads[NORTH_WEST]->quadrant.isContaining( p_rect.p0 ) || quads[NORTH_WEST]->quadrant.isContaining( p_rect.p1 ) || 
            quads[NORTH_WEST]->quadrant.isContaining( p_rect.p2 ) || quads[NORTH_WEST]->quadrant.isContaining( p_rect.p3 ) ) {
                index = index | NorthWest;
        }

        if( quads[NORTH_EAST]->quadrant.isContaining( p_rect.p0 ) || quads[NORTH_EAST]->quadrant.isContaining( p_rect.p1 ) || 
            quads[NORTH_EAST]->quadrant.isContaining( p_rect.p2 ) || quads[NORTH_EAST]->quadrant.isContaining( p_rect.p3 ) ) {
                index = index | NorthEast;
        }

        if( quads[SOUTH_WEST]->quadrant.isContaining( p_rect.p0 ) || quads[SOUTH_WEST]->quadrant.isContaining( p_rect.p1 ) || 
            quads[SOUTH_WEST]->quadrant.isContaining( p_rect.p2 ) || quads[SOUTH_WEST]->quadrant.isContaining( p_rect.p3 ) ) {
                index = index | SouthWest;
        }

        if( quads[SOUTH_EAST]->quadrant.isContaining( p_rect.p0 ) || quads[SOUTH_EAST]->quadrant.isContaining( p_rect.p1 ) || 
            quads[SOUTH_EAST]->quadrant.isContaining( p_rect.p2 ) || quads[SOUTH_EAST]->quadrant.isContaining( p_rect.p3 ) ) {
                index = index | SouthEast;
        }

        return index;
    }

    void Insert( UnitPtr p_unit ) {
        if( p_unit.get() == NULL )
            return;

        int index = getIndex( p_unit->boundingBox->box );

        if( index != 0 ) {
            if( NorthWest == ( index & NorthWest ) )
                quads[NORTH_WEST]->Insert( p_unit );

            if( NorthEast == ( index & NorthEast ) )
                quads[NORTH_EAST]->Insert( p_unit );

            if( SouthWest == ( index & SouthWest ) )
                quads[SOUTH_WEST]->Insert( p_unit );

            if( SouthEast == ( index & SouthEast ) )
                quads[SOUTH_EAST]->Insert( p_unit );

            return;
        }

        units.push_back( p_unit );
    }

    inline unsigned char InxToI( Index p_index ) {
        if( p_index == NorthWest )
            return NORTH_WEST;

        if( p_index == NorthEast )
            return NORTH_EAST;

        if( p_index == SouthWest )
            return SOUTH_WEST;

        if( p_index == SouthEast )
            return SOUTH_EAST;

        return 0;
    }

    // elements are not unique
    void Retrieve( const Rectangle &p_box, std::list< UnitPtr > &retUnits ) {
        if( hasChildren ) {
            int index = getIndex( p_box );

            if( NorthWest == ( index & NorthWest ) )
                quads[NORTH_WEST]->Retrieve( p_box, retUnits );

            if( NorthEast == ( index & NorthEast ) )
                quads[NORTH_EAST]->Retrieve( p_box, retUnits );

            if( SouthWest == ( index & SouthWest ) )
                quads[SOUTH_WEST]->Retrieve( p_box, retUnits );

            if( SouthEast == ( index & SouthEast ) )
                quads[SOUTH_EAST]->Retrieve( p_box, retUnits );
        }

        retUnits.insert( retUnits.end(), units.begin(), units.end() );
    }

    void Split() {
        int subWidth = (int)( quadrant.Width() / 2 );
        int subHeight = (int)( quadrant.Height() / 2 );
        int x = (int) quadrant.p0.getX();
        int y = (int) quadrant.p0.getY();


        quads[SOUTH_WEST] = QuadPtr( new Quad( SouthWest, Rectangle( Vector3( x, y + subHeight, 0.0f ), subWidth, subHeight), level + 1, calcPosition( SOUTH_WEST ), QuadPtr( this, nodelete() ) ) );
        quads[SOUTH_EAST] = QuadPtr( new Quad( SouthEast, Rectangle( Vector3( x + subWidth, y + subHeight, 0.0f ), subWidth, subHeight), level + 1,  calcPosition( SOUTH_EAST ), QuadPtr( this, nodelete() ) ) );
        quads[NORTH_WEST] = QuadPtr( new Quad( NorthWest, Rectangle( Vector3( x, y, 0.0f ), subWidth, subHeight), level + 1, calcPosition( NORTH_WEST ), QuadPtr( this, nodelete() ) ) );
        quads[NORTH_EAST] = QuadPtr( new Quad( NorthEast, Rectangle( Vector3( x + subWidth, y, 0.0f ), subWidth, subHeight ), level + 1, calcPosition( NORTH_EAST ),  QuadPtr( this, nodelete() ) ) );      

        hasChildren = true;

        // add to linear tree
        s_GetLinearTree().push_back( quads[SOUTH_WEST] );
        s_GetLinearTree().push_back( quads[SOUTH_EAST] );
        s_GetLinearTree().push_back( quads[NORTH_WEST] );
        s_GetLinearTree().push_back( quads[NORTH_EAST] );

        // look for neighbors with this as neighbor index in linear tree and increment same index in size with one
        incNeighborSize( position, parent );
    }

    // ToDo: this is not finding all neighbors, only the one within the same parent!
    void incNeighborSize( int p_position, QuadPtr p_entry ) {
        if( parent.get() == NULL )
            return;

        for( auto quad : p_entry->quads ) {
            for( int i = 0; i < 8; i++ ) {
                if( quad->getNeighbor( i ) == p_position ) {

                    if( quad->neighborSizes[i] < 1 )
                        quad->neighborSizes[i] += 1;

                    // recursion: find all children of children with this as neighbor
                    if( quad->hasChildren )
                        quad->incNeighborSize( p_position, quad );
                }
            }
        }
    }

    int getNeighbor( int p_location ) {
        if( neighborSizes[p_location] == INT_MAX ) {
            return INT_MAX;
        }

        int neigborBin = 0;

        switch( p_location ) {
        case WEST:
            neigborBin = WEST_NEIGHBOR;
            break;
        case NORTH:
            neigborBin = NORTH_NEIGHBOR;
            break;
        case EAST:
            neigborBin = EAST_NEIGHBOR;
            break;
        case SOUTH:
            neigborBin = SOUTH_NEIGHBOR;
            break;
        case NORTH_EAST:
            neigborBin = NORTH_EAST_NEIGHBOR;
            break;
        case NORTH_WEST:
            neigborBin = NORTH_WEST_NEIGHBOR;
            break;
        case SOUTH_EAST:
            neigborBin = SOUTH_EAST_NEIGHBOR;
            break;
        case SOUTH_WEST:
            neigborBin = SOUTH_WEST_NEIGHBOR;
            break;
        default:
            return 0;
        }

        if( neighborSizes[p_location] < 0 ) {
            int shift = ( 2 * ( QUAD_MAX_LEVEL - level - neighborSizes[p_location] ) );
            return quad_location_add( ( position >> shift ) << shift, neigborBin << shift );
        } else {
            return quad_location_add( position, neigborBin << ( 2 * ( QUAD_MAX_LEVEL - level ) ) );
        }
    }

    // ToDo: merge quads children to this one, and decrement neighbors size to this one
    void Merge() {
        hasChildren = false;

    }

    int calcPosition( int p_location ) {
        return position | ( p_location << ( 2 * ( QUAD_MAX_LEVEL - ( level + 1 ) ) ) );
    }


    // Fig. 7: change if child is north, take north neighbor of this
    void calcNeighborsSizes( int p_location ) {
        if( p_location == NORTH_WEST  ) {
            if( parent->neighborSizes[NORTH] == INT_MAX )
                neighborSizes[NORTH_EAST] = INT_MAX;
            else
                neighborSizes[NORTH_EAST] = parent->neighborSizes[NORTH] - 1;
        }

        if( p_location == NORTH_WEST || p_location == NORTH_EAST ) {
            if( parent->neighborSizes[NORTH] == INT_MAX )
                neighborSizes[NORTH] = INT_MAX;
            else
                neighborSizes[NORTH] = parent->neighborSizes[NORTH] - 1;
        }

        if( p_location == NORTH_WEST ) {
            if( parent->neighborSizes[NORTH_WEST] == INT_MAX )
                neighborSizes[NORTH_WEST] = INT_MAX;
            else
                neighborSizes[NORTH_WEST] = parent->neighborSizes[NORTH_WEST] - 1;
        }

        if( p_location == NORTH_WEST ) {
            if( parent->neighborSizes[WEST] == INT_MAX )
                neighborSizes[WEST] = INT_MAX;
            else
                neighborSizes[WEST] = parent->neighborSizes[WEST] - 1;
        }

        if( p_location == NORTH_WEST  ) {
            if( parent->neighborSizes[WEST] == INT_MAX )
                neighborSizes[SOUTH_WEST] = INT_MAX;
            else
                neighborSizes[SOUTH_WEST] = parent->neighborSizes[WEST] - 1;
        }


        if( p_location == NORTH_EAST  ) {
            if( parent->neighborSizes[NORTH_EAST] == INT_MAX )
                neighborSizes[NORTH_EAST] = INT_MAX;
            else
                neighborSizes[NORTH_EAST] = parent->neighborSizes[NORTH_EAST] - 1;
        }

        if( p_location == NORTH_EAST  ) {
            if( parent->neighborSizes[EAST] == INT_MAX )
                neighborSizes[SOUTH_EAST] = INT_MAX;
            else
                neighborSizes[SOUTH_EAST] = parent->neighborSizes[EAST] - 1;
        }

        if( p_location == NORTH_EAST  ) {
            if( parent->neighborSizes[NORTH] == INT_MAX )
                neighborSizes[NORTH] = INT_MAX;
            else
                neighborSizes[NORTH] = parent->neighborSizes[NORTH] - 1;
        }

        if( p_location == NORTH_EAST  ) {
            if( parent->neighborSizes[NORTH] == INT_MAX )
                neighborSizes[NORTH_WEST] = INT_MAX;
            else
                neighborSizes[NORTH_WEST] = parent->neighborSizes[NORTH] - 1;
        }

        if( p_location == NORTH_EAST  ) {
            if( parent->neighborSizes[EAST] == INT_MAX )
                neighborSizes[EAST] = INT_MAX;
            else
                neighborSizes[EAST] = parent->neighborSizes[EAST] - 1;
        }


        if( p_location == SOUTH_EAST  ) {
            if( parent->neighborSizes[EAST] == INT_MAX )
                neighborSizes[EAST] = INT_MAX;
            else
                neighborSizes[EAST] = parent->neighborSizes[EAST] - 1;
        }

        if( p_location == SOUTH_EAST  ) {
            if( parent->neighborSizes[EAST] == INT_MAX )
                neighborSizes[NORTH_EAST] = INT_MAX;
            else
                neighborSizes[NORTH_EAST] = parent->neighborSizes[EAST] - 1;
        }

        if( p_location == SOUTH_EAST  ) {
            if( parent->neighborSizes[SOUTH_EAST] == INT_MAX )
                neighborSizes[SOUTH_EAST] = INT_MAX;
            else
                neighborSizes[SOUTH_EAST] = parent->neighborSizes[SOUTH_EAST] - 1;
        }

        if( p_location == SOUTH_EAST  ) {
            if( parent->neighborSizes[SOUTH] == INT_MAX )
                neighborSizes[SOUTH] = INT_MAX;
            else
                neighborSizes[SOUTH] = parent->neighborSizes[SOUTH] - 1;
        }

        if( p_location == SOUTH_EAST  ) {
            if( parent->neighborSizes[SOUTH] == INT_MAX )
                neighborSizes[SOUTH_WEST] = INT_MAX;
            else
                neighborSizes[SOUTH_WEST] = parent->neighborSizes[SOUTH] - 1;
        }

        if( p_location == SOUTH_WEST  ) {
            if( parent->neighborSizes[SOUTH] == INT_MAX )
                neighborSizes[SOUTH_EAST] = INT_MAX;
            else
                neighborSizes[SOUTH_EAST] = parent->neighborSizes[SOUTH] - 1;
        }

        if( p_location == SOUTH_WEST  ) {
            if( parent->neighborSizes[SOUTH] == INT_MAX )
                neighborSizes[SOUTH] = INT_MAX;
            else
                neighborSizes[SOUTH] = parent->neighborSizes[SOUTH] - 1;
        }


        if( p_location == SOUTH_WEST  ) {
            if( parent->neighborSizes[SOUTH_WEST] == INT_MAX )
                neighborSizes[SOUTH_WEST] = INT_MAX;
            else
                neighborSizes[SOUTH_WEST] = parent->neighborSizes[SOUTH_WEST] - 1;
        }

        if( p_location == SOUTH_WEST  ) {
            if( parent->neighborSizes[WEST] == INT_MAX )
                neighborSizes[WEST] = INT_MAX;
            else
                neighborSizes[WEST] = parent->neighborSizes[WEST] - 1;
        }

        if( p_location == SOUTH_WEST  ) {
            if( parent->neighborSizes[WEST] == INT_MAX )
                neighborSizes[NORTH_WEST] = INT_MAX;
            else
                neighborSizes[NORTH_WEST] = parent->neighborSizes[WEST] - 1;
        }
    }

    int quad_location_add( int p_a, int p_b ) {
        return ( ( ( p_a | ty ) + ( p_b & tx ) ) & tx ) | ( ( ( p_a | tx ) + ( p_b & ty ) ) & ty );
    }
};

Desired usage: root = QuadPtr( new Quad( Quad::None, Rectangle(0,0,400,400), 0, 0 ) ); root->Split(); root->quads[SOUTH_EAST]->Split();

std::cout << "NE->SE->S  : " << root->quads[SOUTH_EAST]->quads[NORTH_EAST]->getNeighbor( NORTH_WEST ) << std::endl;
// is !=, but it have to be equal
std::cout << "SE->NE->NW : " << root->quads[SOUTH_EAST]->getNeighbor( NORTH ) << std::endl;

There is a more recent paper (of 2015) that defines the Cardinal Neighbours Quadtree, a novel technique for region subdivision, with which you can find in constant time O(1) all the neighbours of a leaf, independently of their size. The reduction in time complexity is obtained through the addition of 4 pointers per node, the so-called cardinal neighbours.

I implemented the algorithm in Go: https://github.com/arl/go-rquad