C 角度计算经验代码集合

计算两个角度差，两个角度 [-pi,  pi] 之间。弧度制

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);
}

将任意角度换算为 [-pi,  pi] 之间

// wrapping angle to [-pi .. pi]
    angle_increment = fmod(angle_increment + 5*M_PI, 2*M_PI) - M_PI;