计算角度差

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（取反那一步就代表逆时针旋转）；