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)
{
ifreturn (from > to)
return? -pi + std::fmod(from - to + pi, pi * 2);
else
return : 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);
}