新的科目三

yy1717

2020-05-11 a073dc3c983b4c56c5da92642c9ad11995bdb844

新的科目三

 lib/src/main/cpp/common/net.cpp

@@ -17,6 +17,8 @@
#include "net.h"
#include "../jni_log.h"

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

using namespace std;

/*************************************************************
@@ -31,14 +33,14 @@
    bool found_first = false;

    if ((hptr = gethostbyname(host_name)) == NULL) {
        LOGE("gethostbyname error\n");
        DEBUG("gethostbyname error\n");
        return -1;
    }

    LOGD("official hostname: %s\n", hptr->h_name);
    DEBUG("official hostname: %s\n", hptr->h_name);

    for (pptr = hptr->h_aliases; *pptr != NULL; pptr++) {
        LOGD("alias:%s\n", *pptr);
        DEBUG("alias:%s\n", *pptr);
    }

    switch(hptr->h_addrtype)
@@ -46,7 +48,7 @@
        case AF_INET:
        {
            for (pptr = hptr->h_addr_list; *pptr != NULL; pptr++) {
                LOGD("addrsss:%s\n", inet_ntop(hptr->h_addrtype, *pptr, str, sizeof(str)));
                DEBUG("addrsss:%s\n", inet_ntop(hptr->h_addrtype, *pptr, str, sizeof(str)));
                if (!found_first) {
                    strcpy(net_addr, str);
                    found_first = true;
@@ -56,7 +58,7 @@
        }
        case AF_INET6:
        default:
            LOGD("unknown address type\n");
            DEBUG("unknown address type\n");
            break;
    }

@@ -80,53 +82,53 @@

//    int value, value_len;
//    getsockopt(fd, SOL_SOCKET, SO_RCVBUF, &value, &value_len);
//    LOGD("keepalive 0 %d", value);
//    DEBUG("keepalive 0 %d", value);

//    value = 128;
//    setsockopt(fd, SOL_SOCKET, SO_SNDBUF, &value, sizeof(value));
//    LOGD("keepalive 1 %d", value);
//    DEBUG("keepalive 1 %d", value);

//    getsockopt(fd, SOL_SOCKET, SO_SNDBUF, &value, &value_len);
//    LOGD("keepalive 1 %d", value);
//    DEBUG("keepalive 1 %d", value);


    /* Set: use keepalive on fd, default 0 */
    alive = 1;
    if (setsockopt(fd, SOL_SOCKET, SO_KEEPALIVE, &alive, sizeof(alive)) != 0)
    {
        LOGE("TCP Set keepalive error");
        DEBUG("TCP Set keepalive error");
        return -1;
    }
    /* 20 Seconds not data, send keeplive packet, default 7200 */
    idle = 20;
    if (setsockopt (fd, SOL_TCP, TCP_KEEPIDLE, &idle, sizeof(idle)) != 0)
    {
        LOGE("TCP Set keepalive idle error");
        DEBUG("TCP Set keepalive idle error");
        return -1;
    }
    /* If not recv respond, After 5 seconds retry, default 75 */
    intv = 5;
    if (setsockopt (fd, SOL_TCP, TCP_KEEPINTVL, &intv, sizeof(intv)) != 0)
    {
        LOGE("TCP Set keepalive intv error");
        DEBUG("TCP Set keepalive intv error");
        return -1;
    }
    /* If try 9 times and fail, we consider the tcp is disconnected, default 9 */
    cnt = 9;
    if (setsockopt (fd, SOL_TCP, TCP_KEEPCNT, &cnt, sizeof(cnt)) != 0)
    {
        LOGE("TCP Set keepalive cnt error");
        DEBUG("TCP Set keepalive cnt error");
        return -1;
    }

/*    int timeout = 10000;        // 10秒
    if (setsockopt (fd, IPPROTO_TCP, TCP_USER_TIMEOUT, &timeout, sizeof(timeout)) != 0)
    {
        LOGE("TCP Set keepalive timeout error");
        DEBUG("TCP Set keepalive timeout error");
        return -1;
    }*/

    LOGD("TCP Set keepalive OK");
    DEBUG("TCP Set keepalive OK");

    return 0;
}
@@ -142,13 +144,13 @@
    int error_value;
    socklen_t error_value_len;

    LOGI("%s", __func__);
    DEBUG("tcp_connect... %s : %d", ip, port);

    error_value_len = sizeof( error_value );

    if((soc = socket(PF_INET, SOCK_STREAM, 0)) == -1)
    {
        LOGE("%s: socket", __func__);
        DEBUG("%s: socket", __func__);
        return -1;
    }

@@ -160,13 +162,13 @@
    // Set non-blocking
    if( (arg = fcntl(soc, F_GETFL, NULL)) < 0)
    {
        LOGE("%s: fcntl( F_GETFL ) error", __func__);
        DEBUG("%s: fcntl( F_GETFL ) error", __func__);
        goto TCP_CONNECT_1;
    }

    if( fcntl(soc, F_SETFL, arg | O_NONBLOCK) < 0)
    {
        LOGE( "%s: fcntl( F_SETFL ) error", __func__);
        DEBUG( "%s: fcntl( F_SETFL ) error", __func__);
        goto TCP_CONNECT_1;
    }

@@ -198,12 +200,12 @@

    if(fds_ret < 0)
    {
        LOGE( "%s: TCP select error", __func__);
        DEBUG( "%s: TCP select error", __func__);
        goto TCP_CONNECT_1;
    }
    else if(fds_ret == 0)
    {
        LOGE("%s: TCP Connect Timeout %ld", __func__, tv.tv_sec);
        DEBUG("%s: TCP Connect Timeout %ld", __func__, tv.tv_sec);
        goto TCP_CONNECT_1;
    }
    else if(FD_ISSET(soc, &rdfds))
@@ -215,7 +217,7 @@
    }
    else
    {
        LOGE("%s: some error occur in tcp_connect()", __func__);
        DEBUG("%s: some error occur in tcp_connect()", __func__);
        goto TCP_CONNECT_1;
    }

@@ -244,11 +246,11 @@
    arg &= ~O_NONBLOCK;
    if( fcntl(soc, F_SETFL, arg) < 0)
    {
        LOGE( "%s: fcntl( F_SETFL ) error", __func__);
        DEBUG( "%s: fcntl( F_SETFL ) error", __func__);
        goto TCP_CONNECT_1;
    }

    LOGI("%s: tcp connected %s: %d", __func__, ip, port);
    DEBUG("%s: tcp connected %s: %d", __func__, ip, port);

    return( soc );

@@ -317,19 +319,19 @@
    fds_ret = select(fd + 1, NULL, &rdfds, NULL, &tv);

    if (fds_ret < 0) {
        LOGE("tcp error send select error");
        DEBUG("tcp error send select error");
        return -1;
    } else if(fds_ret == 0) {
        LOGE("tcp error Occur failure(such as line disconnect)");
        DEBUG("tcp error Occur failure(such as line disconnect)");
        //Occur failure(such as line disconnect)
        ret = -1;
    } else if(FD_ISSET(fd, &rdfds)) {
        ret = send(fd, buf, len, 0);
        if(ret == -1) {
            LOGE("tcp error TCP Send Error");
            DEBUG("tcp error TCP Send Error");
        }
    } else {
        LOGE("tcp error tcp send has error\n");
        DEBUG("tcp error tcp send has error\n");
    }

    return ret;
@@ -375,7 +377,7 @@

