Revised the algorithm for working with 3d data, need to test with Senze3d