我的数学基础和代码功底实在需要加强啊~下面简单实现了已知机器人的位姿(r_x,r_y,r_th),判断任一点(obj_x,obj_y)的方位。
/声明部分/
double get0to360(double dx, double dy);
int checkLorR(double obj, double r);
double getTheta(double obj, double r);
void getRelation(double r_th, double r_x, double r_y, double obj_x, double obj_y, int &flag, double &theta);
typedef struct {
double x;
double y;
double th;
}odm;
/主函数/
int main() {
double obj_x = 383, obj_y = 397; //点的坐标示例
int flag;
double theta;
odm robot;
robot.x = 386, robot.y = 373, robot.th = 31.6908; //机器人位姿示例
getRelation(robot.th, robot.x, robot.y, obj_x, obj_y, flag, theta);
//根据flag和夹角判断在哪个方位
if (flag == 1)
std::cout << "家具在机器人左、";
else if (flag == 2)
std::cout << "家具在机器人右、";
else
std::cout << "家具在机器人正" ;
if (theta < 90)
std::cout << "前" << std::endl;
else
std::cout << "后" << std::endl;
std::cout << "夹角为" << theta << std::endl;
system("pause");
return 0;
}
/*函数定义部分/
/范围0~360/
double get0to360(double dy, double dx) {
double radian,degree;
radian = atan2(dy, dx); //atan2范围-pi~pi,atan2(y,x)
degree = radian * 180 / PI;
if (degree < 0)degree = degree + 360;
return degree;
}
/* 判断家具在机器人左边还是右边 */
// 1—左边;2—右边;0–方向重合
int checkLorR(double obj, double r) {
if (r > obj) {
if (r - obj < 180) {
return 2;
}
else if (r - obj > 180) {
return 1;
}
else
return 0;
}
else if (obj > r ) {
if (obj - r < 180) {
return 1;
}
else if (obj - r > 180) {
return 2;
}
else
return 0;
}
else
return 0;
}
/点和机器人的夹角/
double getTheta(double obj, double r) {
if (r > obj) {
if (r - obj < 180) {
return r - obj;
}
else if (r - obj >= 180) {
return 360 - (r - obj);
}
}
else if (obj > r) {
if (obj - r < 180) {
return obj - r;
}
else if (obj - r >= 180) {
return 360 - (obj - r);
}
}
else
return 0;
}
void getRelation(double r_th, double r_x, double r_y, double obj_x, double obj_y, int &flag, double &theta) {
double dx,dy,obj_th;
if (r_th < 0) r_th = r_th + 360;
dx = obj_x - r_x, dy = obj_y - r_y;
obj_th = get0to360(dy, dx);
flag = checkLorR(obj_th, r_th);
theta = getTheta(obj_th, r_th);
}