void DisconnectTCP(int fd)
{
    LOGI("DisconnectTCP fd = %d", fd);
    DEBUG("DisconnectTCP fd = %d", fd);
    if (fd >= 0) {
        shutdown(fd, SHUT_RDWR);
        close(fd);
@@ -426,12 +428,12 @@

    if (fds_ret < 0)
    {
        LOGE("UDP send select error");
        DEBUG("UDP send select error");
        return -1;
    }
    else if(fds_ret == 0)
    {
        LOGE("Occur failure(such as line disconnect)");
        DEBUG("Occur failure(such as line disconnect)");
        ret = -1;
    }
    else if(FD_ISSET(fd, &rdfds))
@@ -447,12 +449,12 @@

        if(ret == -1)
        {
            LOGE("UDP Send Error");
            DEBUG("UDP Send Error");
        }
    }
    else
    {
        LOGE("UDP send has error\n");
        DEBUG("UDP send has error\n");
    }

    return ret;
@@ -473,7 +475,7 @@
        if (errno == EINTR || errno == EWOULDBLOCK || errno == EAGAIN) {
            return 0;
        } else {
            LOGE("UDP ERROR!! = %d", errno);
            DEBUG("UDP ERROR!! = %d", errno);
            return -1;
        }
    }

 lib/src/main/cpp/driver_test.cpp

@@ -66,6 +66,7 @@
static LIST_AREA_MAP AreaMapList;

static Polygon RoadMapPoints;
static road_exam_map RoadMap;

static LIST_ROAD_MAP RoadMapList;

@@ -111,7 +112,10 @@
    RoadMapPoints.num = 0;
    RoadMapPoints.point = NULL;

    RoadMapList.clear();
//    RoadMapList.clear();
    RoadMap.roads.clear();
    RoadMap.specialAreas.clear();
    RoadMap.triggerLines.clear();

    CarSensorInit();

@@ -190,11 +194,58 @@
{
    if (ExamStart) return;

    DEBUG("清除旧的路考地图");

    if (RoadMapPoints.point != NULL) {
        free(RoadMapPoints.point);
    }
    RoadMapPoints.num = 0;

    for (int i = 0; i < RoadMap.roads.size(); ++i) {
        for (int j = 0; j < RoadMap.roads[i].leftEdge.size(); ++j) {
            RoadMap.roads[i].leftEdge[j].points.clear();
            vector<PointF>().swap(RoadMap.roads[i].leftEdge[j].points);
        }
        RoadMap.roads[i].leftEdge.clear();
        vector<edge_t>().swap(RoadMap.roads[i].leftEdge);

        for (int j = 0; j < RoadMap.roads[i].rightEdge.size(); ++j) {
            RoadMap.roads[i].rightEdge[j].points.clear();
            vector<PointF>().swap(RoadMap.roads[i].rightEdge[j].points);
        }
        RoadMap.roads[i].rightEdge.clear();
        vector<edge_t>().swap(RoadMap.roads[i].rightEdge);

        for (int j = 0; j < RoadMap.roads[i].separate.size(); ++j) {
            for (int k = 0; k < RoadMap.roads[i].separate[j].lines.size(); ++k) {
                for (int m = 0; m < RoadMap.roads[i].separate[j].lines[k].size(); ++m) {
                    RoadMap.roads[i].separate[j].lines[k][m].points.clear();
                    vector<PointF>().swap(RoadMap.roads[i].separate[j].lines[k][m].points);
                }

                RoadMap.roads[i].separate[j].lines[k].clear();
                vector<segment_t>().swap(RoadMap.roads[i].separate[j].lines[k]);
            }
            RoadMap.roads[i].separate[j].lines.clear();
            vector<vector<segment_t>>().swap(RoadMap.roads[i].separate[j].lines);
        }
        RoadMap.roads[i].separate.clear();
        vector<separate_t>().swap(RoadMap.roads[i].separate);
    }

    RoadMap.roads.clear();
    vector<road_t>().swap(RoadMap.roads);

    for (int i = 0; i < RoadMap.specialAreas.size(); ++i) {
        RoadMap.specialAreas[i].area.clear();
        vector<PointF>().swap(RoadMap.specialAreas[i].area);
    }
    RoadMap.specialAreas.clear();
    vector<special_area_t>().swap(RoadMap.specialAreas);

    RoadMap.triggerLines.clear();
    vector<trigger_line_t>().swap(RoadMap.triggerLines);
/*
    for (int i = 0; i < RoadMapList.size(); ++i) {
        struct road_exam_map map = RoadMapList[i];

@@ -244,7 +295,18 @@
        }
    }

    RoadMapList.clear();
    RoadMapList.clear();*/
}

void SetRoadMap(road_exam_map &map)
{
    if (ExamStart) return;

    RoadMap.roads.assign(map.roads.begin(), map.roads.end());
    RoadMap.specialAreas.assign(map.specialAreas.begin(), map.specialAreas.end());
    RoadMap.triggerLines.assign(map.triggerLines.begin(), map.triggerLines.end());

    DEBUG("得到新的路考地图 路数量 %d 特殊区域数量 %d 触发线数量 %d", RoadMap.roads.size(), RoadMap.specialAreas.size(), RoadMap.triggerLines.size());
}

void SetRoadMapPoints(vector<double> &mapPoints)
@@ -271,7 +333,7 @@
                      vector<int> area,
                      vector<int> stopLine)
{
    struct road_exam_map newMap;
/*    struct road_exam_map newMap;

    newMap.id = id;
    newMap.type = type;
@@ -400,7 +462,7 @@
              newMap.stopLine.Y2);
    }

    RoadMapList.push_back(newMap);
    RoadMapList.push_back(newMap);*/
}

void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
@@ -554,7 +616,7 @@
//        err = true;
//        MA_SendExamStatus(0, -3);
    }
    if (type != TEST_TYPE_AREA && (RoadMapPoints.num == 0 || RoadMapPoints.point == NULL || RoadMapList.size() == 0)) {
    if (type != TEST_TYPE_AREA && RoadMap.roads.size() == 0) {
        DEBUG("没有路考地图");
        err = true;
        MA_SendExamStatus(0, -1);
@@ -645,8 +707,8 @@
        brief.pointNum = CarModel->pointNum;
        brief.point = (double *) malloc(CarModel->pointNum * 2 * sizeof(double));
        for (int i = 0, j = 0; i < CarModel->pointNum; ++i) {
            brief.point[j++] = CarModel->carXY[i].X;
            brief.point[j++] = CarModel->carXY[i].Y;
            brief.point[j++] = round(CarModel->carXY[i].X, 4);
            brief.point[j++] = round(CarModel->carXY[i].Y, 4);
        }

        MA_SendCarPosition(&brief);
@@ -801,7 +863,7 @@
            // 汇报灯光考试结束
            if (exam_dummy_light == 2) {
                DEBUG("灯光考试结束");
                InitRoadExam();
                InitRoadExam(RoadMap);
            }
        }
    }
@@ -819,7 +881,8 @@

    if (ExamType != TEST_TYPE_AREA) {
        if (exam_dummy_light == 2) {
            TestRoadGeneral(RoadMapList, CarModel, CarModelList, speed, move, rtkTime);
//            TestRoadGeneral(RoadMapList, CarModel, CarModelList, speed, move, rtkTime);
            TestRoadGeneral(RoadMap, CarModel, CarModelList, speed, move, rtkTime);
        }
    } else {
        TestAreaGeneral(AreaMapList, CarModel, CarModelList, speed, move, azimuth, rtkTime);

 lib/src/main/cpp/driver_test.h

@@ -92,12 +92,14 @@
    int wrong_id;
};

