After rotation be sure that result lies into [0,π)

[std10093]

As you can see below ,if the angle α (alpha) does not lie into [0,π) we use a loop to bring it back to this range.Well of course this is not efficient.It has complexity linear to the angle.So in order to improve it a would say to use fmod ( equivalent to the operator % but for doubles) to get the number into the range without looping at all.If i have 375%360 i will get 15 which is what i want.Where i can not make it work is for the negative case.For example given -375 i should result it to 360 - 15 = 345 degrees.


The initial program to rotate is this :

#include <math.h>
#include <stdio.h>
#include <string.h>

typedef struct {
        double x;
        double y;
} pos_t;

typedef struct {
        double a;
} ori_t;

typedef struct {
        pos_t pos;
        ori_t ori;
} pose_t;

/*double pos_x, pos_y, ori_a;*/

void forward(pose_t *pose,double dist_units) {
        pose->pos.x += cos(pose->ori.a)*dist_units;
        pose->pos.y += sin(pose->ori.a)*dist_units;
}

inline double deg_to_rad(double angle_deg) {
        return angle_deg*M_PI/180.0;
}

inline double rad_to_deg(double angle_rad) {
        return 180.0*angle_rad/M_PI;
}
/*ROTATION*/
void rotate(pose_t *pose,double angle_deg) {
        pose->ori.a += deg_to_rad(angle_deg);
        while (pose->ori.a<0.0) pose->ori.a += 2.0*M_PI;
        while (pose->ori.a>=2.0*M_PI) pose->ori.a -= 2.0*M_PI;
}

int main(void) {
        pose_t pose;
        pose.pos.x = 0.0;
        pose.pos.y = 0.0;
        pose.ori.a = 0.0;
        char command[3];
        double parameter;
        printf("robot:");
        while (scanf("%2s %lf",command,&parameter)==2) {
                if (strcmp(command,"fw")==0) {
                        forward(&pose,parameter);
                        printf("Moved to   (%.1f,%.1f,%.0f)!\n",pose.pos.x,pose.pos.y,rad_to_deg(pose.ori.a));
                } else if (strcmp(command,"rt")==0) {
                        rotate(&pose,parameter);
                        printf("Turned to (%.1f,%.1f,%.0f)!\n",pose.pos.x,pose.pos.y,rad_to_deg(pose.ori.a));
                } else {
                        printf("Command '%s' not recognized!\n",command);
                }
                printf("robot:");
        };
        return 0;
}

the new function is this

void rotate(pose_t *pose,double angle_deg) {
      pose->ori.a += deg_to_rad(angle_deg);
      if( angle_deg >= 360.0 ) {
         pose->ori.a = fmod(pose->ori.a,(2.0*M_PI));
      }
      if( angle_deg < 0.0 ) {
          pose->ori.a = fmod(-pose->ori.a,(2.0*M_PI));
      }

}

in the negative case ,if we take for instance -375 then the result will be 15(because i pass the as first parameter -(-375)..

I tried to subtract it for 2π but did not got the result...

I can not figure it out..

[Nominal Animal]

Well, you first do the modulus:

pose->ori.a = fmod(pose->ori.a,(2.0*M_PI));

Because modulus works the same whether the argument is positive or negative, the above results in a pose->ori.a between -2.0*M_PI and 2.0*M_PI exclusive (not including the limits), right?

(It does not matter that it will be between 0 and -2.0*M_PI if pose->ori.a was negative, and between 0 and 2.0*M_PI if pose->ori.a was positive. Consider all finite arguments instead.)

Okay, so add a final adjustment that makes it positive.

if (pose->ori.a < 0.0)
    pose->ori.a += 2.0*M_PI;

[smokeyangel]

I'd adjust it to be positive before doing the modulus. If you know the maximum amount that an angle could be negatively rotated by, then you can use a fixed value. If not, you can just keep adding 360 to it til it's positive.

double angle = -375;
angle = angle + 360*5; // nice big adjustment, unnecessarily large in this case

angle = fmod(angle, 360);

Result is 345. For positive numbers it's harmless to add a few multiples of 360 or 2*M_PI before a modulus operation.

[std10093]

smokeyangel i do not the maximum angle,because it comes from input now...later i guess from another program.So i decided to go with Nominal Animal's method which seems to be more clear to me
Thank you both very much