double angle_diff(double a, double b)
{
double d1, d2;
d1 = a-b;
d2 = 2*PI - fabs(d1);
if(d1 > 0)
d2 *= -1.0;
if(fabs(d1) < fabs(d2))
return(d1);
else
return(d2);
}如果d1>0，那么相当于从b顺时针旋转d1能到a。
那么当这个旋转角度大于180度时，不如从b逆时针旋d2转到a（取反那一步就代表逆时针旋转）；