Поможите реализовать эту программу поиск ближайших объектов

class SPHERE_SET_TREE
{
public:
  class POSITION;
  class ITEM;
  class NODE;
 
  // Структура задает элемент множества сфер
  struct SPHERE
  {
    void* pData; // Указатель на данные пользователя, шаблоны не применяются для сокращения записи
    POINT3D Center; // Центр сферы
    float Radius; // Радиус сферы
 
    SPHERE( void* const pData_=NULL, const POINT3D& Center_=POINT3D(0,0,0), const float Radius_=0) 
      : pData(pData_), Center(Center_), Radius(Radius_) {}
  };
  
  // Тип функции, которая принимает уведомление о пересечении
  typedef void (*SSTCALLBACK)(const SPHERE&);
 
 
  // Добавляет новую сферу в множество
  POSITION Push( const SPHERE& Sphere );
  // Удаляет сферу из множества
  void Pop( const POSITION& Pos );
  // Перемещает одну из сфер множества в пространстве с получением новой позиции в дереве
  void Move( const SPHERE& Sphere, POSITION& Pos );
  // Удаляет все сферы из множества
  void Clear();
 
  // Проверяет пересечение сферы "Sphere" с множеством сфер, 
  // заданным объектом класса SPHERE_SET_TREE
  void TestIntersection( const SPHERE& Sphere, const SSTCALLBACK Callback ) const;
 
 
  SPHERE_SET_TREE( const AABB& BoundingBox_, // "бокс" описывающий множество
                   const float MaxSphereRadius_,  // радиус наибольшей сферы
                   const float Density_ // Плотность распределения сфер в пространстве, (0,1), 
                                        // 0-низкая, 1-высокая
                   );
  ~SPHERE_SET_TREE() { Clear(); }
 
 
  // --------------------------------------------------------------------------------------------
private:
  NODE* pRootNode;
  const AABB BoundingBox;
  const float MaxSphereRadius;
  const float Density;
 
  SPHERE_SET_TREE( const SPHERE_SET_TREE& ); //запрет копирования
  SPHERE_SET_TREE& operator = ( const SPHERE_SET_TREE& );//запрет присваивания
 
  POSITION RecursivePush( NODE*& pNode, NODE* const pParent, const AABB& B, const SPHERE& Sphere);
  void DeleteItem( const POSITION& Pos ); //удаляет элемент из списка, прикрепленного к узлу
  bool DeleteNode( NODE*& pNode ); //удаляет узел дерева
  void RecursiveTestIntersection( const NODE* const pNode, const SPHERE& Sphere, 
                                  const SSTCALLBACK Callback ) const;
  bool IsSphereInsideBox( const SPHERE& Sphere, const AABB& Box ) const;//проверяет находится ли
                                                                        //сфера внутри "бокса"
  bool TestIntersection( const SPHERE&, const SPHERE& ) const;//проверяет пересечение двух сфер
};
 
 
// задает позицию элемента множества в дереве
class SPHERE_SET_TREE::POSITION
{
public:
  POSITION() : pNode(NULL), pItem(NULL) {}
 
private:
  friend SPHERE_SET_TREE;
 
  NODE* pNode;
  ITEM* pItem;
 
  POSITION( NODE* const pNode_, ITEM* const pItem_ ) : pNode(pNode_), pItem(pItem_) {}
};
 
// Внутреннее представление элемента множества
class SPHERE_SET_TREE::ITEM
{
public:
  SPHERE Sphere;
  ITEM* pPrev, * pNext;
 
  ITEM( const SPHERE& Sphere_, ITEM* const pNext_ ) : Sphere(Sphere_),pPrev(NULL),pNext(pNext_){}
  ~ITEM() { delete pNext; }
 
private:
  ITEM( const ITEM& );//запрет копирования
  ITEM& operator=( const ITEM& );//запрет присваивания
};
 
// Узел дерева
class SPHERE_SET_TREE::NODE
{
public:
  const AABB B;
  const int IndexOfMaxBoxDimension;
  ITEM* pItem;
  NODE* pParent, * pPositive, * pNegative;
 
