If all you want is to get the SnapToGrid function to work, just replace the main body of the function (after the IF check) with the two lines I originally posted.
Insanity:
Your code is essentially the same as mine, although I think yours is slower due to the overhead from calling RInt(). Also, RInt is inefficient because of the unnecessary if(). Try this:
int RInt(float u){ int i = int(u + 0.5); return(i);}// or this, which saves stack space by not creating ''i''int RInt(float u){ return int(u + 0.5); // may give compiler warning}
... 