struct trigger_line_t {

struct trigger_line_tx {
    int triggerMapId;
    Polygon line;
};

struct road_exam_map {

struct road_exam_mapx {
    int id;
    int type;

@@ -113,7 +115,7 @@
    Polygon *redArea;

    int triggerLineNum;
    trigger_line_t *triggerLine;
    trigger_line_tx *triggerLine;

    int roadEdgeLineNum;    // 道路边线，只有直线驾驶、靠边停车才有
    Polygon *roadEdgeLine;
@@ -123,10 +125,23 @@
    int flagStop;           // 到达开始线前，是否需要停车
};

typedef vector<struct road_exam_map> LIST_ROAD_MAP;
typedef vector<struct road_exam_mapx> LIST_ROAD_MAP;

#define ROAD_DOTTED_LINE            0
#define ROAD_SOLID_LINE             1
#define LINE_DOTTED            0
#define LINE_SOLID             1
#define LINE_HALF_SOLID_LEFT        2
#define LINE_HALF_SOLID_RIGHT       3

//车道方向（按位组合）,如果为0，则表无车道方向说明；
#define LANE_FORWARD    0x01
#define LANE_LEFT        0x02
#define LANE_RIGHT        0x04
#define LANE_BACKWARD    0x08

#define ZEBRA_CROSSING              0
#define SCHOOL_AREA                 1
#define BUS_STATION_AREA            2
#define GRID_AREA                   3

typedef struct {
    int character;                  // 属性（实线、虚线，有些可以掉头的路段）
@@ -134,42 +149,51 @@
} edge_t;

typedef struct {
    int character;          // 属性《实线、虚线》
    std::vector<Polygon> lines;      // 每一条线
    int character;          // 属性《实线、虚线、半实半虚线》
    int left_lane_direct;
    int right_lane_direct;
    std::vector<PointF> points;
} segment_t;

// 一组平行的分道线
typedef struct {
    std::vector<std::vector<segment_t>> lines;      // 每段域下的平行的一组线
} separate_t;

struct road_t {
typedef struct {
    int id;
    Line startLine;
    Line stopLine;
    int active;         // 到达路口尾部的行进方向
    int targetRoad;
    int stopFlag;
    string tts;
    Polygon area;
    Polygon area;       // 整个道路区域
    std::vector<edge_t> leftEdge;
    std::vector<edge_t> rightEdge;
    std::vector<separate_t> separate;
};
} road_t;

struct special_area_t {
typedef struct {
    int id;
    int road;
    int character;
    Line startLine;
    Polygon area;
};
    int type;
    std::vector<PointF> area;       // 人行道等右侧2点，网格线4点
    std::vector<PointF> leftPoints; // 对应到道路左侧的点
} special_area_t;

struct trigger2_line_t {
typedef struct {
    int id;
    int road;
    int active;
    string tts;
    Line line;
};
} trigger_line_t;

struct road_exam2_map {
    std::vector<struct road_t> roads;
    std::vector<struct special_area_t> specialAreas;
    std::vector<struct trigger2_line_t> triggerLines;
struct road_exam_map {
    std::vector<road_t> roads;
    std::vector<special_area_t> specialAreas;
    std::vector<trigger_line_t> triggerLines;
};

struct area_exam_map {
@@ -193,6 +217,8 @@


void CleanRoadMap(void);
void SetRoadMap(road_exam_map &map);

void SetRoadMapPoints(vector<double> &mapPoints);
void AddRoadMapParent(int id, int type, string tts, int stopFlag, vector<vector<int>> &redLines,
                      vector<vector<int>> &redAreas, vector<vector<int>> &greenLines,

 lib/src/main/cpp/master/comm_if.cpp

@@ -37,7 +37,8 @@
#define ID_SM_RTK_PLAT_LOGIN     0x0005
#define ID_SM_READ_MAP          0x0006
#define ID_MS_MAP               0x8006
#define ID_MS_ROAD_MAP          0x8013
#define ID_MS_ROAD_MAP          0x8014
#define ID_MS_ROAD_MAP2         0x8013

#define ID_SM_READ_CAR          0x0007
#define ID_MS_CAR               0x8007
@@ -745,6 +746,263 @@
            }
            break;
        }
        case ID_MS_ROAD_MAP2: {
            Document doc;
            doc.Parse(value);
            if (!doc.HasParseError()) {
                DEBUG("开始解析路考地图");

                vector<PointF> mapPoints;
                mapPoints.clear();

                if (doc.HasMember("points")) {
                    const Value &s = doc["points"];

                    DEBUG("得到所有点 共 %d", s.Size()/2);

                    int n = 0;
                    PointF temp;

                    // X-Y坐标集合
                    for (Value::ConstValueIterator itr = s.Begin();
                         itr != s.End(); ++itr, ++n) {
                        if ((n % 2) == 0) {
                            temp.X = (*itr).GetDouble();
                        } else {
                            temp.Y = (*itr).GetDouble();
                            mapPoints.push_back(temp);
                        }
                    }
                }

                road_exam_map map;

                map.roads.clear();
                map.specialAreas.clear();
                map.triggerLines.clear();

                if (doc.HasMember("road")) {
                    const Value &a = doc["road"];

                    if (a.IsArray()) {
                        for (Value::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) {
                            road_t road;

                            if (itr->HasMember("id")) {
                                const Value &s = (*itr)["id"];
                                DEBUG("路id %d", s.GetInt());
                                road.id = s.GetInt();
                            }
                            if (itr->HasMember("start_line")) {
                                const Value &a2 = (*itr)["start_line"];

                                PointF p1, p2;
                                int n = 0;

                                for (Value::ConstValueIterator itr2 = a2.Begin(); itr2 != a2.End(); ++itr2, ++n) {
                                    DEBUG("起始线 %d", (*itr2).GetInt());
                                    if (n == 0) {
                                        p1 = mapPoints[(*itr2).GetInt()];
                                    } else if (n == 1) {
                                        p2 = mapPoints[(*itr2).GetInt()];
                                    }
                                }

                                MakeLine(&road.startLine, &p1, &p2);
                            }
                            if (itr->HasMember("stop_line")) {
                                const Value &a2 = (*itr)["stop_line"];

                                PointF p1, p2;
                                int n = 0;

                                for (Value::ConstValueIterator itr2 = a2.Begin(); itr2 != a2.End(); ++itr2, ++n) {
                                    DEBUG("结束线 %d", (*itr2).GetInt());
                                    if (n == 0) {
                                        p1 = mapPoints[(*itr2).GetInt()];
                                    } else if (n == 1) {
                                        p2 = mapPoints[(*itr2).GetInt()];
                                    }
                                }

                                MakeLine(&road.stopLine, &p1, &p2);
                            }
                            if (itr->HasMember("active")) {
                                const Value &s = (*itr)["active"];
                                DEBUG("路口动作 %d", s.GetInt());

                                road.active = s.GetInt();
                            }
                            if (itr->HasMember("tts")) {
                                const Value &s = (*itr)["tts"];
                                road.tts = s.GetString();
                            }
                            if (itr->HasMember("stop_flag")) {
                                const Value &s = (*itr)["stop_flag"];
                                road.stopFlag = s.GetInt();
                            }
                            if (itr->HasMember("next_road")) {
                                const Value &s = (*itr)["next_road"];
                                road.targetRoad  = s.GetInt();
                            }
                            if (itr->HasMember("left_edge")) {
                                const Value &a2 = (*itr)["left_edge"];
                                DEBUG("左边线");

                                if (a2.IsArray()) {
                                    for (Value::ConstValueIterator itr2 = a2.Begin(); itr2 != a2.End(); ++itr2) {
                                        edge_t edge;

                                        if (itr2->HasMember("type")) {
                                            const Value &s = (*itr2)["type"];
                                            DEBUG("\t类型 %d", s.GetInt());

                                            edge.character = s.GetInt();
                                        }
                                        if (itr2->HasMember("line")) {
                                            const Value &a3 = (*itr2)["line"];
                                            if (a3.IsArray()) {
                                                for (Value::ConstValueIterator itr3 = a3.Begin(); itr3 != a3.End(); ++itr3) {
                                                    DEBUG("\t线点 %d", (*itr3).GetInt());
                                                    edge.points.push_back(mapPoints[(*itr3).GetInt()]);
                                                }
                                            }
                                        }

                                        road.leftEdge.push_back(edge);
                                    }
                                }
                            }
                            if (itr->HasMember("right_edge")) {
                                const Value &a2 = (*itr)["right_edge"];
                                DEBUG("右边线");
                                if (a2.IsArray()) {
                                    for (Value::ConstValueIterator itr2 = a2.Begin(); itr2 != a2.End(); ++itr2) {
                                        edge_t edge;

                                        if (itr2->HasMember("type")) {
                                            const Value &s = (*itr2)["type"];
                                            DEBUG("\t类型 %d", s.GetInt());
                                            edge.character = s.GetInt();
                                        }
                                        if (itr2->HasMember("line")) {
                                            const Value &a3 = (*itr2)["line"];
                                            if (a3.IsArray()) {
                                                for (Value::ConstValueIterator itr3 = a3.Begin(); itr3 != a3.End(); ++itr3) {
                                                    DEBUG("\t线点 %d", (*itr3).GetInt());
                                                    edge.points.push_back(mapPoints[(*itr3).GetInt()]);
                                                }
                                            }
                                        }

                                        road.rightEdge.push_back(edge);
                                    }
                                }
                            }
                            if (itr->HasMember("separate")) {
                                const Value &a2 = (*itr)["separate"];
                                DEBUG("段数量 %d", a2.Size());

                                for (Value::ConstValueIterator itr2 = a2.Begin(); itr2 != a2.End(); ++itr2) {
                                    DEBUG("\t线数量 %d", (*itr2).Size());
                                    separate_t sep;

                                    for (Value::ConstValueIterator itr3 = (*itr2).Begin(); itr3 != (*itr2).End(); ++itr3) {
                                        DEBUG("\t\t节数量 %d", (*itr3).Size());
                                        vector<segment_t> sline;

                                        for (Value::ConstValueIterator itr4 = (*itr3).Begin(); itr4 != (*itr3).End(); ++itr4) {
                                            const Value &type = (*itr4)["type"];
                                            const Value &line = (*itr4)["line"];

                                            segment_t seg;

                                            DEBUG("\t\t\t节类型 = %d", type.GetInt());
                                            seg.character = type.GetInt();

                                            if ((*itr4).HasMember("left_lane_direct")) {
                                                const Value &dir = (*itr4)["left_lane_direct"];
                                                DEBUG("\t\t\t左车道方向 %d", dir.GetInt());
                                                seg.left_lane_direct = dir.GetInt();
                                            } else {
                                                seg.left_lane_direct = 0;
                                            }

                                            if ((*itr4).HasMember("right_lane_direct")) {
                                                const Value &dir = (*itr4)["right_lane_direct"];
                                                DEBUG("\t\t\t右车道方向 %d", dir.GetInt());
                                                seg.right_lane_direct = dir.GetInt();
                                            } else {
                                                seg.right_lane_direct = 0;
                                            }

                                            for (Value::ConstValueIterator itr5 = line.Begin(); itr5 != line.End(); ++itr5) {
                                                DEBUG("\t\t\t点 = %d", (*itr5).GetInt());

                                                seg.points.push_back(mapPoints[(*itr5).GetInt()]);
                                            }

                                            sline.push_back(seg);
                                        }
                                        sep.lines.push_back(sline);
                                    }

                                    road.separate.push_back(sep);
                                }
                            }

                            map.roads.push_back(road);
                        }
                    }
                }

                if (doc.HasMember("special_area")) {
                    const Value &a = doc["special_area"];

                    vector<special_area_t> specialAreas;

                    for (Value::ConstValueIterator itr = a.Begin();
                         itr != a.End(); ++itr) {
                        special_area_t specialArea;

                        if (itr->HasMember("type")) {
                            const Value &s = (*itr)["type"];
                            specialArea.type = s.GetInt();
                        }
                        if (itr->HasMember("id")) {
                            const Value &s = (*itr)["id"];
                            specialArea.id = s.GetInt();
                        }
                        if (itr->HasMember("road")) {
                            const Value &s = (*itr)["road"];
                            specialArea.road = s.GetInt();
                        }

                        if (itr->HasMember("area")) {
                            const Value &a2 = (*itr)["area"];

                            for (Value::ConstValueIterator itr2 = a2.Begin();
                                 itr2 != a2.End(); ++itr2) {
                                specialArea.area.push_back(mapPoints[(*itr2).GetInt()]);
                            }
                        }

                        map.specialAreas.push_back(specialArea);
                    }
                }
                if (doc.HasMember("trigger_line")) {

                }
                DEBUG("地图解析完毕");

                CleanRoadMap();
                SetRoadMap(map);
            } else {
                DEBUG("############## 地图解析出错###################");
            }

            break;
        }
        case ID_MS_MAP: {
            Document doc;
            doc.Parse(value);

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

@@ -11,6 +11,9 @@
#include <malloc.h>
#include <initializer_list>
#include <cctype>
#include <iosfwd>
#include <iomanip>
#include <sstream>

#include "../jni_log.h"

@@ -37,6 +40,13 @@
inline double toDegree(double radians)
{
    return (radians * 180.0) / M_PI;
}

double round(double number, unsigned int bits) {
    stringstream ss;
    ss << setiosflags(ios::fixed) << setprecision(bits) << number;
    ss >> number;
    return number;
}

void MakeLine(Line *line, const PointF *p1, const PointF *p2)
@@ -442,6 +452,7 @@
    }
}

// 0 - straight, 1 - left, -1 - right, 2 - front, -2 - back
int IntersectionOfLine(PointF p, Line line)
{
    PointF p1, p2;
@@ -451,7 +462,7 @@
    p2.X = line.X2;
    p2.Y = line.Y2;

    IntersectionOfLine(p1, p2, p);
    return IntersectionOfLine(p1, p2, p);
}

/***************************************************************
@@ -514,6 +525,26 @@
    return false;
}

bool VerticalPointOnLine(PointF point, Line line, PointF &vp)
{
    PointF p1, p2;

    p1.X = line.X1;
    p1.Y = line.Y1;

    p2.X = line.X2;
    p2.Y = line.Y2;

    PointF pv = GetVerticalPoint(p1, p2, point);

    if (isEqual2(pv.X, MIN(p1.X, p2.X)) || isEqual2(pv.X, MAX(p1.X, p2.X)) ||
        (pv.X > MIN(p1.X, p2.X) && pv.X < MAX(p1.X, p2.X))) {
        vp = pv;
        return true;
    }
    return false;
}

/****************************************************************
 *                      p3
 *                      |  'L'

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

@@ -37,6 +37,7 @@
inline double toDegree(double radians);
inline bool isEqual(double a, double b);
inline bool isEqual2(double a, double b);
double round(double number, unsigned int bits);

void MakeLine(Line *line, const PointF *p1, const PointF *p2);
void MakePolygon(Polygon *polygon, std::initializer_list<PointF> point_set);
@@ -60,6 +61,7 @@
int IntersectionOfLine(PointF p, Line line);
PointF GetVerticalPoint(PointF p1, PointF p2, PointF p3);
bool VerticalPointOnLine(PointF point, Line line);
bool VerticalPointOnLine(PointF point, Line line, PointF &vp);
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

@@ -18,6 +18,7 @@

#include <vector>
#include <list>
#include <map>
#include <string>
#include <cstdlib>

@@ -39,6 +40,9 @@
static const int TURN_CHECK_INTERVAL = 500;
const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;

static const int FIND_POSITION = -2;
static const int INVALID_POSITION = -1;

static bool occurCrashRedLine;
static bool occurCrashGreenLine;
@@ -76,9 +80,16 @@
static bool handBreakActive = false;
static bool reportRPMOver = false;

static int currCarOnRoadIndex;

static const uint32_t TURN_ERROR_COLD_TIME = D_SEC(10);

static bool turnError13Cold, turnError14Cold;
static struct car_on_lane {
    int road;
    int separate;
    int lane;
} CarOnLane;

static const int MAX_ENGINE_RPM = 2500;
static const double START_CAR_MOVE_DISTANCE = 10.0;
@@ -90,10 +101,16 @@
static const uint32_t CRASH_DOTTED_LINE_TIMEOUT = D_SEC(10);
static const uint32_t TURN_SIGNAL_LAMP_ADVANCE = D_SEC(3);

static const int CRL_NONE = 0;
static const int CRL_SEP_DOTTED = 1;
static const int CRL_SEP_SOLID = 2;
static const int CRL_EDGE_DOTTED = 3;
static const int CRL_EDGE_SOLID = 4;

static const double MAX_SPEED = 40.0 * 1000.0 / 3600.0;
static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};

static void TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime);
static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime);

static char isTurn(int currYaw, int prevYaw, int &ang);
static char CheckCarTurn(LIST_CAR_MODEL &CarModelList);
@@ -106,8 +123,12 @@
static void TurnSignalError13ColdTimer(union sigval sig);
static void TurnSignalError14ColdTimer(union sigval sig);
static void ReportTurnSignalError(int err, const struct RtkTime *rtkTime);
static bool UpdateLane(struct car_on_lane &out, road_t &road, const car_model *car);
static int CrashRoadLine(road_t &road, const car_model *car);
static bool LaneIsSame(struct car_on_lane lane1, struct car_on_lane lane2);
static bool LaneIsValid(struct car_on_lane lane);

void InitRoadExam(void)
void InitRoadExam(road_exam_map &RoadMap)
{
    DEBUG("Start road_exam");

@@ -132,7 +153,7 @@
    prevGearNSlide = false;
    gearNSlideTime = 0;

    currExamMapIndex = -1;
    currExamMapIndex = FIND_POSITION;

    startCar = START_CAR_NOT_DO;

@@ -143,6 +164,12 @@
    checkStartCarSignal = startCarLeftTurnSignal = false;

    turnError13Cold = turnError14Cold = true;

    currCarOnRoadIndex = FIND_POSITION;

    CarOnLane.road = CarOnLane.separate = CarOnLane.lane = -1;

    InitThroughSomething(RoadMap);
}

void TerminateRoadExam(void)
@@ -188,7 +215,7 @@
    }
}

static void TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    double moveDistance;

@@ -256,6 +283,558 @@
    } else {

    }

    return startCar;
}

void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    // 起步检测
    TestRoadStartCar(car, speed, moveDirect, rtkTime);

    // 超速检测
    if (moveDirect != 0 && speed > MAX_SPEED) {
        if (!occurOverSpeed) {
            occurOverSpeed = true;
            // 超速，不合格
            AddExamFault(10, rtkTime);
        }
    } else {
        occurOverSpeed = false;
    }

    // 副刹车检测
    if (ReadCarStatus(SECOND_BREAK) == BREAK_ACTIVE) {
        // 副刹车踩下，不合格
        if (!occurSecondBreak) {
            DEBUG("副刹车动作了");
            occurSecondBreak = true;
            AddExamFault(17, rtkTime);
        }
    } else {
        occurSecondBreak = false;
    }

    // 挡位匹配检测
    bool currGearError = false;
    bool currGearNSlide = false;

    switch (ReadCarStatus(GEAR)) {
        case GEAR_N:
            if (moveDirect != 0) {
                // 空档滑行
                currGearNSlide = true;
            }
            break;
        case GEAR_1:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[0][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[0][1]) {
                currGearError = true;
            }
            break;
        case GEAR_2:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[1][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[1][1]) {
                currGearError = true;
            }
            break;
        case GEAR_3:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[2][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[2][1]) {
                currGearError = true;
            }
            break;
        case GEAR_4:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[3][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[3][1]) {
                currGearError = true;
            }
            break;
        case GEAR_5:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[4][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[4][1]) {
                currGearError = true;
            }
            break;
        default:break;
    }
    // 空档滑行超时
    if (currGearNSlide && prevGearNSlide) {
        gearNSlideTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                                      gearNSlideTimePoint.hour, gearNSlideTimePoint.min, gearNSlideTimePoint.sec, gearNSlideTimePoint.msec*10);
    }
    if (gearNSlideTime > GEAR_N_SLIDE_TIMEOUT) {
        // 空档滑行超5秒，不合格
        DEBUG("挡位滑行，超过5秒");
        AddExamFault(8, rtkTime);
        gearNSlideTime = 0;
    }

    prevGearNSlide = currGearNSlide;
    if (prevGearNSlide) {
        Rtk2DriveTimer(gearNSlideTimePoint, rtkTime);
    } else {
        gearNSlideTime = 0;
    }
    // 挡位不匹配超时
    if (currGearError && prevGearError) {
        gearErrorTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                                     gearErrorTimePoint.hour, gearErrorTimePoint.min, gearErrorTimePoint.sec, gearErrorTimePoint.msec*10);
    }
    if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
        // 累计15秒，挡位-车速不匹配，不合格
        DEBUG("挡位错误超过15秒");
        AddExamFault(6, rtkTime);
        gearErrorTime = 0;
    }

    prevGearError = currGearError;
    if (prevGearError) {
        Rtk2DriveTimer(gearErrorTimePoint, rtkTime);
    }

    // 起步后滑
    if (moveDirect != prevMoveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
            reportStopCarOnRedArea = false;

            DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
        } else if (moveDirect == -1 && prevMoveDirect == 0) {
            DEBUG("开始后滑");
            stopPoint = car->basePoint;
            occurSlide = true;
        }
        prevMoveDirect = moveDirect;
    } else if (moveDirect == 0) {
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

        /*if (tp - stopTimepoint >= STOP_CAR_TIME && !reportStopCarOnRedArea && CrashRedArea(RoadMapList, car)) {
            // 停车超2秒，停在红区，不合格
            AddExamFault(16, rtkTime);
            DEBUG("禁停区停车");
            reportStopCarOnRedArea = true;
        }*/
    } else if (moveDirect == -1) {
        if (occurSlide) {
            double slideDistance = DistanceOf(stopPoint, car->basePoint);

            if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
                slideNormalDistance = true;
            }

            if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED && !slideLongDistance) {
                // 后滑超过30厘米, 不合格
                AddExamFault(5, rtkTime);
                DEBUG("后滑超过30厘米");
                slideLongDistance = true;
            }
        }
    } else {
        if (slideNormalDistance) {
            // 后滑，扣10分
            AddExamFault(18, rtkTime);
            DEBUG("后滑超过10厘米, 但不超过30厘米");
        }

        slideNormalDistance = false;
        slideLongDistance = false;
        occurSlide = false;
    }

    // 检测通过路口、人行道等区域时，释放刹车或减速
    CheckBreakActive(RoadMap, car, CarModelList);

    // 检测离开此路段，全车需不在范围内
    if (currExamMapIndex >= 0) {
        Polygon area;
        int n = 0;

        area.num = 0;

        for (int j = 0; j < RoadMap.roads[currExamMapIndex].leftEdge.size(); ++j) {
            if (j > 0) {
                area.num += RoadMap.roads[currExamMapIndex].leftEdge[j].points.size() - 1;
            } else {
                area.num += RoadMap.roads[currExamMapIndex].leftEdge[j].points.size();
            }
        }
        for (int j = 0; j < RoadMap.roads[currExamMapIndex].rightEdge.size(); ++j) {
            if (j > 0) {
                area.num += RoadMap.roads[currExamMapIndex].rightEdge[j].points.size() - 1;
            } else {
                area.num += RoadMap.roads[currExamMapIndex].rightEdge[j].points.size();
            }
        }

        area.point = (PointF *) malloc(area.num * sizeof(PointF));

        for (int j = 0; j < RoadMap.roads[currExamMapIndex].leftEdge.size(); ++j) {
            for (int k = (j>0?1:0); k < RoadMap.roads[currExamMapIndex].leftEdge[j].points.size(); ++k) {
                area.point[n++] = RoadMap.roads[currExamMapIndex].leftEdge[j].points[k];
            }
        }

        for (int j = RoadMap.roads[currExamMapIndex].rightEdge.size() - 1; j >= 0; --j) {
            if (j == RoadMap.roads[currExamMapIndex].rightEdge.size() - 1) {
                for (int k = RoadMap.roads[currExamMapIndex].rightEdge[j].points.size() - 1; k >= 0; --k) {
                    area.point[n++] = RoadMap.roads[currExamMapIndex].rightEdge[j].points[k];
                }
            } else {
                for (int k = RoadMap.roads[currExamMapIndex].rightEdge[j].points.size() - 2; k >= 0; --k) {
                    area.point[n++] = RoadMap.roads[currExamMapIndex].rightEdge[j].points[k];
                }
            }
        }

        // 全车都需不在地图中
        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, &area) == GM_None) {
            DEBUG("离开路段 id = %d", RoadMap.roads[currExamMapIndex].id);
            currExamMapIndex = FIND_POSITION;
        }

        free(carBody.point);
        free(area.point);
    }
    if (currExamMapIndex == FIND_POSITION) {
        for (int i = 0; i < RoadMap.roads.size(); ++i) {
            Polygon area;
            int n = 0;
            
            area.num = 0;

            for (int j = 0; j < RoadMap.roads[i].leftEdge.size(); ++j) {
                if (j > 0) {
                    area.num += RoadMap.roads[i].leftEdge[j].points.size() - 1;
                } else {
                    area.num += RoadMap.roads[i].leftEdge[j].points.size();
                }
            }
            for (int j = 0; j < RoadMap.roads[i].rightEdge.size(); ++j) {
                if (j > 0) {
                    area.num += RoadMap.roads[i].rightEdge[j].points.size() - 1;
                } else {
                    area.num += RoadMap.roads[i].rightEdge[j].points.size();
                }
            }

            area.point = (PointF *) malloc(area.num * sizeof(PointF));

            for (int j = 0; j < RoadMap.roads[i].leftEdge.size(); ++j) {
                for (int k = (j>0?1:0); k < RoadMap.roads[i].leftEdge[j].points.size(); ++k) {
                    area.point[n++] = RoadMap.roads[i].leftEdge[j].points[k];
                }
            }

            for (int j = RoadMap.roads[i].rightEdge.size() - 1; j >= 0; --j) {
                if (j == RoadMap.roads[i].rightEdge.size() - 1) {
                    for (int k = RoadMap.roads[i].rightEdge[j].points.size() - 1; k >= 0; --k) {
                        area.point[n++] = RoadMap.roads[i].rightEdge[j].points[k];
                    }
                } else {
                    for (int k = RoadMap.roads[i].rightEdge[j].points.size() - 2; k >= 0; --k) {
                        area.point[n++] = RoadMap.roads[i].rightEdge[j].points[k];
                    }
                }
            }

            if (IntersectionOf(car->carXY[car->axial[AXIAL_FRONT]], &area) == GM_Containment) {
                currExamMapIndex = i;
                DEBUG("进入道路 id = %d", RoadMap.roads[i].id);
                break;
            }

            free(area.point);
        }
        if (currExamMapIndex < 0) {
            currExamMapIndex = INVALID_POSITION;
            DEBUG("搜寻未果");
        }
    } else if (currExamMapIndex == INVALID_POSITION) {
        for (int i = 0; i < RoadMap.roads.size(); ++i) {
            if (CrashTheLine(RoadMap.roads[i].startLine, car, CarModelList)) {
                currExamMapIndex = i;
                DEBUG("进入道路 id = %d", RoadMap.roads[i].id);
                break;
            }
        }
    }

    if (currExamMapIndex >= 0) {
        int crl = CrashRoadLine(RoadMap.roads[currExamMapIndex], car);

        if (crl == CRL_NONE) {
            DEBUG("什么都没压");
        } else if (crl == CRL_SEP_DOTTED) {
            DEBUG("压分道虚线");
        } else if (crl == CRL_SEP_SOLID) {
            DEBUG("压分道实线");
        } else if (crl == CRL_EDGE_DOTTED) {
            DEBUG("压边沿虚线");
        } else if (crl == CRL_EDGE_SOLID) {
            DEBUG("压边沿实线");
        }

        if (crl != CRL_SEP_DOTTED || crl != CRL_SEP_SOLID) {
            struct car_on_lane lane;
            UpdateLane(lane, RoadMap.roads[currExamMapIndex], car);
            if (!LaneIsSame(lane, CarOnLane)) {
                if (LaneIsValid(CarOnLane)) {
                    // 车道变换
                    DEBUG("变更车道");
                }
                CarOnLane = lane;
            }
        }
    }
}

