科目二修改

yy1717

2023-03-31 4bd08f0355b6b2cf3c027202d5ad301b4e182953

 lib/src/main/cpp/test_items/driving_curve.cpp

@@ -34,7 +34,7 @@

static bool UpdateStartLine(struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
static bool UpdateEndLine(bool mode, struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone, int &who);
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model_t *car, struct scan_window_t *zone, int &who);

void StartDrivingCurve(int index, int moveDirect, const struct RtkTime *rtkTime)
{
@@ -55,7 +55,54 @@
    MA_EnterMap(mapIndex, MAP_TYPE_CURVE, 1);
}

int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
// 曲线场地关键点
// 入口左右两点，出口左右两点，前半部大小圆圆心和直径，后半部大小圆圆心和直径，实际测量大小圆圆心偏差在20cm内
int TestCurve(const curve_map_t *map, const car_model_t *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    // 判断是否在范围内，如果4个车轮都不在曲线内，视为车辆离开场地
    vector<PointF> pxs = {car->carXY[car->left_front_tire[TIRE_OUTSIDE]],
                          car->carXY[car->left_rear_tire[TIRE_OUTSIDE]],
                          car->carXY[car->right_front_tire[TIRE_OUTSIDE]],
                          car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]};

    int collide = 0;

    for (auto px: pxs) {
        if (IntersectionOfLine(map->front_half_small_circle_centre, map->back_half_small_circle_centre, px) == RELATION_RIGHT) {
            // 位于前半部,实际场地左右边线起点、终点和圆心基本不会共线，可能差异近一米，所以要分开计算
            // 判断的点超过左右起点，即在线的左侧
            if (AngleOf(map->front_half_small_circle_centre, map->left_start_point, px) < 180) {
                if (DistanceOf(map->front_half_small_circle_centre, px) < map->front_half_small_circle_radius) {
                    collide = 1;
                    continue;
                }
            }
            if (AngleOf(map->front_half_big_circle_centre, map->right_start_point, px) < 180) {
                if (DistanceOf(map->front_half_big_circle_centre, px) > map->front_half_big_circle_radius) {
                    collide = 2;
                    continue;
                }
            }
        } else {
            // 判断的点不超过左右终点，即在线的左侧
            if (AngleOf(map->back_half_small_circle_centre, map->right_end_point, px) < 180) {
                if (DistanceOf(map->back_half_small_circle_centre, px) < map->back_half_small_circle_radius) {
                    collide = 2;
                    continue;
                }
            }
            if (AngleOf(map->back_half_big_circle_centre, map->left_end_point, px) < 180) {
                if (DistanceOf(map->back_half_big_circle_centre, px) > map->back_half_big_circle_radius) {
                    collide = 1;
                    continue;
                }
            }
        }
    }

}

int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model_t *car, const car_model_t *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    Polygon tireRect;
    int who = 0;
@@ -279,7 +326,7 @@
    return crash;
}

bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model *car)
bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model_t *car)
{
// 全车都需不在地图中
    bool ret = false;
@@ -316,7 +363,7 @@
}

// 车轮是否压边线
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone, int &who)
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model_t *car, struct scan_window_t *zone, int &who)
{
    bool ret = false;