坐标

yy1717

2020-04-17 148a951e0d44577997fd2790f889dcdd7dbd6889

坐标

 lib/src/main/cpp/driver_test.h

@@ -135,7 +135,7 @@

typedef struct {
    int character;          // 属性《实线、虚线》
    std::vector<Polygon> line;      // 每一条线
    std::vector<Polygon> lines;      // 每一条线
} separate_t;

struct road_t {

 lib/src/main/cpp/test_common/Geometry.cpp

@@ -442,6 +442,18 @@
    }
}

int IntersectionOfLine(PointF p, Line line)
{
    PointF p1, p2;

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

    IntersectionOfLine(p1, p2, p);
}

/***************************************************************
 * 得到p3于p1,p2组成的直线上的垂点
 * @param p1
@@ -568,3 +580,20 @@

    return p3;
}

/*******************************************************
 * ori点在yaw方向上延长的距离
 * @param ori
 * @param length
 * @param yaw
 * @return
 */
PointF PointExtend(PointF ori, double length, double yaw)
{
    PointF ext;

    ext.X = ori.X + length * sin(toRadians(yaw));
    ext.Y = ori.Y + length * cos(toRadians(yaw));

    return ext;
}

 lib/src/main/cpp/test_common/Geometry.h

@@ -57,8 +57,9 @@
bool PartInsidePolygon(const Polygon *t1, const Polygon *t2);
bool OutsidePolygon(const Polygon *t1, const Polygon *t2);
int IntersectionOfLine(PointF p1, PointF p2, PointF p3);
int IntersectionOfLine(PointF p, Line line);
PointF GetVerticalPoint(PointF p1, PointF p2, PointF p3);
bool VerticalPointOnLine(PointF point, Line line);
PointF Calc3Point(PointF p1, PointF p2, double L, char dir);

PointF PointExtend(PointF ori, double length, double yaw);
#endif //GUI_GEOMETRY_H

 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

 lib/src/main/cpp/test_items2/through_something.cpp

@@ -132,3 +132,37 @@

    return index;
}

static void ThroughZebraCrossing(struct road_exam2_map &map, const car_model *car)
{
    for (int i = 0; i < map.specialAreas.size(); i++) {
        if (map.specialAreas[i].character == 'zebra') {
            double distance = 0.0;

            double yaw = YawOf(car->carXY[ car->axial[AXIAL_FRONT] ], car->carXY[ car->axial[AXIAL_REAR] ]);
            PointF extPoint = PointExtend(car->carXY[ car->axial[AXIAL_FRONT] ], LASTEST_BREAK_POINT, yaw);
            Line extLine;
            MakeLine(&extLine, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint);

            if (IntersectionOf(extLine, map.specialAreas[i].startLine) == GM_Intersection &&
                    IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], map.specialAreas[i].startLine) == 1 ) {
                jj = true;
                if (ReadCarStatus(BREAK) == BREAK_ACTIVE) {
                    breakActive = 1;
                }
            } else {
                jj = false;
            }





            if (distance < LASTEST_BREAK_POINT) {
                if (ReadCarStatus(BREAK) == BREAK_ACTIVE) {
                    breakActive = 1;
                }
            }
        }
    }
}