I found stupid typo bug in code. Radius compensation now works correctly.
Here is a screenshot showing toolpath with (green) and without (red) radius compensation.
Code example is from NIST site.
Second screenshot shows G43 (tool offset) toolpath cutting 5 holes to the same depth Z. First one without tool offset, second with positive offset (longer tool needs shorter path) third without offset, fourth with negative offset (shorter tool needs longer path) and fifth again without offset.
I will publish this version when I finish all tests (tommorow).