Premetto che ho letto il codice molto velocemente ma forse ti passo un'idea che può essere utile:

fai muovere l'omino prima lungo una direzione x fino ad arrivare alla x desiderata (o fintanto che non arriva sul bordo) poi quando è sulla x giusta lo fai muovere nella y .... da quello che ho capito lui invece fa le verifiche di x ed y simultaneamente, quindi questo potrebbe darti l'effetto diagonale ....

PS: mi scuso se ho detto una fesseria