I tried the solution by acfrancis and I found that the function has its limits when it comes to negative indices. Just in case someone else will tackle this issue:
Reason for issue: negative values like -0.1.... will be cast to 0 instead of -1.
Its the classic "there is only one zero" problem for arrays.
To solve it: before casting the x2, y2 values to int:
check if xr/diag < 0 and, if true, result = result - 1
(respectively for y2: yr * -1 / diag < 0 then result = result -1)
you then cast the result values to int like before.
Hope it helps.
Addition:
The translation of the origin by 128*5 seems to specific to a certain case so i guess this should be removed in order to generalize the function.