How can I simplify this code to compute the shortest rotation between two angles?

Question

The following code will find the shortest rotation (in radians) (from pi to -pi) that I need to apply to from to leave me with to.

Scalar rotationBetween(Scalar from, Scalar to)
{
    Scalar fromMod = std::signbit(from) ? 
        pi * 2 + std::fmod(from, pi * 2) : std::fmod(from, pi * 2);
    Scalar toMod = std::signbit(to) ?
        pi * 2 + std::fmod(to, pi * 2) : std::fmod(to, pi * 2);

    Scalar rotation = toMod - fromMod;

    if(rotation > pi)
        return rotation - 2 * pi;
    else if(rotation < -pi)
        return 2 * pi + rotation;
    else
        return rotation;
}

Scalar is a c++ float.

Can I simplify this? I feel like this is far too much logic to do something so simple.

Answer 1

To get a value inside [-pi,pi], you can add pi, do fmod, and subtract pi again. Here is one way that works. Unfortunately you still need to somehow test whether from - to is positive or negative before calling fmod:

Scalar rotationBetween(Scalar from, Scalar to)
{
    return (from > to) ? -pi + std::fmod(from - to + pi, pi * 2)
                       :  pi - std::fmod(to - from + pi, pi * 2);
}

If you are absolutely sure that from and to are in [-2pi,2pi], then this simpler version will work:

Scalar rotationBetween(Scalar from, Scalar to)
{
    std::fmod(from - to + 5 * pi, pi * 2) - pi;
}

Finally, if you just want short code and don’t care about performance, this can be interesting:

Scalar rotationBetween(Scalar from, Scalar to)
{
    return 2 * std::atan(std::tan((from - to) / 2);
}

Answer 2

I think the following should do the trick.

(Edit: as pointed out in the comments, this gives the unsigned rotation).

Scalar rotationBetween(Scalar from, Scalar to)
{
   //Calculates the two possibilities, and take the smallest
   Scalar difference = std::min(
      std::fmod(from - to, pi * 2),
      std::fmod(to - from, pi * 2)); 

   //takes the above that is in the range 0 to 2*pi 
   //and brings it in the range -pi to pi
   if (difference > pi) 
      difference -= 2 * pi;

   return difference;
}