修改科目三道路识别问题

fctom1215

2021-04-28 148254bb1dc170db320bcb208ca79b0e252751d8

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

@@ -22,8 +22,8 @@
static int CurrExamMapIndex = -1;
static int CurrEnterMapIndex = -1;

static void DetectEnterOrExitMap(const car_model *CarModel, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList);
static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList);
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);
@@ -43,7 +43,7 @@
{
    UpdataOdo(speed, moveDirect, rtkTime);

    DetectEnterOrExitMap(car, CarModelList, AreaMapList);
    DetectEnterOrExitMap(moveDirect, car, CarModelList, AreaMapList);

    ExecuteExam(CurrExamMapIndex, AreaMapList, car, CarModelList, speed, moveDirect, azimuth, rtkTime);
}
@@ -103,11 +103,11 @@
    array.push_back(rd);
}

static void DetectEnterOrExitMap(const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList)
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(car, CarModelList, mapList);
            CurrEnterMapIndex = EnterMap(moveDirect, car, CarModelList, mapList);
            if (CurrEnterMapIndex >= 0) {
                DEBUG("进入某个子项目 idx = %d", CurrEnterMapIndex);
                CurrExamMapIndex = CurrEnterMapIndex;
@@ -199,7 +199,7 @@
    }
}

static int EnterMap(const car_model *car, 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)
{
    vector<int> score(mapList.size(), 0);       // 场地重合时，车头尾都在内的优先

@@ -222,7 +222,7 @@
            triggerLine.X2 = x9;
            triggerLine.Y2 = y9;

            if (CrashTriggerLine(triggerLine, car, CarModelList))
            if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
                return i;
        }
        if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
@@ -230,14 +230,33 @@

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

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

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

            if (CrashTriggerLine(triggerLine, car, CarModelList))
            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;
@@ -266,8 +285,28 @@

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

            if (CrashTriggerLine(triggerLine, car, CarModelList))
            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;
@@ -293,7 +332,7 @@

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

            if (CrashTriggerLine(triggerLine, car, CarModelList))
            if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
                return i;
        }
        if (mapList[i].type == MAP_TYPE_CURVE) {
@@ -301,7 +340,7 @@
            Line triggerLine;

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

    car_model *c1 = *iter;

    trace.point[pn] = c1->carXY[c1->left_front_tire[TIRE_OUTSIDE]];
    trace2.point[pn++] = c1->carXY[c1->left_rear_tire[TIRE_OUTSIDE]];
    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;

@@ -435,8 +474,8 @@
        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] = c2->carXY[c2->left_front_tire[TIRE_OUTSIDE]];
            trace2.point[pn++] = c2->carXY[c2->left_rear_tire[TIRE_OUTSIDE]];
            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;
@@ -457,11 +496,11 @@
        MakeLine(&trace2_line, &trace2.point[pp], &trace2.point[p]);

        if ((IntersectionOf(trace_line, triggerLine) == GM_Intersection &&
            IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 &&
            DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], triggerLine) > 0.1) ||
            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, car->carXY[car->left_rear_tire[TIRE_OUTSIDE]]) == -1 &&
                        DistanceOf(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], triggerLine) > 0.1) ) {
                        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;