/*************************************************************
 * 检测当前车道，以车头中点为基准
 * @param road
 * @param car
 */
static bool UpdateLane(struct car_on_lane &out, road_t &road, const car_model *car)
{
    bool leftExt = false, rightExt = false;
    Line leftExtLine, rightExtLine;
    struct car_on_lane lane;

    for (int i = 0; i < road.leftEdge.size(); ++i) {
        PointF p1, p2;
        Line edge;

        p1 = road.leftEdge[i].points[0];
        for (int j = 1; j < road.leftEdge[i].points.size(); ++j) {
            p2 = road.leftEdge[i].points[j];
            MakeLine(&edge, &p1, &p2);

            PointF vp;
            if (VerticalPointOnLine(car->carXY[car->axial[AXIAL_FRONT]], edge, vp)) {
                leftExt = true;
                MakeLine(&leftExtLine, &car->carXY[car->axial[AXIAL_FRONT]], &vp);
                goto LEFT_EXT_CMP;
            }
            p1 = p2;
        }
    }
LEFT_EXT_CMP:
    for (int i = 0; i < road.rightEdge.size(); ++i) {
        PointF p1, p2;
        Line edge;

        p1 = road.rightEdge[i].points[0];
        for (int j = 1; j < road.rightEdge[i].points.size(); ++j) {
            p2 = road.rightEdge[i].points[j];
            MakeLine(&edge, &p1, &p2);

            PointF vp;
            if (VerticalPointOnLine(car->carXY[car->axial[AXIAL_FRONT]], edge, vp)) {
                rightExt = true;
                MakeLine(&rightExtLine, &car->carXY[car->axial[AXIAL_FRONT]], &vp);
                goto RIGHT_EXT_CMP;
            }
            p1 = p2;
        }
    }
RIGHT_EXT_CMP:

    if (!leftExt || !rightExt) {
        return false;
    }

    bool orthogonalInSegment = false;

    for (int i = 0; i < road.separate.size(); ++i) {        // 段
        PointF p1, p2;
        Line sep;

        map<int, int> orthogonal;

        // 一段分道组内，有一条正交，就必须保证同组的全都正交，否则直接退出
        for (int j = 0; j < road.separate[i].lines.size(); ++j) {   // 线组
            bool intersection = false;

            for (int k = 0; !intersection && k < road.separate[i].lines[j].size(); ++k) {   // 节

                p1 = road.separate[i].lines[j][k].points[0];
                for (int m = 1; m < road.separate[i].lines[j][k].points.size(); ++m) {
                    p2 = road.separate[i].lines[j][k].points[m];
                    MakeLine(&sep, &p1, &p2);

                    if (IntersectionOf(leftExtLine, sep) == GM_Intersection) {
                        orthogonal.insert(pair<int, int>(j, 1));
                        orthogonalInSegment = true;
                        intersection = true;
                        DEBUG("分道线 %d 左正交", j);
                        break;
                    } else if (IntersectionOf(rightExtLine, sep) == GM_Intersection) {
                        orthogonal.insert(pair<int, int>(j, 2));
                        orthogonalInSegment = true;
                        intersection = true;
                        DEBUG("分道线 %d 右正交", j);
                        break;
                    }
                    p1 = p2;
                }
            }
        }
        DEBUG("目标 %d 当前 %d", road.separate[i].lines.size(), orthogonal.size());

        if (orthogonal.size() > 0) {
            if (orthogonal.size() == road.separate[i].lines.size()) {
                // 得到当前在第几个车道
                int x = 0;
                for (x = 0; x < orthogonal.size(); ++x) {
                    auto itx = orthogonal.find(x);
                    if (itx != orthogonal.end()) {
                        if (itx->second != 1) {
                            lane.road = road.id;
                            lane.separate = i;
                            lane.lane = itx->first;

                            DEBUG("路 %d 段 %d 车道 %d", lane.road, lane.separate, lane.lane);
                            break;
                        }
                    }
                }
                if (x >= orthogonal.size()) {
                    lane.road = road.id;
                    lane.separate = i;
                    lane.lane = orthogonal.size();

                    DEBUG("路 %d 段 %d 车道 %d", lane.road, lane.separate, lane.lane);
                }
                out = lane;
                return true;
            } else {
                // 不完全正交，直接退出
            }
            return false;
        }
    }
    return false;
}

