坐标

yy1717

2020-04-17 148a951e0d44577997fd2790f889dcdd7dbd6889

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

@@ -983,9 +983,30 @@
    }
    return -1;
}
#if 0

#if 1

typedef struct {
    int road;
    int segment;
    int track;
} CarOnTrackInfo_t;

static CarOnTrackInfo_t CarOnTrackInfo;

void TestRoadGeneral(struct road_exam2_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    // 检测车道变更
    CarOnTrackInfo_t newCarOnTrackInfo;

    if (newCarOnTrackInfo.road == CarOnTrackInfo.road &&
            newCarOnTrackInfo.segment == CarOnTrackInfo.segment &&
            newCarOnTrackInfo.track != CarOnTrackInfo.track) {

    }

    CarOnTrackInfo = newCarOnTrackInfo;


}

@@ -1075,19 +1096,6 @@
    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;
@@ -1096,12 +1104,11 @@
        carBody.point[i] = car->carXY[car->body[i]];
    }

    if (IntersectionOf(&carBody, &roadArea) == GM_None) {
    if (IntersectionOf(&carBody, &map.roads[currIndex].area) == GM_None) {
        ret = true;
    }

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

@@ -1134,61 +1141,94 @@
    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;

    CarOnTrackInfo_t newInfo;

    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();
        int separate_line_num = map.roads[currIndex].separate[i].lines.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) {
                        // 首尾两端，应采用延长线的方式
        struct vrecord_ {
            int valid;
            int p1;
            int p2;
        };

        vector<struct vrecord_> vrecord;
        vrecord.clear();

        bool match1 = false;
        // 遍历当前分段的每一条线
        for (int j = 0; j < separate_line_num; ++j) {
            Line theLine;
            int p1 = 0;

            struct vrecord_ v;
            v.valid = 0;

            // 单独的一条虚线
            for (int p2 = 1; p2 < map.roads[currIndex].separate[i].lines[j].num; ++p2) {
                MakeLine(&theLine, &map.roads[currIndex].separate[i].lines[j].point[p1],
                         &map.roads[currIndex].separate[i].lines[j].point[p2]);
                if (VerticalPointOnLine(car->basePoint, theLine)) {
                    v.valid = 1;
                    v.p1 = p1;
                    v.p2 = p2;
                    match1 = true;
                    break;
                }
                p1 = p2;
            }

            vrecord.push_back(v);

            track = separate_line_num;//
        }



        if (match1) {
            for (int x = 0; x < vrecord.size(); ++x) {
                if (vrecord[i].valid == 0) {
                    // 首尾两段线的延申必有一个垂点
                    if (DistanceOf(car->carXY[ car->axial[AXIAL_FRONT] ], map.roads[currIndex].separate[i].lines[x].point[0]) <
                        DistanceOf(car->carXY[ car->axial[AXIAL_FRONT] ], map.roads[currIndex].separate[i].lines[x].point[1])) {
                        vrecord[x].p1 = 0;
                        vrecord[x].p2 = 1;
                    } else {
                        vrecord[x].p1 = map.roads[currIndex].separate[i].lines[x].num - 2;
                        vrecord[x].p2 = map.roads[currIndex].separate[i].lines[x].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;
                int rel = IntersectionOfLine(map.roads[currIndex].separate[i].lines[x].point[vrecord[x].p1],
                                             map.roads[currIndex].separate[i].lines[x].point[vrecord[x].p2],
                                             car->basePoint);
                if (rel != -1) {
                    newInfo.road = currIndex;
                    newInfo.segment = i;
                    newInfo.track = x;
                    break;
                }
            }

            newInfo.road = currIndex;
            newInfo.segment = i;
            newInfo.track = vrecord.size();

            break;
        }
    }

    DETECT_SEPARATE_END:
    ;
}



#endif