  NODE( const AABB& B_, const int IndexOfMaxBoxDimension_, NODE* const pParent_ );
  ~NODE();
 
  bool IsEmpty() const { return !pItem && !pPositive && !pNegative; }
 
private:
  NODE( const NODE& );//запрет копирования
  NODE& operator=( const NODE& );//запрет присваивания
};
 
 
SPHERE_SET_TREE::NODE::NODE(const AABB& B_,const int IndexOfMaxBoxDimension_,NODE* const pParent_)
  : B(B_), IndexOfMaxBoxDimension(IndexOfMaxBoxDimension_), 
    pItem(NULL), pParent(pParent_), pPositive(NULL), pNegative(NULL) {}
 
SPHERE_SET_TREE::NODE::~NODE() 
{
  delete pItem; delete pPositive; delete pNegative;
}
 
 
SPHERE_SET_TREE::POSITION SPHERE_SET_TREE::RecursivePush( NODE*& pNode, NODE* const pParent, 
                                                          const AABB& B, const SPHERE& Sphere  )
{
  // Ищем максимальный размер бокса
  int IndexOfMaxBoxDimension=0;
  { const POINT3D Size(B.Size());float MaxBoxDimension=Size[0];
    for( int i=1; i<3; ++i ) if( Size[i]>MaxBoxDimension ) 
    { 
      MaxBoxDimension=Size[i]; 
      IndexOfMaxBoxDimension=i; 
    } 
  }
 
  // Если эта часть пространства не описана деревом, строим новый узел
  if( !pNode ) pNode=new NODE(B,IndexOfMaxBoxDimension,pParent); 
 
  assert( B.Equal(pNode->B) );
  assert( IndexOfMaxBoxDimension==pNode->IndexOfMaxBoxDimension );
 
  // Вычисляем ширину общей области двух подпространств
  const float MaxBoxDimension=B.Max[IndexOfMaxBoxDimension]-B.Min[IndexOfMaxBoxDimension],
              BorderHalfWidth=std::min(float(MaxSphereRadius),MaxBoxDimension*Density*0.25f);
 
  if( Sphere.Radius>=BorderHalfWidth ) //проверяем соотношение радиуса сферы и ширины границы
  { // прикрепляем сферу к узлу дерева
    pNode->pItem=new ITEM(Sphere,pNode->pItem); 
    if( pNode->pItem->pNext ) pNode->pItem->pNext->pPrev=pNode->pItem;
    return POSITION(pNode,pNode->pItem);
  }
  else
  { // продолжаем разбиение пространства
    AABB B2(B);
    const float MaxBoxDimensionCenter=
                (B.Max[IndexOfMaxBoxDimension]+B.Min[IndexOfMaxBoxDimension])*0.5f,
                UnusedSpace=0.5f*MaxBoxDimension-BorderHalfWidth;
 
    if( Sphere.Center[IndexOfMaxBoxDimension]>MaxBoxDimensionCenter )
    {
      B2.Min[IndexOfMaxBoxDimension]+=UnusedSpace;
      return RecursivePush(pNode->pPositive,pNode,B2,Sphere);
    }
    else
    {
      B2.Max[IndexOfMaxBoxDimension]-=UnusedSpace;
      return RecursivePush(pNode->pNegative,pNode,B2,Sphere);
    }
  }
}
 
SPHERE_SET_TREE::POSITION SPHERE_SET_TREE::Push( const SPHERE& Sphere )
{
  assert( Sphere.Radius>0 );
 
  return RecursivePush(pRootNode,NULL,BoundingBox,Sphere);
}
 
// удаляет элемент, при этом корректирует двусвязный список
void SPHERE_SET_TREE::DeleteItem( const POSITION& Pos )
{
  assert( Pos.pNode && Pos.pItem );
 
  if( Pos.pItem->pPrev ) 
    Pos.pItem->pPrev->pNext=Pos.pItem->pNext;
  else                 
    Pos.pNode->pItem=Pos.pItem->pNext;
 
  if( Pos.pItem->pNext )
  { 
    Pos.pItem->pNext->pPrev=Pos.pItem->pPrev; 
    Pos.pItem->pNext=NULL;
  }
 
  delete Pos.pItem;
}
 
