Quadtree.cpp

#include "Quadtree.h"
 
#include "assert.h"
 
int Cogs::Core::SeaCurrentQuadtree::getSubQuad(const QuadBounds& bounds, const glm::vec2& arrow) const
{
  glm::vec2 center = (bounds.min + bounds.max) * 0.5f;
 
  if (arrow.x < center.x) // Left
  {
    if (arrow.y < center.y) // Bottom
      return 0;
    else
      return 2; // Top
  }
  else // Right
  {
    if (arrow.y < center.y) // Bottom
      return 1;
    else
      return 3; // Top
  }
}
 
void Cogs::Core::SeaCurrentQuadtree::split(QuadNode* node)
{
  assert(node != nullptr);
  assert(isLeaf(node) && "Error. Trying to split a non leaf node");
 
  int i = 0;
  // Create children
  for (auto& child : node->mChildren)
  {
    child = std::make_unique<QuadNode>();
    QuadBounds b = computeSubQuad(node->mBounds, i++);
 
    child->mBounds = b;
  }
}
 
void Cogs::Core::SeaCurrentQuadtree::clear(QuadNode * node)
{
  for (auto& child : node->mChildren)
  {
 
    if (child.get() != nullptr)
    {
      clear(child.get());
      child.release();
    }
  }
}
 
Cogs::Core::QuadBounds Cogs::Core::SeaCurrentQuadtree::computeSubQuad(const QuadBounds& bounds, int i) const
{
  glm::vec2 center = (bounds.min + bounds.max) * 0.5f;
  switch (i)
  {
    // Bot Left
  case 0:
    return QuadBounds{ bounds.min, center };
    // Bot Right
  case 1:
    return QuadBounds{ glm::vec2(center.x, bounds.min.y), glm::vec2(bounds.max.x, center.y) };
    // Top Left
  case 2:
    return QuadBounds{ glm::vec2(bounds.min.x, center.y), glm::vec2(center.x, bounds.max.y) };
    // Top Right
  case 3:
    return QuadBounds{ center, bounds.max };
  default:
    return bounds;
  }
}
 
void Cogs::Core::SeaCurrentQuadtree::insert(QuadNode* node, size_t depth, const glm::vec2& arrow)
{
  // What is a better policy, assert or log and return?
  assert(node != nullptr);
  if (node->mNearestArrow.x)
  {
    glm::vec2 center = (node->mBounds.min + node->mBounds.max) * 0.5f;
    if (glm::distance(arrow, center) < glm::distance(node->mNearestArrow, center))
    {
      if (isLeaf(node))
        split(node);
 
      int id = getSubQuad(node->mBounds, node->mNearestArrow);
      if (id != -1)
        insert(node->mChildren[id].get(), depth + 1, node->mNearestArrow);
      
      node->mNearestArrow = arrow;
    }
    else
    {
      if (isLeaf(node))
        split(node);
 
      int id = getSubQuad(node->mBounds, arrow);
      if (id != -1)
        insert(node->mChildren[id].get(), depth + 1, arrow);
    }
  }
  else
  {
    node->mNearestArrow = arrow;
  }
}
 
int Cogs::Core::SeaCurrentQuadtree::search(QuadNode* node, const glm::vec2& arrow, int priority) const
{
  assert(node != nullptr);
 
  // Narrow down the search by checking bounds against the child nodes 
  int i = getSubQuad(node->mBounds, arrow);
 
  if (i != -1 && node->mChildren[i].get())
    return search(node->mChildren[i].get(), arrow, ++priority);
  else // It is not contained within the children so it has to be in the current parent
  {
      return priority;
  }
}