Continous angles in C++ (eq unwrap function in matlab)

angle, c++

Solution

After some work, came up with this. Seems to be working fine.

//Normalize to [-180,180):
inline double constrainAngle(double x){
    x = fmod(x + M_PI,M_2PI);
    if (x < 0)
        x += M_2PI;
    return x - M_PI;
}
// convert to [-360,360]
inline double angleConv(double angle){
    return fmod(constrainAngle(angle),M_2PI);
}
inline double angleDiff(double a,double b){
    double dif = fmod(b - a + M_PI,M_2PI);
    if (dif < 0)
        dif += M_2PI;
    return dif - M_PI;
}
inline double unwrap(double previousAngle,double newAngle){
    return previousAngle - angleDiff(newAngle,angleConv(previousAngle));
}

I used code from this post: Dealing with Angle Wrap in c++ code

Problem

I guess it is not that hard, but I have been stuck on that one for a while. I have a joint that can rotate both direction. A sensor gives me the angle of the joint in the range -pi and +pi. I would like to convert it in the range -infinity and +infinity. Meaning that if for example the joint rotate clockwise forever, the angle would start at 0 and then increase to infinity. In matlab, the unwrap function does that very well: ``` newAngle = unwrap([previousAngle newAngle]); previousAngle = newAngle; ``` Note: it is assumed the angle does not make big jump, nothing superior to PI for sure. Note: I really looked hard before asking ... Thanks !

Original source