Fixed bug when mapping coordinates to matrix indices