/* Finds the minimum angle difference ang1-ang2 such that difference is between   */
float inv_angle_diff(float ang1, float ang2)
/* Finds the minimum angle difference ang1-ang2 such that difference is between   */
float inv_angle_diff(float ang1, float ang2)
{
    float d;
    ang1 = inv_wrap_angle(ang1);
    ang2 = inv_wrap_angle(ang2);
    d = ang1 - ang2;
    if (d > M_PI)
        d -= 2 * (float)M_PI;
    else if (d < -(float)M_PI)
        d += 2 * (float)M_PI;
    return d;
}