sorry, i've been quite busy..
At first sight the problem seems to be one of:
- a rounding problem: probably you round some float value to integer, but you're working with values in the range of 0.5 so rounding will loose precision. I'm quite sure this is the problem but i couldn't find where it is in the code.
- a scale problem: maybe you're using too big values for radius, distances, etc. Try and control them.

Bye!