/*************************************************************************
 * 碰触车道线型
 * @param road
 * @param car
 */
static int CrashRoadLine(road_t &road, const car_model *car)
{
    Line frontAxle, rearAxle;

    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 < road.leftEdge.size(); ++i) {
        PointF p1, p2;
        Line edge;

        p1 = road.leftEdge[i].points[0];
        for (int j = 1; j < road.leftEdge[i].points.size(); ++j) {
            p2 = road.leftEdge[i].points[j];
            MakeLine(&edge, &p1, &p2);

            if (IntersectionOf(edge, frontAxle) == GM_Intersection ||
                IntersectionOf(edge, rearAxle) == GM_Intersection) {
                // 压道路左边线
                if (road.leftEdge[i].character != LINE_SOLID) {
                    return CRL_EDGE_DOTTED;
                }
                return CRL_EDGE_SOLID;
            }
            p1 = p2;
        }
    }

    for (int i = 0; i < road.rightEdge.size(); ++i) {
        PointF p1, p2;
        Line edge;

        p1 = road.rightEdge[i].points[0];
        for (int j = 1; j < road.rightEdge[i].points.size(); ++j) {
            p2 = road.rightEdge[i].points[j];
            MakeLine(&edge, &p1, &p2);

            if (IntersectionOf(edge, frontAxle) == GM_Intersection ||
                IntersectionOf(edge, rearAxle) == GM_Intersection) {
                // 压道路右边线
                if (road.rightEdge[i].character != LINE_SOLID) {
                    return CRL_EDGE_DOTTED;
                }
                return CRL_EDGE_SOLID;
            }
            p1 = p2;
        }
    }

    // 检查分道线
    for (int i = 0; i < road.separate.size(); ++i) {
        // 段
        PointF p1, p2;
        Line sep;

        for (int j = 0; j < road.separate[i].lines.size(); ++j) {
            // 线
            for (int k = 0; k < road.separate[i].lines[j].size(); ++k) {
                // 节
                p1 = road.separate[i].lines[j][k].points[0];

                for (int m = 1; m < road.separate[i].lines[j][k].points.size(); ++m) {
                    p2 = road.separate[i].lines[j][k].points[m];
                    MakeLine(&sep, &p1, &p2);

                    if (IntersectionOf(sep, frontAxle) == GM_Intersection ||
                        IntersectionOf(sep, rearAxle) == GM_Intersection) {
                        // 检查道路分隔线类型
                        if (road.separate[i].lines[j][k].character == LINE_DOTTED) {
                            // 压虚线
                            return CRL_SEP_DOTTED;
                        } else if (road.separate[i].lines[j][k].character == LINE_SOLID) {
                            // 压实线
                            return CRL_SEP_SOLID;
                        } else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_LEFT) {
                            if (LaneIsValid(CarOnLane) && CarOnLane.lane <= j) {
                                return CRL_SEP_SOLID;
                            }
                            return CRL_SEP_DOTTED;
                        } else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_RIGHT) {
                            if (LaneIsValid(CarOnLane) && CarOnLane.lane > j) {
                                return CRL_SEP_SOLID;
                            }
                            return CRL_SEP_DOTTED;
                        }
                    }

                    p1 = p2;
                }
            }
        }
    }

    return CRL_NONE;
}

