Good job !
I was stuck on this part :
float radDist(float t1, float t2) {
while (t1>=TWO_PI) t1-= TWO_PI;
while (t1<0) t1 += TWO_PI;
while (t2>=TWO_PI) t2-= TWO_PI;
while (t2<0) t2 += TWO_PI;
float d = abs(t1-t2);
if (d>=PI) {
d = PI-(d-PI);
}
return d;
}
Thanks @lmccandless @kfrajer