Octree implementation using a Thread pool

Octree.cpp

#include "Octree.h"

ThreadPool* Octree::threadPool;

OctreeNode* Octree::BulidOctree(Point center, float halfWidth, int stopDepth, int MaxStopDepth)
{
	if(stopDepth == MaxStopDepth) threadPool = new ThreadPool(MAX_THREADS);
	
	if (stopDepth < 0) return nullptr;
	else
	{
		//create root node
		OctreeNode* pNode = new OctreeNode;
		pNode->center = center;
		pNode->halfWidth = halfWidth;
		pNode->pObjects = nullptr;

		//recurvivly create the children nodes
		Point offset;
		float step = halfWidth * 0.5f;
		for (int i = 0; i < 8; i++)
		{
			offset.x = ((i & 1) ? step : -step);
			offset.y = ((i & 2) ? step : -step);
			offset.z = ((i & 3) ? step : -step);			
			pNode->pChild[i] = BulidOctree(center + offset, step, stopDepth - 1, MaxStopDepth);
		}	
		return pNode;
	}
}

void Octree::InsertObject(OctreeNode* pTree, ColliderObject* object)
{
	int index = 0;

	for (int i = 0; i < 3; i++)
	{
		float delta = object->position[i] - pTree->center[i];
		if (delta > 0.0f) index |= (1 << i);
	}
	if (pTree->pChild[index])
	{		
		//fully inside a segemnt
		InsertObject(pTree->pChild[index], object);
	}
	else
	{
		//shopuld add objects to next in list
		object->pNext = pTree->pObjects;
		pTree->pObjects = object;
	}
}


void Octree::TestAllCollisions(OctreeNode* pTree)
{
	ColliderObject* pA, * pB;

	for (pA = pTree->pObjects; pA != nullptr; pA = pA->pNext)
	{
		for (pB = pTree->pObjects; pB != nullptr; pB = pB->pNext)
		{
			//dont double check
			if (pA == pB) break;
			//Test collisions
			if (pA->checkCollision(pA, pB))
			{
				pA->resolveCollision(pA, pB);
			}
		}			
	}
	//recursivly check
	for (int i = 0; i < 8; i++)
	{
		OctreeNode* child = pTree->pChild[i];
		if (child)
		{
			//TestAllCollisions(pTree->pChild[i]);
			threadPool->Enqueue([child] { TestAllCollisions(child); });
		}
	}	
}



void Octree::ClearOctree(OctreeNode* pTree)
{	
	for (int i = 0; i < 8; i++)
	{
		if (pTree->pChild[i])
			ClearOctree(pTree->pChild[i]);
	}
	pTree->pObjects = nullptr;	
}

Octree.h

#pragma once

#include "ColliderObject.h"
#include "ThreadPool.h"


struct Point
{
	float x, y, z;

	Point() { x = 0; y = 0; z = 0; }

	Point operator+(const Point& other) const
	{
		Point ret;
		ret.x = x + other.x;
		ret.y = y + other.y;
		ret.z = z + other.z;		
		return ret;
	}

	float operator[](int i)
	{
		switch (i)
		{
		case 0:
			return x;
			break;
		case 1:
			return y;
			break;
		case 2:
			return z;
			break;
		default:
			break;
		}
	}
};

struct OctreeNode
{
	Point center;
	float halfWidth = 0.0f;
	OctreeNode* pChild[8] = { nullptr ,nullptr ,nullptr ,nullptr ,nullptr ,nullptr ,nullptr ,nullptr};
	ColliderObject* pObjects = nullptr;
};

class Octree
{
private:
	//static std::vector<OctreeNode*> finalSegments[64];	
protected:

public:
	static ThreadPool* threadPool;

	static OctreeNode* BulidOctree(Point center, float halfWidth, int stopDepth, int MaxStopDepth = OCTREE_MAX_DEPTH);
	static void InsertObject(OctreeNode* pTree, ColliderObject* object);
	
	//bool CheckCollision(ColliderObject* a, ColliderObject* b);
	//void ResolveCollision(ColliderObject* a, ColliderObject* b);
	
	static void TestAllCollisions(OctreeNode* pTree);
	//static void Collide(OctreeNode* pNode);
	static void ClearOctree(OctreeNode* pTree);	


};

ThreadPool.cpp

#include "ThreadPool.h"


ThreadPool::ThreadPool(size_t numThreads)
{
	//create worker threads 
	for (int i = 0; i < numThreads; i++)
	{
		threads.emplace_back([this]
			{
				while (true)
				{
					std::function<void()> task;
					std::unique_lock<std::mutex> lock(queueMutex);

					condVar.wait(lock, [this] {return !tasks.empty() || stop; });
					if (stop && tasks.empty()) return;

					threadTaskCounter++;
					task = move(tasks.front());
					tasks.pop();

					lock.unlock();
					task();

					//need to lock
					lock.lock();
					threadTaskCounter--;
					mainConVar.notify_one();
				}
			});
	}
}

ThreadPool::~ThreadPool()
{
	{
		std::unique_lock<std::mutex> lock(queueMutex);
		stop = true;
	}
	condVar.notify_all();

	for (auto& thread : threads)
	{
		thread.join();
	}
}

void ThreadPool::Enqueue(std::function<void()> task)
{
	{
		std::unique_lock<std::mutex> lock(queueMutex);
		tasks.emplace(move(task));
	}
	condVar.notify_one();
}

void ThreadPool::WaitForTasks()
{
	//wait for octree condional to be found
	std::unique_lock<std::mutex> mainUnique(queueMutex);
	mainConVar.wait(mainUnique, [this] {return threadTaskCounter == 0 && tasks.empty(); });
}

ThreadPool.h

#pragma once
#include <condition_variable>
#include <functional>
#include <iostream>
#include <mutex>
#include <queue>
#include <thread>

class ThreadPool
{
private:
	//threads
	std::vector<std::thread> threads;
	//queue of tasks
	std::queue<std::function<void()> > tasks;
	//mutex to lock the queue
	std::mutex queueMutex;
	//condition to falg when queue has updated
	std::condition_variable condVar;

	bool stop = false;

	int threadTaskCounter = 0;
	std::condition_variable mainConVar;


public:
	ThreadPool(size_t numThreads = 8);
	~ThreadPool();
	void Enqueue(std::function<void()> task);
	void WaitForTasks();


};