00001
00002
00003
00004
00005
00006
00007 #ifdef __cplusplus
00008 extern "C" {
00009 #endif
00010
00011 #include <stdlib.h>
00012 #include <math.h>
00013
00014
00015 void WalkMotionModel(double *x, double *y, double *theta,
00016 double dx, double dy, double da, unsigned int time)
00017 {
00018 double x_motion, y_motion, theta_motion, arc_length;
00019 double xpos_before_angles, ypos_before_angles;
00020 double chord_length, total_motion_angle;
00021
00022
00023 if((dx == 0) && (dy == 0) && (da == 0)) return;
00024
00025
00026 x_motion = dx * ((double) time)/1000.0;
00027 y_motion = dy * ((double) time)/1000.0;
00028 theta_motion = da * ((double) time)/1000.0;
00029
00030
00031 arc_length = sqrt(x_motion*x_motion + y_motion*y_motion);
00032
00033
00034
00035
00036 if(fabs(theta_motion) < 0.000001) {
00037 xpos_before_angles = arc_length;
00038 ypos_before_angles = 0;
00039 }
00040 else if(theta_motion < 0) {
00041 double hypotenuse = arc_length/theta_motion;
00042 xpos_before_angles = sin(theta_motion)*hypotenuse;
00043 ypos_before_angles = hypotenuse - cos(theta_motion)*hypotenuse;
00044 }
00045 else {
00046 double hypotenuse = arc_length/theta_motion;
00047 xpos_before_angles = sin(theta_motion)*hypotenuse;
00048 ypos_before_angles = -(hypotenuse - cos(theta_motion)*hypotenuse);
00049 }
00050
00051
00052
00053 chord_length = sqrt(xpos_before_angles*xpos_before_angles +
00054 ypos_before_angles*ypos_before_angles);
00055
00056
00057 total_motion_angle = atan2(y_motion, x_motion) +
00058 atan2(ypos_before_angles, xpos_before_angles) + *theta;
00059
00060
00061 *x += cos(total_motion_angle) * chord_length;
00062 *y += sin(total_motion_angle) * chord_length;
00063 *theta += theta_motion;
00064
00065
00066 while(*theta >= 2*M_PI) *theta -= 2*M_PI;
00067 while(*theta < 0) *theta += 2*M_PI;
00068
00069
00070 }
00071
00072 #ifdef __cplusplus
00073 }
00074 #endif