static bool LaneIsSame(struct car_on_lane lane1, struct car_on_lane lane2)
{
    if (lane1.road == lane2.road && lane1.separate == lane2.separate && lane1.lane == lane2.lane) {
        return true;
    }
    return false;
}

static bool LaneIsValid(struct car_on_lane lane)
{
    if (lane.road >= 0 && lane.separate >= 0 && lane.lane >= 0) {
        return true;
    }
    return false;
}

void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
@@ -974,6 +1553,28 @@
    return map_idx;
}

bool CrashTheLine(Line line, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    if (CarModelList.size() < 2)
        return false;

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

    Line trace;
    PointF p1, p2;

    p1 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]];
    ++iter;
    p2 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]];
    MakeLine(&trace, &p1, &p2);

    if (IntersectionOf(trace, line) == GM_Intersection &&
            IntersectionOfLine(p1, line) == -1) {
        return true;
    }
    return false;
}

static int FindMapIndexById(int id, LIST_ROAD_MAP &RoadMapList)
{
    for (int i = 0; i < RoadMapList.size(); ++i) {
@@ -984,12 +1585,63 @@
    return -1;
}

#if 1
/*********************************************************************
 * 计算某点到道路左边线的最近垂点
 * @param edge
 * @param road
 * @param point
 * @return
 */
PointF GetSELine(vector<edge_t> &edge, PointF point)
{
    PointF p1, p2;
    PointF px;

    vector<PointF> vps;

    Line line;

    for (int i = 0; i < edge.size(); ++i) {
        p1 = edge[i].points[0];
        for (int j = 1; j < edge[i].points.size(); ++j) {
            p2 = edge[i].points[j];
            MakeLine(&line, &p1, &p2);

            PointF vp;
            if (VerticalPointOnLine(point, line, vp)) {
                vps.push_back(vp);
            }

            p1 = p2;
        }
    }

    if (vps.size() == 0) {
        if (DistanceOf(point, edge[0].points[0]) < DistanceOf(point, edge[edge.size() - 1].points[edge[edge.size() - 1].points.size() - 1])) {
            px = GetVerticalPoint(edge[0].points[0], edge[0].points[1], point);
        } else {
            px = GetVerticalPoint(edge[edge.size() - 1].points[edge[edge.size() - 1].points.size() - 2], edge[edge.size() - 1].points[edge[edge.size() - 1].points.size() - 1], point);
        }
    } else if (vps.size() == 1) {
        px = vps[0];
    } else {
        px = vps[0];
        for (int i = 1; i < vps.size(); ++i) {
            if (DistanceOf(point, vps[i]) < DistanceOf(point, px)) {
                px = vps[i];
            }
        }
    }

    return px;
}

#if 0

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

static CarOnTrackInfo_t CarOnTrackInfo;
@@ -1001,7 +1653,7 @@

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

    }

