After checking over the complex maths and not seeing any problems I noticed that your error is simply due to the integer division in the line
zy = y * (SCALED_HEIGHT)/(HEIGHT-1) + SCALED_Y_MIN;
and similarly for zx. Because (SCALED_HEIGHT) and (HEIGHT-1) are both integers this is not going to give you the floating point result you require. Try using:
zy = y * SCALED_HEIGHT * 1.0/(HEIGHT-1) + SCALED_Y_MIN;
and similarly for zx.
EDIT: Sorry the above was in error. Your SCALED_HEIGHT was in fact double so the above was actually ok. The real problem is simply in the line
const int EPSILON = 1e-3;
This will in fact always return zero, because it's an integer. You need to make EPSILON a floating point type.