科目二修改

yy1717

2023-03-31 4bd08f0355b6b2cf3c027202d5ad301b4e182953

 lib/src/main/cpp/test_items/area_exam.cpp

@@ -16,17 +16,208 @@
#include "../common/apptimer.h"
#include "../test_common/odo_graph.h"
#include "../test_common/Geometry.h"
#include "../common/observer.h"

#define DEBUG(fmt, args...)     LOGD("<area_exam> <%s>: " fmt, __func__, ##args)

static int CurrExamStatus = EXAM_AREA_NONE;      // 1 测试完成 0 测试中 -1 测试错误退出
static int CurrExamMapIndex = -1;
static int CurrEnterMapIndex = -1;
ilovers::Observer<std::function<void(move_status_t)>> CarMoveEvent;

static void DetectEnterOrExitMap(int moveDirect, const car_model *CarModel, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList);
static int EnterMap(int moveDirect, const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList);
static bool ExitMap(const car_model *car, int index, LIST_AREA_MAP &mapList);
static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList);
static void ExecuteExam(int index, LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int move, double azimuth, const struct RtkTime* rtkTime);
static bool ProximityArea(Line &base_line, Line &line);
static bool CrossingStartLine(Line &trace, Line &start_line);
static void ProximityReminders(prime_t &prime);
static void DetectCarMove(prime_t &prime);

void AreaExam(prime_t &prime)
{
    switch (prime.curr_exam_map.type) {
        case MAP_TYPE_PARK_BUTTOM:
            break;
        case MAP_TYPE_PARK_EDGE:
            break;
        case MAP_TYPE_TURN_90:
            break;
        case MAP_TYPE_UPHILL:
            break;
        case MAP_TYPE_CURVE:
            break;
        default:
            if (EnterMap()) {

            }
            break;
    }
    DetectCarMove(prime);
}

static void DetectCarMove(prime_t &prime)
{
    static move_status_t prevMove = STOP;
    if (prime.pMotion->move != prevMove) {
        // Notify
        CarMoveEvent.Notify(prime.pMotion->move);
        prevMove = prime.pMotion->move;
    }
}

void RegisterCarMoveObserver(void (*ptr)(move_status_t))
{
    CarMoveEvent.Connect(ptr);
}

void UnregisterCarMoveObserver(int handle)
{
    CarMoveEvent.Disconnect(handle);
}

void EnterMap(prime_t &prime)
{
    if (prime.curr_exam_map.type != 0) {
        return;
    }

    if (prime.prev_modeling_index == -1 || prime.curr_modeling_index == -1) {
        return;
    }

    PointF &lp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
    PointF &lp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
    PointF &rp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
    PointF &rp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];

    Line left_trace, right_trace;

    MAKE_LINE(left_trace, lp1, lp2);
    MAKE_LINE(right_trace, rp1, rp2);

    Line start_line;
    for (int i = 0; i < prime.pMap->park_button_map.size(); ++i) {
        MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[1], prime.pMap->park_button_map[i].map[0]);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.curr_exam_map.type = MAP_TYPE_PARK_BUTTOM;
            prime.curr_exam_map.map_idx = i;
            StartParkBottom(prime);
            return;
        }
        MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[7], prime.pMap->park_button_map[i].map[6]);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.curr_exam_map.type = MAP_TYPE_PARK_BUTTOM;
            prime.curr_exam_map.map_idx = i;
            StartParkBottom(prime);
            return;
        }
    }

    for (int i = 0; i < prime.pMap->park_edge_map.size(); ++i) {
        MAKE_LINE(start_line, prime.pMap->park_edge_map[i].map[1], prime.pMap->park_edge_map[i].map[0]);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.curr_exam_map.type = MAP_TYPE_PARK_EDGE;
            prime.curr_exam_map.map_idx = i;
            return;
        }
    }

    for (int i = 0; i < prime.pMap->uphill_map.size(); ++i) {
        PointF vPoint = Calc3Point(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[0], DistanceOf(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[7]), 'R');
        MAKE_LINE(start_line, prime.pMap->uphill_map[i].map[0], vPoint);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.curr_exam_map.type = MAP_TYPE_UPHILL;
            prime.curr_exam_map.map_idx = i;
            return;
        }
    }
    for (int i = 0; i < prime.pMap->curve_map.size(); ++i) {
        MAKE_LINE(start_line, prime.pMap->curve_map[i].right_start_point, prime.pMap->curve_map[i].left_start_point);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.curr_exam_map.type = MAP_TYPE_CURVE;
            prime.curr_exam_map.map_idx = i;
            return;
        }
    }
    for (int i = 0; i < prime.pMap->turn_a90_map.size(); ++i) {
        MAKE_LINE(start_line, prime.pMap->turn_a90_map[i].map[0], prime.pMap->turn_a90_map[i].map[1]);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.curr_exam_map.type = MAP_TYPE_TURN_90;
            prime.curr_exam_map.map_idx = i;
            return;
        }
    }
}