@@ -1145,7 +1797,7 @@
static void DetectSeparate(int currIndex, struct road_exam2_map &map, const car_model *car)
{
    int segment;
    int track = -1;
    int lane = -1;

    CarOnTrackInfo_t newInfo;

@@ -1190,7 +1842,7 @@

            vrecord.push_back(v);

            track = separate_line_num;//
            lane = separate_line_num;//
        }


@@ -1215,14 +1867,14 @@
                if (rel != -1) {
                    newInfo.road = currIndex;
                    newInfo.segment = i;
                    newInfo.track = x;
                    newInfo.lane = x;
                    break;
                }
            }

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

            break;
        }

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

@@ -27,10 +27,14 @@
};

void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime);
void InitRoadExam(void);
void InitRoadExam(road_exam_map &RoadMap);
void TerminateRoadExam(void);
void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);
void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);
bool ExitSonArea(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car);
bool CrashSonRedLine(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList);

bool CrashTheLine(Line line, const car_model *car, LIST_CAR_MODEL &CarModelList);
PointF GetSELine(std::vector<edge_t> &edge, PointF point);

#endif //MYAPPLICATION2_ROAD_EXAM_H

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

@@ -2,6 +2,9 @@
// Created by YY on 2020/3/20.
//

#include <map>
#include <algorithm>

