double PurePursuitController::judge_lr(const TrajectoryPoint& start_point, const TrajectoryPoint& end_point, const TrajectoryPoint& j_point) { double is_prec = (start_point.path_point.y - end_point.path_point.y) * j_point.path_point.x + (end_point.path_point.x - start_point.path_point.x) * j_point.path_point.y + start_point.path_point.x * end_point.path_point.y - end_point.path_point.x * start_point.path_point.y; if (is_reverse) { is_prec = (end_point.path_point.y - start_point.path_point.y) * j_point.path_point.x + (start_point.path_point.x - end_point.path_point.x) * j_point.path_point.y + start_point.path_point.y * end_point.path_point.x - end_point.path_point.y * start_point.path_point.x; } double np = is_prec > 0.0 ? 1.0 : (-1.0); return np; }
时间: 2024-05-05 22:22:29 浏览: 131
leetcode和oj-hello_world_ints:hello_world_ints
这段代码是C++语言编写的。它是一个纯追踪控制器(Pure Pursuit Controller)中的一部分,用于判断当前车辆在轨迹上的位置相对于目标点的左侧还是右侧。该函数接受三个轨迹点作为输入参数,计算车辆当前位置相对于目标点的位置关系,并返回一个double类型的值,表示车辆相对于目标点的左右位置关系,其中1.0表示在目标点左侧,-1.0表示在目标点右侧。
阅读全文