// 车轮驶过线，且车头位于右侧
static bool CrossingStartLine(Line &trace, Line &start_line)
{
    PointF head = {.X = trace.X1, .Y = trace.Y1};

    if (IntersectionOf(trace, start_line) == GM_Intersection
        && IntersectionOfLine(head, start_line) == RELATION_RIGHT) {
        return true;
    }
    return false;
}

void FarawayMap(prime_t &prime)
{
    if (prime.arriving_map.type != 0) {
        PointF &car_head = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]];
        PointF car_head_trend = PointExtend(car_head, 7, prime.pModeling->yaw);
        Line car_head_line;
        MAKE_LINE(car_head_line, car_head, car_head_trend);


    }
}

static void ProximityReminders(prime_t &prime)
{
    if (prime.curr_exam_map.type != MAP_TYPE_NONE && prime.arriving_map.type != MAP_TYPE_NONE) {
        return;
    }

    PointF &car_head = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]];
    PointF car_head_trend = PointExtend(car_head, 6, prime.pModeling->yaw);
    Line car_head_line;
    MAKE_LINE(car_head_line, car_head, car_head_trend);

    Line start_line;
    for (int i = 0; i < prime.pMap->park_button_map.size(); ++i) {  // 左右2条控制线都可作为入口
        MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[1], prime.pMap->park_button_map[i].map[0]);

        MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[7], prime.pMap->park_button_map[i].map[6]);

    }
    for (int i = 0; i < prime.pMap->park_edge_map.size(); ++i) {
        MAKE_LINE(start_line, prime.pMap->park_edge_map[i].map[1], prime.pMap->park_edge_map[i].map[0]);
    }

    for (int i = 0; i < prime.pMap->uphill_map.size(); ++i) {
        PointF vPoint = Calc3Point(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[0], DistanceOf(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[7]), 'R');
        MAKE_LINE(start_line, prime.pMap->uphill_map[i].map[0], vPoint);
    }
    for (int i = 0; i < prime.pMap->curve_map.size(); ++i) {
        MAKE_LINE(start_line, prime.pMap->curve_map[i].right_start_point, prime.pMap->curve_map[i].left_start_point);
    }
    for (int i = 0; i < prime.pMap->turn_a90_map.size(); ++i) {
        MAKE_LINE(start_line, prime.pMap->turn_a90_map[i].map[0], prime.pMap->turn_a90_map[i].map[1]);
    }
}

// 判断是否接近场地的起始线
// 车头趋势线是否和入口线相交
// 和入口线的夹角
static bool ProximityArea(Line &base_line, Line &line)
{
    PointF head = {.X = line.X1, .Y = line.Y1};

    if (IntersectionOf(base_line, line) == GM_Intersection
        && DistanceOf(head, base_line) > 1) {
        double angle = AngleOfTowLine(base_line, line);
        if (angle >= 240 && angle <= 300) {
            return true;
        }
    }

    return false;
}

