Implemented sub2ind matlab function