// Если у узла нет детей и прикрепленных элементов, удаляет узел и корректирует дерево
bool SPHERE_SET_TREE::DeleteNode( NODE*& pNode )
{
  if( pNode && pNode->IsEmpty() )
  {
    if( pNode->pParent )
    {
      if( pNode->pParent->pPositive==pNode ) pNode->pParent->pPositive=NULL;
      if( pNode->pParent->pNegative==pNode ) pNode->pParent->pNegative=NULL;
 
      NODE* const pNode2=pNode;
      pNode=pNode->pParent;
      delete pNode2;
    }
    else
    {
      assert( pRootNode==pNode );
      pRootNode=NULL;
      delete pNode;
      pNode=NULL;
    }
    return true;
  }
  return false;
}
 
// Удаляет элемент и корректирует дерево
void SPHERE_SET_TREE::Pop( const POSITION& Pos )
{
  assert( Pos.pNode && Pos.pItem );
 
  DeleteItem(Pos);
  for( NODE* pNode=Pos.pNode; DeleteNode(pNode); );
}
 
// Перемещает сферу в пространстве, перестроение дерева минимально, 
// для незначительного перемещения
void SPHERE_SET_TREE::Move( const SPHERE& Sphere, POSITION& Pos )
{
  assert( Sphere.Radius>0 );
  assert( Pos.pNode && Pos.pItem );
 
  if( IsSphereInsideBox(Sphere,Pos.pNode->B) )
  {
    Pos.pItem->Sphere=Sphere;
  }
  else
  {
    DeleteItem(Pos);
 
    NODE* pNode=Pos.pNode;
    if( !DeleteNode(pNode) ) pNode=pNode->pParent;
 
    while( pNode && !IsSphereInsideBox(Sphere,pNode->B) )
      if( !DeleteNode(pNode) ) pNode=pNode->pParent;
 
    if( !pNode ) Pos=Push(Sphere);
    else         Pos=RecursivePush(pNode,pNode->pParent,pNode->B,Sphere);
  }
}
 
void SPHERE_SET_TREE::Clear() 
{ 
  delete pRootNode; pRootNode=NULL; 
}
 
SPHERE_SET_TREE::SPHERE_SET_TREE( const AABB& BoundingBox_, const float MaxSphereRadius_, 
                                  const float Density_ )
  : pRootNode(NULL), BoundingBox(BoundingBox_),MaxSphereRadius(MaxSphereRadius_),Density(Density_)
{}
 
// Рекурсивный проход по дереву для определения пересечения, 
// скорость работы функции достаточно высока из-за простых сравнений
void SPHERE_SET_TREE::RecursiveTestIntersection( const NODE* const pNode, const SPHERE& Sphere,
                                                 const SSTCALLBACK Callback ) const
{
  assert( pNode && !pNode->IsEmpty() );
 
  for( const ITEM* pItem=pNode->pItem; pItem; pItem=pItem->pNext )
    if( TestIntersection(Sphere,pItem->Sphere) )
      Callback(pItem->Sphere); // Callback вызывается в случае пересечения
 
  const int IndexOfMaxBoxDimension=pNode->IndexOfMaxBoxDimension;
 
  if( pNode->pPositive && Sphere.Center[IndexOfMaxBoxDimension]+Sphere.Radius>=
                          pNode->pPositive->B.Min[IndexOfMaxBoxDimension] )
    RecursiveTestIntersection(pNode->pPositive,Sphere,Callback); //продолжаем в позитивном 
                                                                 //подпространстве
 
  if( pNode->pNegative && Sphere.Center[IndexOfMaxBoxDimension]-Sphere.Radius<=
                          pNode->pNegative->B.Max[IndexOfMaxBoxDimension] )
    RecursiveTestIntersection(pNode->pNegative,Sphere,Callback); //продолжаем в негативном 
                                                                 //подпространстве
}
 
void SPHERE_SET_TREE::TestIntersection( const SPHERE& Sphere, const SSTCALLBACK Callback ) const
{
  if( pRootNode )
    RecursiveTestIntersection(pRootNode,Sphere,Callback);
}