Fix lerp function

Use a robust function against floating point errors while not needing to change the order of parameters/invert the end result