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;