新的科目三

yy1717

2020-05-11 a073dc3c983b4c56c5da92642c9ad11995bdb844

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