#include "through_something.h"
#include "../driver_test.h"
#include "../test_common/Geometry.h"
@@ -12,12 +15,17 @@

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

using namespace std;

static const double LASTEST_BREAK_POINT = 30.0;
static const double DISTANCE_STOP_CAR_TO_STOP_LINE = 5.0;
static const double PASS_SCHOOL_MAX_SPEED = 30.0;           // kmh

static int breakActive;
static int stopActive;
static bool crashRedLine;

static map<int, bool> breakRecord;              // 记录车辆驶过各个停止线前刹车动作

void StartThroughExam(int index, LIST_ROAD_MAP &RoadMapList)
{
@@ -33,6 +41,20 @@
    breakActive = 0;
    stopActive = 0;
    crashRedLine = false;
}

void InitThroughSomething(road_exam_map &RoadMap)
{
    breakRecord.clear();
    // 为每一个刹车检测点
    for (int i = 0; i < RoadMap.specialAreas.size(); ++i) {
        if (RoadMap.specialAreas[i].type == ZEBRA_CROSSING || RoadMap.specialAreas[i].type == BUS_STATION_AREA) {
            breakRecord.insert(pair<int, bool>(RoadMap.specialAreas[i].id, false));
        }
    }
    for (int i = 0; i < RoadMap.roads.size(); ++i) {
        breakRecord.insert(pair<int, bool>(RoadMap.roads[i].id, false));
    }
}

int ExecuteThroughExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
@@ -133,36 +155,57 @@
    return index;
}

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

    for (int i = 0; i < map.specialAreas.size(); i++) {
        if (map.specialAreas[i].type == ZEBRA_CROSSING || map.specialAreas[i].type == BUS_STATION_AREA) {
            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;
            // 计算点到左侧路边线的垂点
            int road = 0;
            for (road = 0; road < map.roads.size(); ++road) {
                if (map.roads[road].id == map.specialAreas[i].road)
                    break;
            }

            PointF vPoint = GetSELine(map.roads[road].leftEdge, map.specialAreas[i].area[0]);
            Line startLine;

            DEBUG("计算垂点 (%f, %f)", vPoint.X, vPoint.Y);

            MakeLine(&startLine, &map.specialAreas[i].area[0], &vPoint);


            if (distance < LASTEST_BREAK_POINT) {
                if (ReadCarStatus(BREAK) == BREAK_ACTIVE) {
                    breakActive = 1;
            // 车头和斑马线距离不足30米
            if (IntersectionOf(extLine, startLine) == GM_Intersection &&
                    IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], startLine) == 1 ) {
                DEBUG("进入减速区");
                if (BreakDone == BREAK_ACTIVE) {
                    auto itx = breakRecord.find(map.specialAreas[i].id);
                    if (itx != breakRecord.end()) {
                        itx->second = true;
                    }
                }
            }

            // 跨线后，检查刹车动作
            if (CrashTheLine(startLine, car, CarModelList)) {
                auto itx = breakRecord.find(map.specialAreas[i].id);
                if (itx != breakRecord.end()) {
                    if (itx->second == false) {
                        // 不按规定减速，不合格
                        DEBUG("不按规定减速");
                    }
                    itx->second = false;
                }
            }
        } else if (map.specialAreas[i].type == SCHOOL_AREA) {

        }
    }
}


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

@@ -8,6 +8,8 @@
#include "../driver_test.h"

void StartThroughExam(int index, LIST_ROAD_MAP &RoadMapList);
void InitThroughSomething(road_exam_map &RoadMap);
void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList);
int ExecuteThroughExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
                       LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);


 lib/src/main/java/com/anyun/exam/lib/RemoteService.java

@@ -93,7 +93,7 @@

        new Thread(new TestThread()).start();

        PlayRing(this);
//        PlayRing(this);
    }

    public void PlayRing(Context context) {