Kąty ciągłe w C++ (funkcja rozwijania eq w programie Matlab)

Question

Myślę, że to nie jest takie trudne, ale utknąłem na tym przez jakiś czas.

Mam złącze, które może obracać się w obu kierunkach. Czujnik daje mi kąt przegubu w zakresie -pi i + pi.

Chciałbym przekonwertować go w zakresie -infinity i + nieskończoności. Oznacza to, że jeśli na przykład staw będzie obracał się zgodnie z ruchem wskazówek zegara na zawsze, kąt zacznie się od 0, a następnie wzrośnie do nieskończoności. w Matlab funkcja unwrap robi to bardzo dobrze:

newAngle = unwrap([previousAngle newAngle]); 
previousAngle = newAngle; 

Uwaga: zakłada kąt nie zrobić duży skok, nic wyższą PI na pewno.

Uwaga: Naprawdę ciężko spojrzał przed pytaniem ...

Dzięki!

Answer 1

Po kilku pracach wymyśliłem to. Wydaje się działać dobrze.

//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)); 
} 

użyłem kodu z tego postu: Dealing with Angle Wrap in c++ code

Answer 2

// wrap to [-pi,pi] 
inline double angle_norm(double x) 
{ 
    x = fmod(x + M_PI, M_2PI); 
    if (x < 0) 
     x += M_2PI; 
    return x - M_PI; 
} 

double phase_unwrap(double prev, double now) 
{ 
    return prev + angle_norm(now - prev); 
} 

To działa.

Answer 3

Poniższa funkcja spełnia swoje zadanie przy założeniu, że bezwzględna różnica między wejście kątów jest mniejszy niż 2 * pi:

float unwrap(float previous_angle, float new_angle) { 
    float d = new_angle - previous_angle; 
    d = d > M_PI ? d - 2 * M_PI : (d < -M_PI ? d + 2 * M_PI : d); 
    return previous_angle + d; 
} 

Jeśli trzeba rozpakować tablicę, można użyć tego rutynowego:

void unwrap_array(float *in, float *out, int len) { 
    out[0] = in[0]; 
    for (int i = 1; i < len; i++) { 
     float d = in[i] - in[i-1]; 
     d = d > M_PI ? d - 2 * M_PI : (d < -M_PI ? d + 2 * M_PI : d); 
     out[i] = out[i-1] + d; 
    } 
}