void TerminateAreaExam(void)
{
@@ -37,479 +228,4 @@
{
    CurrExamMapIndex = -1;
    ResetOdo();
}

void TestAreaGeneral(LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, double azimuth, const struct RtkTime *rtkTime)
{
    UpdataOdo(speed, moveDirect, rtkTime);

    DetectEnterOrExitMap(moveDirect, car, CarModelList, AreaMapList);

    ExecuteExam(CurrExamMapIndex, AreaMapList, car, CarModelList, speed, moveDirect, azimuth, rtkTime);
}

void DistanceOfTire2X(std::vector<double> &array, const car_model *car, std::vector<Line> line_set)
{
    double ld = 100, rd = 100;

    array.clear();

    for (auto line = line_set.begin(); line != line_set.end(); line++) {
        double ld_t = 100, rd_t = 100;
        double d;
        if (VerticalPointOnLine(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], *line)) {
            d = DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], *line);
            if (d < ld_t) {
                ld_t = d;
            }
        }
        if (VerticalPointOnLine(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], *line)) {
            d = DistanceOf(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], *line);
            if (d < ld_t) {
                ld_t = d;
            }
        }

        if (VerticalPointOnLine(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], *line)) {
            d = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], *line);
            if (d < rd_t) {
                rd_t = d;
            }
        }
        if (VerticalPointOnLine(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], *line)) {
            d = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], *line);
            if (d < rd_t) {
                rd_t = d;
            }
        }

        if (isEqual2(ld_t, rd_t)) {
            if (!isEqual(ld_t, 100)) {
                if (rd_t < rd)
                    rd = rd_t;
                if (ld_t < ld)
                    ld = ld_t;
            }
        } else if (ld_t > rd_t) {
            if (rd_t < rd)
                rd = rd_t;
        } else {
            if (ld_t < ld)
                ld = ld_t;
        }
    }

    array.push_back(ld);
    array.push_back(rd);
}

static void DetectEnterOrExitMap(int moveDirect, const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList)
{
    if (CurrExamMapIndex < 0) {
        if (CurrEnterMapIndex < 0) {
            CurrEnterMapIndex = EnterMap(moveDirect, car, CarModelList, mapList);
            if (CurrEnterMapIndex >= 0) {
                DEBUG("进入某个子项目 idx = %d", CurrEnterMapIndex);
                CurrExamMapIndex = CurrEnterMapIndex;
                CurrExamStatus = EXAM_AREA_START;
            }
        } else {
            if (ExitMap(car, CurrEnterMapIndex, mapList)) {
                DEBUG("离开某个子项目 idx = %d", CurrEnterMapIndex);
                CurrEnterMapIndex = -1;
            }
        }
    }
}

