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 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

public:
static std::vector< QuadPtr > & s_GetLinearTree() {
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;

bool hasChildren;

std::list< UnitWeakPtr > units;

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 )
}

void Clear() {
units.clear();

}

}

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

int index = None;

index = index | NorthWest;
}

index = index | NorthEast;
}

index = index | SouthWest;
}

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 ) )

if( NorthEast == ( index & NorthEast ) )

if( SouthWest == ( index & SouthWest ) )

if( SouthEast == ( index & SouthEast ) )

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 ) )

if( NorthEast == ( index & NorthEast ) )

if( SouthWest == ( index & SouthWest ) )

if( SouthEast == ( index & SouthEast ) )
}

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

void Split() {
int subWidth = (int)( quadrant.Width() / 2 );
int subHeight = (int)( quadrant.Height() / 2 );

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;

// 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( int i = 0; i < 8; i++ ) {
if( quad->getNeighbor( i ) == p_position ) {

// recursion: find all children of children with this as neighbor
}
}
}
}

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 {
}
}

// 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;
}
}