0

可能重复：
C：如何将浮点数包装到区间 [-pi, pi)

我有以下代码，我想优化它以防止当第二个参数很大时旋转函数中的while循环。代码是：

#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;


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

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() {
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;
}

关于旋转功能发生什么变化的任何想法？

4

0 回答 0