Smallest difference between two angles?

I'm trying to calculate the smallest difference between two angles.

This is m开发者_运维技巧y current code (a slight variation of something I found online):

float a1 = MathHelper.ToDegrees(Rot);
float a2 = MathHelper.ToDegrees(m_fTargetRot);

float dif = (float)(Math.Abs(a1 - a2);

if (dif > 180)
  dif = 360 - dif;

dif = MathHelper.ToRadians(dif);

It works fine except for in cases at the edge of a circle. For example if the current angle is 355 and the target angle is 5 it calculates the difference is -350 rather than 10 since 365 degrees is equal to 5 degrees.

Any ideas on what I can do to make this work?

---

You basically had it. Just take the dif modulus 360 before checking to see if greater than 180:

float a1 = MathHelper.ToDegrees(Rot);
float a2 = MathHelper.ToDegrees(m_fTargetRot);

float dif = (float)Math.Abs(a1 - a2) % 360;

if (dif > 180)
    dif = 360 - dif;

dif = MathHelper.ToRadians(dif);

Edit: @Andrew Russell made a great point in comments to your question and the solution below takes advantage of the MathHelper.WrapAngle method as he suggested:

diff = Math.Abs(MathHelper.WrapAngle(a2 - a1));

---

You would expand the check for out of bound angles:

if (dif < 0) dif = dif + 360;
if (dif > 180) dif = 360 - dif;

---

I never like handling the zero-wrapping with case statements. Instead, I use the definition of the dot product to compute the (unsigned) angle between two angles:

vec(a) . vec(b) = ||a|| ||b|| cos(theta)

We're just going to make a and b unit vectors, so ||a|| == ||b|| == 1.

Since vec(x) = [cos(x),sin(x)], we get:

unsigned_angle_theta(a,b) = acos(cos(a)cos(b) + sin(a)sin(b))

(n.b. all angles in radians)

---

You can normalize the result to be 0 <= theta < 360:

while(theta < 0) { theta += 360; }

If you want to keep the answer in radians (recommended):

const Double TwoPi = 2 * Math.Pi;
while(theta < 0) { theta += TwoPi; }

---

We can use Euler's formula: exp(iA) = cos A + i sin A.

In the case of the difference between two angles this becomes:

exp(i(A-B))

Using the laws of exponents:

= exp(iA).exp(-iB).

-iB is the conjugate of iB thus:

= exp(iA).exp(conjugate(iB)).

The complex exponent can be calcuated by Taylors series:

taylor_e(P={cplx,_,_}) ->
    taylor_e(
      #{sum => to_complex(1),
    term => 0,
    term_value => to_complex(1),
    min_terms => 3,
    quadrature => 1,
    error_term => 1.0e-4,
    domain => P}
       );

taylor_e(P=#{sum := Sum,
       term := Term,
       term_value := TermValue0,
       min_terms := MinTerms, 
       domain := Dom, 
       quadrature := Q,
       error_term := ErrorTerm
      }) 
  when ((Term =< MinTerms) or (abs(1-Q) > ErrorTerm)) and
       (Term < 20) ->
    NewTerm = Term+1,
    TermValue1 = scalar_divide(multiply(TermValue0,Dom),NewTerm),
    PartialSum = add(Sum,TermValue1),
    taylor_e(P#{sum := PartialSum,
        term :=  Term+1,
        term_value := TermValue1,
        quadrature := quadrance(PartialSum)
          });
taylor_e(#{sum := Result}) ->
    Result.

The angle difference is the the argument (direction) of the resulting complex number and is retrieved by atan2.

Of course you will need some basic complex number routines. This method does not have a discontinuity around 0/360 degrees, and the sign of the result gives the direction of turning. Where this is a difference between some reference direction (say in an autopilot) it only needs calculating once and then storing until a new course is chosen. The deviations from the course would need to be calculated from every sample however.