坐标

yy1717

2020-08-11 03ca970f73af542456326b2fa277090ce0ea532f

坐标

 lib/src/main/cpp/CMakeLists.txt

@@ -29,6 +29,7 @@
        mcu/mcu_if.cpp

        test_common/car_sensor.cpp
        test_common/odo_graph.cpp
        test_items/error_list.cpp
        test_items/park_edge.cpp
        test_items/park_bottom.cpp

 lib/src/main/cpp/driver_test.h

@@ -128,8 +128,6 @@

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


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

@@ -808,23 +808,6 @@

                                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("crossing") && itr->IsArray()) {
                                const Value &a2 = (*itr)["crossing"];

@@ -838,14 +821,17 @@
                                    if (itr2->HasMember("active")) {
                                        const Value &s = (*itr2)["active"];
                                        temp.active = s.GetInt();
                                        DEBUG("路口动作 %d", temp.active);
                                    }
                                    if (itr2->HasMember("stop_flag")) {
                                        const Value &s = (*itr2)["stop_flag"];
                                        temp.stopFlag = s.GetInt();
                                        DEBUG("路口停车 %d", temp.stopFlag);
                                    }
                                    if (itr2->HasMember("tts")) {
                                        const Value &s = (*itr2)["tts"];
                                        temp.tts = s.GetString();
                                        DEBUG("路口提示 %s", temp.tts.c_str());
                                    }
                                    if (itr2->HasMember("line")) {
                                        const Value &s = (*itr2)["line"];
@@ -870,20 +856,6 @@
                                road.stopLine.assign(crossing.begin(), crossing.end());
                            }

                            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();
@@ -947,49 +919,64 @@
                                DEBUG("段数量 %d", a2.Size());

                                for (Value::ConstValueIterator itr2 = a2.Begin(); itr2 != a2.End(); ++itr2) {
                                    DEBUG("\t线数量 %d", (*itr2).Size());
                                    separate_t sep;
                                    if (!itr2->IsObject())
                                        break;
                                    if (itr2->HasMember("lane_guide") && itr2->IsArray()) {
                                        const Value &a3 = (*itr2)["lane_guide"];

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

                                        for (Value::ConstValueIterator itr4 = (*itr3).Begin(); itr4 != (*itr3).End(); ++itr4) {
                                            const Value &type = (*itr4)["type"];
                                            const Value &line = (*itr4)["line"];
                                            if (itr3->HasMember("head_tail")) {
                                                const Value &a4 = (*itr3)["head_tail"];
                                                int n = 0;

                                            segment_t seg;
                                                for (Value::ConstValueIterator itr4 = a4.Begin(); itr4 != a4.End(); ++itr4, ++n) {
                                                    if (n == 0)
                                                        temp.start = mapPoints[(*itr4).GetInt()];
                                                    else if (n == 1)
                                                        temp.end = mapPoints[(*itr4).GetInt()];
                                                }
                                            }
                                            if (itr3->HasMember("guide")) {
                                                const Value &a4 = (*itr3)["guide"];

                                            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;
                                                for (Value::ConstValueIterator itr4 = a4.Begin(); itr4 != a4.End(); ++itr4) {
                                                    temp.direct.push_back((*itr4).GetInt());
                                                }
                                            }

                                            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.lane_direct.push_back(temp);
                                        }
                                        sep.lines.push_back(sline);
                                    }
                                    if (itr2->HasMember("lane_line") && itr2->IsArray() ) {
                                        const Value &a3 = (*itr2)["lane_line"];
                                        DEBUG("\t线数量 %d", a3.Size());
                                        for (Value::ConstValueIterator itr3 = a3.Begin(); itr3 != a3.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();

                                                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);
                                }
                            }

 lib/src/main/cpp/test_common/odo_graph.cpp

New file
@@ -0,0 +1,46 @@
//
// Created by YY on 2020/8/11.
//

#include "odo_graph.h"
#include "../test_items2/road_exam.h"
#include "../common/apptimer.h"
#include "../utils/xconvert.h"

static double odoGraph;
static struct drive_timer odoTimer;
static double odoPrevSpeed;
static int odoCnt;

void ResetOdo(void)
{
    odoCnt = 0;
    odoGraph = 0;
}

double ReadOdo(void)
{
    return odoGraph;
}

void UpdataOdo(double speed, int moveDirect, const struct RtkTime *rtkTime) {
    // 行驶距离，不包含倒车
    if (odoCnt == 0 && moveDirect == 1) {
        odoPrevSpeed = speed;
        odoCnt = 1;
        Rtk2DriveTimer(odoTimer, rtkTime);
    } else if (odoCnt == 1) {
        if (moveDirect == 1) {
            uint32_t tm = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss * 10,
                                      odoTimer.hour, odoTimer.min, odoTimer.sec,
                                      odoTimer.msec * 10);
            if (tm >= D_SEC(1)) {
                odoGraph += ((double) tm) * (odoPrevSpeed + speed) / 2.0 / 1000.0;
                Rtk2DriveTimer(odoTimer, rtkTime);
                odoPrevSpeed = speed;
            }
        } else {
            odoCnt = 0;
        }
    }
}

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

