Fix bug related to diagonals where STRAIGHT_COST was used instead of DIAGONAL_COST.