3D Path-finding in a voxelized model of an indoor environment