static void ExecuteExam(int index, LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int move, double azimuth, const struct RtkTime* rtkTime)
{
    if (index >= 0) {
        if (CurrExamStatus == EXAM_AREA_START) {
            DEBUG("CurrExamMapIndex %d mtype %d", AreaMapList[index].id, AreaMapList[index].type);

            switch (AreaMapList[index].type) {
                case MAP_TYPE_PARK_BUTTOM:
                    DEBUG("进入倒车入库场地 %d", AreaMapList[index].id);

                    StartParkBottom(AreaMapList[index].id, move, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_STOP_START:
                    DEBUG("进入上坡起步场地 %d", AreaMapList[index].id);

                    StartSAS(AreaMapList[index].id, &AreaMapList[index].map, car, move, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_PART_EDGE:
                    DEBUG("进入侧方位停车场地 %d",  AreaMapList[index].id);

                    StartParkEdge(AreaMapList[index].id, move, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_CURVE:
                    DEBUG("进入曲线行驶场地 %d", AreaMapList[index].id);

                    StartDrivingCurve(AreaMapList[index].id, move, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_TURN_90:
                    DEBUG("进入直角转弯场地 %d", AreaMapList[index].id);

                    StartTurnA90(AreaMapList[index].id, move, azimuth, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                default:break;
            }
        } else if (CurrExamStatus == EXAM_AREA_RUN) {
            int testing = 0;
            switch (AreaMapList[index].type) {
                case MAP_TYPE_PARK_BUTTOM:
                    testing = TestParkBottom(&AreaMapList[index].map,
                                             car, NULL, speed, move, rtkTime);
                    break;
                case MAP_TYPE_STOP_START:
                    testing = TestSAS(&AreaMapList[index].map, car, NULL, speed, move, rtkTime);
                    break;
                case MAP_TYPE_PART_EDGE:
                    testing = TestParkEdge(&AreaMapList[index].map, car, NULL, speed, move, rtkTime);
                    break;
                case MAP_TYPE_CURVE:
                    testing = TestDrivingCurve(&AreaMapList[index].map, &AreaMapList[index].map2, car, NULL, speed, move, rtkTime);
                    break;
                case MAP_TYPE_TURN_90:
                    testing = TestTurnA90(&AreaMapList[index].map, car, NULL, azimuth, speed, move, rtkTime);
                    break;
                default:
                    break;
            }

            if (testing > 0) {
                CurrExamStatus = EXAM_AREA_RUN;
            } else {
                CurrExamStatus = EXAM_AREA_END;
            }
        }
        if (CurrExamStatus != EXAM_AREA_RUN) {
            // 某项结束
            CurrExamStatus = EXAM_AREA_NONE;
            CurrExamMapIndex = -1;
            DEBUG("结束当前项目");
        }
    }
}

static int EnterMap(int moveDirect, const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList)
{
    vector<int> score(mapList.size(), 0);       // 场地重合时，车头尾都在内的优先

    for (int i = 0; i < mapList.size() && car != NULL; ++i) {
        // 车前轮或后轮轨迹越过触发线
        if (mapList[i].type == MAP_TYPE_STOP_START) {
            // 构造虚拟的左上角点
            double x9, y9, xo, yo;

            xo = (mapList[i].map.point[0].X + mapList[i].map.point[7].X) / 2;
            yo = (mapList[i].map.point[0].Y + mapList[i].map.point[7].Y) / 2;

            x9 = 2*xo - mapList[i].map.point[8].X;
            y9 = 2*yo - mapList[i].map.point[8].Y;

            Line triggerLine;

            triggerLine.X1 = mapList[i].map.point[0].X;
            triggerLine.Y1 = mapList[i].map.point[0].Y;
            triggerLine.X2 = x9;
            triggerLine.Y2 = y9;

            if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
                return i;
        }
        if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
            Line triggerLine;

            MakeLine(&triggerLine, &(mapList[i].map.point[1]), &(mapList[i].map.point[0]));

            if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
                return i;

            MakeLine(&triggerLine, &(mapList[i].map.point[7]), &(mapList[i].map.point[6]));

            if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
                return i;

            // 全车在场地内
            if (moveDirect == 0) {
                Polygon carBody;
                bool ret = false;
                carBody.num = car->bodyNum;
                carBody.point = (PointF *) malloc(carBody.num * sizeof(PointF));
                for (int i = 0; i < carBody.num; ++i) {
                    carBody.point[i] = car->carXY[car->body[i]];
                }

                if (IntersectionOf(&carBody, &mapList[i].map) == GM_Containment) {
                    ret = true;
                }

                free(carBody.point);

                if (ret)
                    return i;
            }
//            // 车头顶点在场地内
//            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
//                Line enterLine1, enterLine2;
//
//                MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
//                MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
//
//                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
//                    DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
//                    score[i]++;
//            }
//            // 车尾顶点在场地内
//            if (IntersectionOf(car->carXY[ car->axial[AXIAL_REAR] ], &mapList[i].map) == GM_Containment) {
//                Line enterLine1, enterLine2;
//
//                MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
//                MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
//
//                if (DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine1) > 0.1 &&
//                    DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine2) > 0.1)
//                    score[i]++;
//            }
        }
        if (mapList[i].type == MAP_TYPE_PART_EDGE) {
            Line triggerLine;

            MakeLine(&triggerLine, &(mapList[i].map.point[1]), &(mapList[i].map.point[0]));

            if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
                return i;

            // 全车在场地内
            if (moveDirect == 0) {
                Polygon carBody;
                bool ret = false;
                carBody.num = car->bodyNum;
                carBody.point = (PointF *) malloc(carBody.num * sizeof(PointF));
                for (int i = 0; i < carBody.num; ++i) {
                    carBody.point[i] = car->carXY[car->body[i]];
                }

                if (IntersectionOf(&carBody, &mapList[i].map) == GM_Containment) {
                    ret = true;
                }

                free(carBody.point);

                if (ret)
                    return i;
            }
//            // 车头顶点在场地内
//            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
//                Line enterLine;
//
//                MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
//
//                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine) > 0.1)
//                    score[i]++;
//            }
//            // 车尾顶点在场地内
//            if (IntersectionOf(car->carXY[ car->axial[AXIAL_REAR] ], &mapList[i].map) == GM_Containment) {
//                Line enterLine;
//
//                MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
//
//                if (DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine) > 0.1)
//                    score[i]++;
//            }
        }
        if (mapList[i].type == MAP_TYPE_TURN_90) {
            // 车前轮或后轮轨迹越过触发线
            Line triggerLine;

            MakeLine(&triggerLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));

            if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
                return i;
        }
        if (mapList[i].type == MAP_TYPE_CURVE) {
            // 车前轮或后轮轨迹越过触发线
            Line triggerLine;

            MakeLine(&triggerLine, &mapList[i].map2.point[0], &mapList[i].map.point[0]);
            if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
                return i;
        }
    }

    for (int i = 0; i < score.size(); ++i) {
        if (score[i] == 2) {
            return i;
        }
    }

    for (int i = 0; i < score.size(); ++i) {
        if (score[i] == 1) {
            return i;
        }
    }

    return -1;
}

static bool ExitMap(const car_model *car, int index, LIST_AREA_MAP &mapList)
{
    bool ret = false;
    if (index < 0 || index >= mapList.size()) return true;

    if (mapList[index].type == MAP_TYPE_PARK_BUTTOM ||
        mapList[index].type == MAP_TYPE_PART_EDGE ||
        mapList[index].type == MAP_TYPE_TURN_90) {
        // 全车都需不在地图中
        Polygon carBody;

        carBody.num = car->bodyNum;
        carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
        for (int i = 0; i < carBody.num; ++i) {
            carBody.point[i] = car->carXY[car->body[i]];
        }

        if (IntersectionOf(&carBody, &mapList[index].map) == GM_None) {
            ret = true;
        }

        free(carBody.point);
    }
    if (mapList[index].type == MAP_TYPE_STOP_START) {
        // 构造虚拟的左上角点
        double x10, y10, x9, y9, xo, yo;

        bool enter = false;

        xo = (mapList[index].map.point[0].X + mapList[index].map.point[7].X) / 2;
        yo = (mapList[index].map.point[0].Y + mapList[index].map.point[7].Y) / 2;

        x9 = 2*xo - mapList[index].map.point[8].X;
        y9 = 2*yo - mapList[index].map.point[8].Y;

        if (mapList[index].map.num > 9) {
            // 构造虚拟的右上角点
            xo = (mapList[index].map.point[9].X + mapList[index].map.point[7].X) / 2;
            yo = (mapList[index].map.point[9].Y + mapList[index].map.point[7].Y) / 2;
            x10 = 2*xo - mapList[index].map.point[8].X;
            y10 = 2*yo - mapList[index].map.point[8].Y;
        }

        Polygon map;

        map.num = 4;
        map.point = (PointF *) malloc(map.num * sizeof(PointF));

        if (mapList[index].map.num <= 9) {
            map.point[0] = mapList[index].map.point[0];
            map.point[1] = mapList[index].map.point[8];
            map.point[2] = mapList[index].map.point[7];
            map.point[3].X = x9;
            map.point[3].Y = y9;
        } else {
            map.point[0] = mapList[index].map.point[0];
            map.point[1] = mapList[index].map.point[9];
            map.point[2].X = x10;
            map.point[2].Y = y10;
            map.point[3].X = x9;
            map.point[3].Y = y9;
        }
        // 全车都需不在地图中
        Polygon carBody;

        carBody.num = car->bodyNum;
        carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
        for (int i = 0; i < carBody.num; ++i) {
            carBody.point[i] = car->carXY[car->body[i]];
        }

        if (IntersectionOf(&carBody, &map) == GM_None) {
            ret = true;
        }

        free(carBody.point);
        free(map.point);
    }
    if (mapList[index].type == MAP_TYPE_CURVE) {
        ret = ExitDrivingCurveArea(&mapList[index].map, &mapList[index].map2, car);
    }

    return ret;
}

static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    bool trigger = false;

    if (CarModelList.size() < 5)
        return trigger;

    Polygon trace, trace2;
    int pn = 0;

    trace2.num = trace.num = 5;
    trace.point = (PointF *) malloc(sizeof(PointF) * trace.num);
    trace2.point = (PointF *) malloc(sizeof(PointF) * trace2.num);

    list<car_model *>::iterator iter = CarModelList.begin();

    car_model *c1 = *iter;

    trace.point[pn] = centerOfTwoPoint(c1->carXY[c1->left_front_tire[TIRE_OUTSIDE]], c1->carXY[c1->right_front_tire[TIRE_OUTSIDE]]);
    trace2.point[pn++] = centerOfTwoPoint(c1->carXY[c1->left_rear_tire[TIRE_OUTSIDE]], c1->carXY[c1->right_rear_tire[TIRE_OUTSIDE]]);

    ++iter;

    while (iter != CarModelList.end() && pn < trace.num) {
        car_model *c2 = *iter;

        uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);

        if (tdiff >= D_SEC(1)) {
            trace.point[pn] = centerOfTwoPoint(c2->carXY[c2->left_front_tire[TIRE_OUTSIDE]], c2->carXY[c2->right_front_tire[TIRE_OUTSIDE]]);
            trace2.point[pn++] = centerOfTwoPoint(c2->carXY[c2->left_rear_tire[TIRE_OUTSIDE]], c2->carXY[c2->right_rear_tire[TIRE_OUTSIDE]]);
            c1 = c2;
        }
        ++iter;
    }

    PointF p1, p2;

    p1.X = triggerLine.X1;
    p1.Y = triggerLine.Y1;
    p2.X = triggerLine.X2;
    p2.Y = triggerLine.Y2;

    int pp = 0;
    for (int p = 1; p < pn; ++p) {
        Line trace_line, trace2_line;

        MakeLine(&trace_line, &trace.point[pp], &trace.point[p]);
        MakeLine(&trace2_line, &trace2.point[pp], &trace2.point[p]);

        if ((IntersectionOf(trace_line, triggerLine) == GM_Intersection &&
            IntersectionOfLine(p1, p2, centerOfTwoPoint(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], car->carXY[car->right_front_tire[TIRE_OUTSIDE]])) == -1 &&
            DistanceOf(centerOfTwoPoint(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]), triggerLine) > 0.1) ||
                (IntersectionOf(trace2_line, triggerLine) == GM_Intersection &&
                        IntersectionOfLine(p1, p2, centerOfTwoPoint(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], car->carXY[car->right_rear_tire[TIRE_OUTSIDE]])) == -1 &&
                        DistanceOf(centerOfTwoPoint(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]), triggerLine) > 0.1) ) {
            // 碰到触发线
            DEBUG("碰撞触发线 引发地图");
            trigger = true;
            goto SEARCH_TRIGGER_LINE_END;
        }
    }

    SEARCH_TRIGGER_LINE_END:
    free(trace.point);

    return trigger;
}