Fixed octree logic, created tests