New file
@@ -0,0 +1,12 @@
//
// Created by YY on 2020/8/11.
//

#ifndef MYAPPLICATION2_ODO_GRAPH_H
#define MYAPPLICATION2_ODO_GRAPH_H

void ResetOdo(void);
double ReadOdo(void);
void UpdataOdo(double speed, int moveDirect, const struct RtkTime *rtkTime);

#endif //MYAPPLICATION2_ODO_GRAPH_H

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

@@ -15,6 +15,7 @@
#include "drive_straight.h"
#include "stop_car.h"
#include "operate_gear.h"
#include "../test_common/odo_graph.h"

#include <cmath>
#include <vector>
@@ -115,15 +116,13 @@
static bool laneChanging;
static int changeLaneDirect;

static double odoGraph;
static struct drive_timer odoTimer;
static double odoPrevSpeed;
static int odoCnt;


typedef struct {
    int road;
    int sep;
    int lane;
    int guide;
} lane_t;

typedef struct {
@@ -161,8 +160,6 @@
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);
static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList);
static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
static void ClearAll(road_exam_map &map);
@@ -199,7 +196,7 @@
    prevGearNSlide = false;
    gearNSlideTime = 0;

    currExamMapIndex = FIND_POSITION;
    currExamMapIndex = -1;

    startCar = START_CAR_NOT_DO;
    stopCar = STOP_CAR_NOT_DO;
@@ -222,8 +219,7 @@
    checkTurn = false;
    ClearAll(RoadMap);

    odoGraph = 0.0;
    odoCnt = 0;
    ResetOdo();

    // 初始化考项
}
@@ -426,7 +422,7 @@

