Initial implementation of adding octree function and test