QuadTree.cpp

class QuadTree
{
private:
	static const int MAX_ENTITIES = 20;
	std::vector<Entity *> m_entities;
	sf::Vector2i m_position;
	sf::Vector2i m_size;
	QuadTree * m_nodes;

	int getIndex(const Entity * e) const
	{
		const CircleCollider& collider = e->getCollider();
		const double radius = collider.getRadius();
		const sf::Vector2f max = collider.getPosition() + sf::Vector2f(radius, radius);
		const sf::Vector2f min = collider.getPosition() - sf::Vector2f(radius, radius);

		const sf::Vector2i quadCenter = m_position + (m_size / 2);

		const bool down = max.y > quadCenter.y && min.y > quadCenter.y;
		const bool up = max.y < quadCenter.y && min.y < quadCenter.y;
		const bool right = max.x > quadCenter.x && min.x > quadCenter.x;
		const bool left = max.x < quadCenter.x && min.x < quadCenter.x;

		if (left)
		{
			if (up) return 1;
			if (down) return 2;
		}
		if (right)
		{
			if (up) return 0;
			if (down) return 3;
		}

		return -1;
	}

public:
	QuadTree() :
		m_entities(),
		m_position(),
		m_size(),
		m_nodes(NULL)
	{
	}
	QuadTree(const sf::Vector2i& position, const sf::Vector2i& size) :
		m_entities(),
		m_position(position),
		m_size(size),
		m_nodes(NULL)
	{
	}
	~QuadTree()
	{
		if (m_nodes != NULL) 
		{
			delete[] m_nodes;
		}
	}

	void clear()
	{
		m_entities.clear();
		if (m_nodes != NULL) {
			for (int i = 0; i < 4; ++i)
			{
				m_nodes[i].clear();
			}
			delete m_nodes;
			m_nodes = NULL;
		}
	}

	void split()
	{
		if (m_nodes != NULL) {
			sf::err() << "Attempted to split a quadtree with existing childern." << std::endl;
			return;
		}
		sf::Vector2i halfsize = m_size / 2;
		m_nodes = new QuadTree[4];
		m_nodes[0] = QuadTree(sf::Vector2i(m_position.x + halfsize.x, m_position.y), sf::Vector2i(halfsize));
		m_nodes[1] = QuadTree(sf::Vector2i(m_position.x, m_position.y), sf::Vector2i(halfsize));
		m_nodes[2] = QuadTree(sf::Vector2i(m_position.x, m_position.y + halfsize.y), sf::Vector2i(halfsize));
		m_nodes[3] = QuadTree(sf::Vector2i(m_position.x + halfsize.x, m_position.y + halfsize.y), sf::Vector2i(halfsize));
	}

	void insert(Entity * e) 
	{
		if (m_nodes != NULL)
		{
			int index = getIndex(e);

			if (index != -1) 
			{
				m_nodes[index].insert(e);
				return;
			}
		}

		m_entities.push_back(e);

		if (m_entities.size() > MAX_ENTITIES) 
		{
			split();

			// We walk backwards to maintain valid iterators despite mutating the list.
			for (auto i = m_entities.end(); i != m_entities.begin(); --i)
			{
				int index = getIndex(*i);
				if (index != -1)
				{
					m_nodes[index].insert(*i);
					m_entities.erase(i);
				}
			}
		}
	}

	void retrieve(std::vector<Entity *>& retrievedEntities, const Entity * e) const
	{
		int index = getIndex(e);

		if (index != -1 && m_nodes != NULL)
		{
			m_nodes[index].retrieve(retrievedEntities, e);
		}

		retrievedEntities.reserve(retrievedEntities.size() + m_entities.size());
		for (int i = 0; i < m_entities.size(); ++i)
		{
			retrievedEntities.push_back(m_entities[i]);
		}
	}
};