/***************************************************************
 * 车辆所在道路，根据车辆的中轴前点
 * @param currRoadIndex
 * @param currRoadIndex, 优先检测当前道路
 * @param RoadMap
 * @param car
 * @return
@@ -493,7 +489,7 @@

/************************************************
 * 车轮压实线，前后轴交叉，前后轮迹交叉
 * @param mode
 * @param mode 1 - 道路实线/分道实线  2 - 分道虚线/半实半虚线
 * @param RoadMap
 * @param car
 * @param CarModelList
@@ -505,6 +501,8 @@
    Line frontLeftTireTrack, frontRightTireTrack;
    Line rearLeftTireTrack, rearRightTireTrack;
    bool track = false;
    int lineType = -1;


    MakeLine(&frontTireAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
    MakeLine(&rearTireAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
@@ -542,6 +540,7 @@
    PointF p1, p2;
    Line redLine;

    // 左右路边实线
    for (int n = 0; n < RoadMap.roads.size(); ++n) {
        for (int m = 0; m < RoadMap.roads[n].leftEdge.size(); ++m) {
            if (RoadMap.roads[n].leftEdge[m].character == LINE_SOLID && RoadMap.roads[n].leftEdge[m].points.size() >= 2) {
@@ -553,7 +552,8 @@

                    if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
                        IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
                        return true;
                        lineType = LINE_SOLID;
                        goto CRASH_LINE_CONFIRM;
                    }

                    if (track &&
@@ -561,7 +561,8 @@
                        IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
                        IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
                        IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
                        return true;
                        lineType = LINE_SOLID;
                        goto CRASH_LINE_CONFIRM;
                    }
                    p1 = p2;
                }
@@ -578,7 +579,8 @@

                    if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
                        IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
                        return true;
                        lineType = LINE_SOLID;
                        goto CRASH_LINE_CONFIRM;
                    }

                    if (track &&
@@ -586,13 +588,15 @@
                         IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
                         IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
                         IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
                        return true;
                        lineType = LINE_SOLID;
                        goto CRASH_LINE_CONFIRM;
                    }
                    p1 = p2;
                }
            }
        }

        // 分道实线
        for (int m = 0; m < RoadMap.roads[n].separate.size(); ++m) {
            // 一组车道
            for (int l = 0; l < RoadMap.roads[n].separate[m].lines.size(); ++l) {
@@ -601,38 +605,54 @@
                    // 一根分道线中线型相同的
                    int character = RoadMap.roads[n].separate[m].lines[l][a].character;

                    if (character == LINE_SOLID && RoadMap.roads[n].separate[m].lines[l][a].points.size() >= 2) {
                        p1 = RoadMap.roads[n].separate[m].lines[l][a].points[0];
                    if (RoadMap.roads[n].separate[m].lines[l][a].points.size() < 2)
                        continue;

                        for (int b = 1; b < RoadMap.roads[n].separate[m].lines[l][a].points.size(); ++b) {
                            p2 = RoadMap.roads[n].separate[m].lines[l][a].points[b];
                            MakeLine(&redLine, &p1, &p2);
                            if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
                                IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
                                return true;
                            }
                            if (track &&
                                (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
                                 IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
                                 IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
                                 IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
                                return true;
                            }
                            p1 = p2;
                    p1 = RoadMap.roads[n].separate[m].lines[l][a].points[0];

                    for (int b = 1; b < RoadMap.roads[n].separate[m].lines[l][a].points.size(); ++b) {
                        p2 = RoadMap.roads[n].separate[m].lines[l][a].points[b];
                        MakeLine(&redLine, &p1, &p2);
                        if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
                            IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
                            lineType = character;
                            goto CRASH_LINE_CONFIRM;
                        }
                        if (track &&
                            (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
                             IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
                             IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
                             IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
                            lineType = character;
                            goto CRASH_LINE_CONFIRM;
                        }
                        p1 = p2;
                    }
                    // 比较最近的连续的n个左侧点和上一个右侧点
                    if (character == LINE_HALF_SOLID_LEFT) {
                        // 不能从左移动到右

                    }
                }
            }
        }
    }

CRASH_LINE_CONFIRM:
    if (lineType == LINE_HALF_SOLID_LEFT) {
        if (currCrashLineType != LINE_HALF_SOLID_LEFT && track) {
            if (IntersectionOfLine(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], redLine) == -1) {
                // 非法跨线
            }
        }
    } else if (lineType == LINE_HALF_SOLID_RIGHT) {
        if (currCrashLineType != LINE_HALF_SOLID_RIGHT && track) {
            if (IntersectionOfLine(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], redLine) == 1) {
                // 非法跨线
            }
        }
    }

    return false;
}



static int GetGuideDirect(road_exam_map &RoadMap, PointF point, int roadIndex, int sepIndex, int laneNo)
{
@@ -657,13 +677,13 @@
{
    Line leftProjLine, rightProjLine;
    Line sep;
    PointF p1, p2;
    PointF p0, p1, p2;

    if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
        return false;

    p1 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].leftEdge, point);
    p2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, point);
    p0 = p2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, point);

    MakeLine(&leftProjLine, &point, &p1);
    MakeLine(&rightProjLine, &point, &p2);
@@ -760,7 +780,7 @@
                    break;
                }
            }

            theLane.guide = GetGuideDirect(RoadMap, p0, theLane.road, theLane.sep, theLane.lane);
            return true;
        }
    }
@@ -877,7 +897,7 @@
{
    uint32_t cts = AppTimer_GetTickCount();
    int ri = CalcRoadIndex(-1, RoadMap, car);
    bool crash = CrashRedLine(0, RoadMap, car, CarModelList);
    bool crash = CrashRedLine(0, 0, RoadMap, car, CarModelList);
    lane_t laneInfo;
    double redist = -1;

@@ -901,24 +921,7 @@

    DEBUG("当前道路索引 %d, 触发红线 %d lane %d 距离 %f %ld", ri, crash, laneInfo.lane, redist, AppTimer_GetTickCount() - cts);

    // 行驶距离，不包含倒车
    if (odoCnt == 0 && moveDirect == 1) {
        odoPrevSpeed = speed;
        odoCnt = 1;
        Rtk2DriveTimer(odoTimer, rtkTime);
    } else if (odoCnt == 1) {
        uint32_t tm = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                                  odoTimer.hour, odoTimer.min, odoTimer.sec, odoTimer.msec*10);
        if (tm >= D_SEC(1)) {
            odoGraph += ((double)tm)*(odoPrevSpeed + speed)/2.0/1000.0;
            if (moveDirect == 1) {
                Rtk2DriveTimer(odoTimer, rtkTime);
                odoPrevSpeed = speed;
            } else {
                odoCnt = 0;
            }
        }
    }
    UpdataOdo(speed, moveDirect, rtkTime);

    // 超速检测
    if (moveDirect != 0 && speed > MAX_SPEED) {
@@ -1097,137 +1100,7 @@
                                 speed, moveDirect, rtkTime);

    // 检测离开此路段，全车需不在范围内
    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);

            RoadMap.roads[currExamMapIndex].arrivedTail = false;

            currExamMapIndex = FIND_POSITION;
        }

        free(carBody.point);
        free(area.point);
    }
    if (currExamMapIndex == FIND_POSITION) {
        DEBUG("搜索道路");
        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);
                if (nextRoadId >= 0 && RoadMap.roads[i].id != nextRoadId) {
                    DEBUG("不按规矩行驶，进入错误路段");
                    AddExamFault(3, rtkTime);
                }
                nextRoadId = -1;
                checkTurn = false;
                break;
            }

            free(area.point);
        }
        if (currExamMapIndex < 0) {
            currExamMapIndex = FIND_POSITION;//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;
            }
        }
    }
    currExamMapIndex = CalcRoadIndex(currExamMapIndex, RoadMap, car);

    // 检测压线状态
    bool crashRedLineNow = false;
@@ -1497,7 +1370,7 @@
    if (startCar != START_CAR_DONE)
        return;

    if (odoGraph > EXAM_RANGE && currRoadItem == NULL && AllCmp(RoadMap) && stopCar == STOP_CAR_NOT_DO) {
    if (ReadOdo() > EXAM_RANGE && currRoadItem == NULL && AllCmp(RoadMap) && stopCar == STOP_CAR_NOT_DO) {
        // 在合适条件下停车结束考试
        StartStopCarExam("请靠边停车");
        stopCar = STOP_CAR_DOING;
@@ -1827,22 +1700,6 @@
    }

    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 Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)