Fast Marching Square source code for Matlab