Поиск соседей Quadtree в постоянном времени с QTLCLD

Я хочу реализовать этот Алгоритм бумаги для доступа к соседям узлов дерева в постоянное время.

Я сталкиваюсь с проблемами при попытке получить доступ к диагональным соседям (например, когда квад на один или несколько уровней меньше, чем искомый сосед). Пример: root->Child(SE)->Child(NE)->GetNeighbor(NW) должен вернуться root->Child(NE), Тем не менее, я получаю результат root->Child(NW),

Единственная проблема — диагональные поиски на разных уровнях. Другие вещи работают правильно; Я могу найти соседей на том же уровне или от меньшего уровня к большему уровню без проблем.

Вот код:

#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 0xAAAAAAAAclass 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 );
}
};

Желаемое использование:
root = QuadPtr (новый Quad (Quad :: None, Rectangle (0,0,400,400), 0, 0));
корне-> Split ();
корне-> каре [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;