Changed from R-tree to Octree and making things more thread safe