科目三

yy1717

2020-04-16 0820c9f0ffa19ba234bcc6858961c9e824f0ffa9

 lib/src/main/cpp/test_items2/road_exam.cpp

@@ -33,6 +33,8 @@
    START_CAR_DONE
};

static const int INVALID_ROAD = -1;

static const int TURN_THRESHOLD = 1;
static const int TURN_CHECK_INTERVAL = 500;
const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
@@ -981,5 +983,212 @@
    }
    return -1;
}
#if 0
void TestRoadGeneral(struct road_exam2_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{

}

/**************************************************
 * 车辆当前所在路段，车头需越过起始线一定距离
 * @param currIndex
 * @param map
 * @param car
 */
static void EnterRoad(int &currIndex, struct road_exam2_map &map, const car_model *car)
{
    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]];
    }

    Polygon roadArea;
    roadArea.num = 0;
    roadArea.point = NULL;

    for (int i = 0; i < map.roads.size(); ++i) {
        if (roadArea.point != NULL) {
            free(roadArea.point);
        }

        for (int x = 0; x < map.roads[i].leftEdge.size(); ++x) {
            roadArea.num += map.roads[i].leftEdge[x].points.size();
        }
        for (int x = 0; x < map.roads[i].rightEdge.size(); ++x) {
            roadArea.num += map.roads[i].rightEdge[x].points.size();
        }
        roadArea.point = (PointF *) malloc(roadArea.num * sizeof(PointF));

        int n = 0;
        for (int j = 0; j < map.roads[i].leftEdge.size(); j++) {
            for (int x = 0; x < map.roads[i].leftEdge[j].points.size(); ++x) {
                if (n > 0 && x == 0 && isEqual2(roadArea.point[n-1].X, map.roads[i].leftEdge[j].points[x].X) &&
                             isEqual2(roadArea.point[n-1].Y, map.roads[i].leftEdge[j].points[x].Y)) {
                    // 第一个点已经存在于上一条线
                } else {
                    roadArea.point[n++] = map.roads[i].leftEdge[j].points[x];
                }
            }
        }
        for (int j = map.roads[i].rightEdge.size(); j > 0; j--) {
            for (int x = map.roads[i].rightEdge[j].points.size(); x > 0; --x) {
                if (n > 0 && x == map.roads[i].rightEdge[j].points.size() &&
                            isEqual2(roadArea.point[n-1].X, map.roads[i].rightEdge[j - 1].points[x-1].X) &&
                            isEqual2(roadArea.point[n-1].Y, map.roads[i].rightEdge[j - 1].points[x-1].Y)) {
                    // 第一个点已经存在于上一条线
                } else {
                    roadArea.point[n++] = map.roads[i].rightEdge[j - 1].points[x - 1];
                }
            }
        }
        roadArea.num = n;

        if (IntersectionOf(car->carXY[car->axial[AXIAL_FRONT]], &roadArea) == GM_Containment) {
            currIndex = i;
            goto CHECK_CAR_ON_ROAD_END;
        }
    }
    currIndex = INVALID_ROAD;

    CHECK_CAR_ON_ROAD_END:
    if (roadArea.point != NULL) {
        free(roadArea.point);
    }
    free(carBody.point);
}

/******************************************************
 * 全车都需离开这个区域
 * @param currIndex
 * @param map
 * @param car
 * @return
 */
static bool ExitRoad(int currIndex, struct road_exam2_map &map, const car_model *car)
{
    // 全车都需不在地图中
    bool ret = false;

    if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
        return ret;

    Polygon roadArea;

    roadArea.num = map.roads[currIndex].leftEdge.size() + map.roads[currIndex].rightEdge.size();;
    roadArea.point = (PointF *) malloc(roadArea.num * sizeof(PointF));;

    int n = 0;
    for (int j = 0; j < map.roads[currIndex].leftEdge.size(); j++) {
        roadArea.point[n++] = map.roads[i].leftEdge[j];
    }
    for (int j = map.roads[currIndex].rightEdge.size(); j > 0; j--) {
        roadArea.point[n++] = map.roads[i].rightEdge[j - 1];
    }

    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, &roadArea) == GM_None) {
        ret = true;
    }

    free(carBody.point);
    free(roadArea.point);
    return ret;
}

static bool CrashSeparateLine(int currIndex, struct road_exam2_map &map, const car_model *car)
{
    Line frontAxle, rearAxle;

    if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
        return false;

    MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
    MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);

    // 分段
    for (int i = 0; i < map.roads[currIndex].separate.size(); i++) {
        // 分段中的每条线
        for (int j = 0; j < map.roads[currIndex].separate[i].line.size(); j++) {
            Line theLine;
            int p1 = 0;
            for (int p2 = 1; p2 < map.roads[currIndex].separate[i].line[j].num; ++p2) {
                MakeLine(&theLine, &map.roads[currIndex].separate[i].line[j].point[p1], &map.roads[currIndex].separate[i].line[j].point[p2]);
                if (IntersectionOf(theLine, frontAxle) == GM_Intersection ||
                    IntersectionOf(theLine, rearAxle) == GM_Intersection) {
                    return true;
                }
                p1 = p2;
            }
        }
    }
    return false;
}

struct {
    int road;
    int segment;
    int track;
} CarOnTrackInfo;

static void DetectSeparate(int currIndex, struct road_exam2_map &map, const car_model *car)
{
    int segment;
    int track = -1;

    if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
        return;

    for (int i = 0; i < map.roads[currIndex].separate.size(); i++) {
        if (map.roads[currIndex].separate[i].character == 0) {
            int separate_line_num = map.roads[currIndex].separate[i].line.size();

            // 遍历当前分段的每一条线
            for (int j = 0; j < separate_line_num; ++j) {
                Line theLine;
                int p1 = 0;
                bool match_line = false;
                // 单独的一条虚线
                for (int p2 = 1; p2 < map.roads[currIndex].separate[i].line[j].num; ++p2) {
                    MakeLine(&theLine, &map.roads[currIndex].separate[i].line[j].point[p1], &map.roads[currIndex].separate[i].line[j].point[p2]);
                    if (p1 == 0 || p2 == map.roads[currIndex].separate[i].line[j].num - 1) {
                        // 首尾两端，应采用延长线的方式
                    }
                    if (VerticalPointOnLine(car->basePoint, theLine)) {
                        match_line = true;
                        int rel = IntersectionOfLine(map.roads[currIndex].separate[i].line[j].point[p1],
                                           map.roads[currIndex].separate[i].line[j].point[p2],
                                           car->basePoint);
                        // 记录所在道路，分道段，分道情况
                        if (rel == 1) {             // 在左侧
                            segment = i;
                            track = j;
                            goto DETECT_SEPARATE_END;
                        } else if (rel != -1) {     // 在线上
                            segment = i;
                            track = j;
                            goto DETECT_SEPARATE_END;
                        }
                        break;
                    }
                    p1 = p2;
                }

                track = separate_line_num;
            }
        }
    }

    DETECT_SEPARATE_END:
    ;
}
#endif