Fix incorrect calculation in get distance for diagonals

[mikeparker]

I am implementing the ability to return incomplete paths and noticed this calculation was off.

For example if a node is 1x,1y away from the target this function incorrectly returns a distance of 2.4 instead of 1.4.

Another example, if a node is 4x,2y away, this should move diagonally for 2y which will also result in 2x movement, then the remaining 2x, so it should be 2*DIAGONAL_COST + 2*STRAIGHT_COST rather than 2*DIAGONAL_COST + 4*STRAIGHT_COST