坐标

yy1717

2020-08-14 576d819c4d9aabd1a8d102ae9c3bc13c9165ee1f

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

@@ -83,13 +83,8 @@
const int OVERTAKE_HOLD_TIME = D_SEC(3);                // 在超车道行驶的一段时间
const double EXAM_RANGE = 3000.0;                       // 至少驾驶距离

static const double LASTEST_BREAK_POINT = 30.0;
static const double NEXT_ROAD_TIP = 100.0;                  // 到达路口前提示下一个怎么走
static const double DISTANCE_STOP_CAR_TO_STOP_LINE = 5.0;
static const double PASS_SCHOOL_MAX_SPEED = 30.0;           // kmh

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

static bool occurCrashRedLine;
static bool occurCrashGreenLine;
@@ -125,7 +120,6 @@
static int startCar, stopCar;
static int currExamMapIndex;
static trigger_line_t *currRoadItem;
static int nextRoadId;
static PointF roadItemStartPoint;
static struct drive_timer roadItemStartTime;
static bool overtake = false;
@@ -168,12 +162,9 @@


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);
static bool AllCmp(road_exam_map &map);
static void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList,
                             double speed, int moveDirect, const struct RtkTime *rtkTime);

static PointF CalcProjectionWithRoadEdge(vector<edge_t> &edge, PointF point);
static bool AllCmp(road_exam_map &map);

static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car);

void InitRoadExam(road_exam_map &RoadMap)
@@ -215,15 +206,14 @@
    checkStartCarSignal = startCarLeftTurnSignal = false;

    Lane.road = Lane.sep = Lane.no = -1;
    Lane.guide = 0;

    nextRoadId = -1;
    checkTurn = false;
    ClearAll(RoadMap);

    CrashLineType = -1;

    ResetOdo();

    ResetTarget(RoadMap);
    // 初始化考项
}

@@ -242,7 +232,7 @@
 * @param point
 * @return
 */
static PointF CalcProjectionWithRoadEdge(vector<edge_t> &edge, PointF point)
PointF CalcProjectionWithRoadEdge(vector<edge_t> &edge, PointF point)
{
    PointF p1, p2;
    PointF proj;
@@ -346,7 +336,7 @@
    return theProj;
}

static double CalcDistanceReference(PointF point, PointF refPoint, vector<edge_t> &edge)
double CalcDistanceReference(PointF point, PointF refPoint, vector<edge_t> &edge)
{
    double distance = 0;

@@ -657,7 +647,7 @@
    for (int i = 0; i < RoadMap.roads[roadIndex].separate[sepIndex].lane_direct.size(); ++i) {
        double d1 = CalcDistanceReference(point, RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].start, RoadMap.roads[roadIndex].rightEdge);
        double d2 = CalcDistanceReference(point, RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].end, RoadMap.roads[roadIndex].rightEdge);
        if (fabs(d1) < 1e-6 && d1 < 0.0 && d2 > 1e-6 && laneNo < RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].direct.size()) {
        if (d1 < -1e-3 && d2 > 1e-3 && laneNo < RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].direct.size()) {
            return RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].direct[laneNo];
        }
    }
@@ -1063,144 +1053,51 @@
    }
}

static map <int, int> TargetReduceRec;

enum {
    NOT_REDUCE_SPEED,
    REDUCE_SPEED,
    STOP_CAR,
    OVER_SPEED
};

static void ChangeTargetReduceRec(int roadIndex, int index, int status)
static int NearbyCrossingGuide(int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    int key = roadIndex * 100 + index;
    int guide = 0;
    double distance;

    auto it = CrossingHint.find(key);

    if (it != CrossingHint.end()) {
        CrossingHint.erase(it);
    }
    CrossingHint.insert(pair<int, int>(key, status));
}

static int GetTargetReduceRec(int roadIndex, int index)
{
    int key = roadIndex * 100 + index;

    auto it = CrossingHint.find(key);

    if (it != CrossingHint.end()) {
        return it->second;
    }
    return -1;
}

void ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, double speed, int moveDirect)
{
    car_sensor_value_t brk = ReadCarSensorValue(BREAK);

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

    for (int n = 0; n < RoadMap.roads[roadIndex].stopLine.size(); ++n) {
        PointF point;
    struct nearby_crossing {
        int guide;
        double distance;
    };

    vector<struct nearby_crossing> set;

    for (int i = 0; i < road.stopLine.size(); ++i) {
        PointF point;

        point.X = road.stopLine[i].line.X1;
        point.Y = road.stopLine[i].line.Y1;

        distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, RoadMap.roads[roadIndex].rightEdge);
        distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point,
                                         road.rightEdge);

        if (distance > 1e-3 && distance < LASTEST_BREAK_POINT) {
            // 记录刹车，停车
            if (brk.value == BREAK_ACTIVE) {
        if (distance > 1e-3) {
            struct nearby_crossing temp;

            }
        } else if (distance > LASTEST_BREAK_POINT + 5) {
            temp.guide = road.stopLine[i].active;
            temp.distance = distance;

            set.push_back(temp);
        }
    }

    for (int i = 0; i < RoadMap.specialAreas.size(); i++) {
        if (RoadMap.specialAreas[i].type == GRID_AREA || RoadMap.specialAreas[i].road != RoadMap.roads[roadIndex].id)
            continue;
    if (set.size() > 0) {
        distance = set[0].distance;
        guide = set[0].guide;

        if (map.specialAreas[i].area.size() == 2 && map.specialAreas[i].leftPoints.size() != 2) {
            // 计算点到左侧路边线的垂点
            int road = 0;
            for (road = 0; road < map.roads.size(); ++road) {
                if (map.roads[road].id == map.specialAreas[i].road)
                    break;
        for (int i = 1; i < set.size(); ++i) {
            if (distance > set[i].distance) {
                distance = set[i].distance;
                guide = set[i].guide;
            }

            PointF vPoint = CalcProjectionWithRoadEdge(map.roads[road].leftEdge, map.specialAreas[i].area[0]);
//            DEBUG("计算垂点1 (%f, %f)", vPoint.X, vPoint.Y);

            map.specialAreas[i].leftPoints.push_back(vPoint);

            vPoint = CalcProjectionWithRoadEdge(map.roads[road].leftEdge, map.specialAreas[i].area[1]);
//            DEBUG("计算垂点2 (%f, %f)", vPoint.X, vPoint.Y);
            map.specialAreas[i].leftPoints.push_back(vPoint);
        }

        if (map.specialAreas[i].type == ZEBRA_CROSSING || map.specialAreas[i].type == BUS_STATION_AREA) {
//            DEBUG("斑马线");
            Line startLine;

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

            // 车头和斑马线距离不足30米
            if (IntersectionOf(extLine, startLine) == GM_Intersection &&
                IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], startLine) == 1 ) {
                DEBUG("进入减速区 %d", map.specialAreas[i].type);
                if (BreakDone == BREAK_ACTIVE) {
                    map.specialAreas[i].activeBreak = true;
                }
            }

            // 跨线后，检查刹车动作
            if (CrashTheLine(startLine, car, CarModelList)) {
                if (!map.specialAreas[i].activeBreak) {
                    // 不按规定减速，不合格
                    DEBUG("不按规定减速");
                    if (map.specialAreas[i].type == ZEBRA_CROSSING) {
                        AddExamFault(48, rtkTime);
                    } else {
                        AddExamFault(50, rtkTime);
                    }
                } else {
                    DEBUG("按规定减速");
                }

            }
        } else if (map.specialAreas[i].type == SCHOOL_AREA) {
            Polygon school;
//            DEBUG("学校");
            school.num = 4;
            school.point = (PointF *) malloc(school.num * sizeof(PointF));

            school.point[0] = map.specialAreas[i].area[0];
            school.point[1] = map.specialAreas[i].area[1];
            school.point[2] = map.specialAreas[i].leftPoints[1];
            school.point[3] = map.specialAreas[i].leftPoints[0];

            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &school) == GM_Containment) {
                if (ConvertMs2KMh(speed) > PASS_SCHOOL_MAX_SPEED) {
                    if (!map.specialAreas[i].overSpeed) {
                        DEBUG("通过学校区域超速");
                        AddExamFault(49, rtkTime);
                        map.specialAreas[i].overSpeed = true;
                    }
                }
            }

            free(school.point);
        }
    }

    return guide;
}



void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
@@ -1384,20 +1281,19 @@
        }
    } else {
        // 前进

    }


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

    // 检测离开此路段，全车需不在范围内
    int oldid = currExamMapIndex;
    currExamMapIndex = CalcRoadIndex(currExamMapIndex, RoadMap, car);
    if (currExamMapIndex >= 0) {
        car_sensor_value_t brk = ReadCarSensorValue(BREAK);
        HintCrossing(currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList);

        // 检测通过路口、人行道等区域时，释放刹车或减速
        ApproachTarget(RoadMap, car, currExamMapIndex, (brk.value == BREAK_ACTIVE), speed, moveDirect, rtkTime);
    }
    ExitTarget(RoadMap, car, CarModelList, rtkTime);

    if (oldid != currExamMapIndex) {
        DEBUG("道路ID切换 %d", currExamMapIndex);
@@ -1411,7 +1307,19 @@
        DEBUG("压线类型切换 %d", CrashLineType);
    }


    oldid = Lane.guide;
    DetectLane(RoadMap, car, currExamMapIndex, rtkTime);
    if (Lane.guide > 0 && currExamMapIndex >= 0) {
        if (!(NearbyCrossingGuide(currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList) & Lane.guide)) {
            DEBUG("不按规定车道标向行驶");
            AddExamFault(9, rtkTime);
        }
    }

    if (Lane.guide != oldid) {
        DEBUG("导向类型切换 %d", Lane.guide);
    }

    // 检测压线状态
    TestRoadStartCar(car, speed, moveDirect, rtkTime);
@@ -1702,405 +1610,3 @@
    }
    return true;
}

/************************************************************************
 * 开始新的考试后，清除地图所有的刹车、停车记录
 * @param map
 */
static void ClearAll(road_exam_map &map)
{
//    for (int i = 0; i < map.roads.size(); ++i) {
//        map.roads[i].activeStop = map.roads[i].activeBreak = false;
//        map.roads[i].arrivedTail = false;
//        map.roads[i].errorLane = false;
//    }
//    for (int i = 0; i < map.specialAreas.size(); i++) {
//        map.specialAreas[i].overSpeed = map.specialAreas[i].activeBreak = false;
//    }
//    for (int i = 0; i < map.triggerLines.size(); ++i) {
//        map.triggerLines[i].cmp = false;
//    }
}

static void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList,
                             double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    int BreakDone = ReadCarStatus(BREAK);

    // 计算车前进轨迹延长线
    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);

    // 路口刹车点
/*    for (int i = 0; i < map.roads.size(); ++i) {
        // 车头和路口距离不足30米
        if (IntersectionOf(extLine, map.roads[i].stopLine) == GM_Intersection &&
            IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], map.roads[i].stopLine) == 1 ) {
            DEBUG("进入减速区");
            if (BreakDone == BREAK_ACTIVE) {
                map.roads[i].activeBreak = true;
            }
            if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]],
                           map.roads[i].stopLine) < DISTANCE_STOP_CAR_TO_STOP_LINE && moveDirect == 0) {
                map.roads[i].activeStop = true;
            }
        }
        // 跨线后，检查刹车动作
        if (CrashTheLine(map.roads[i].stopLine, car, CarModelList)) {
            if (map.roads[i].stopFlag != 0 && !map.roads[i].activeStop) {
                // 不停车瞭望，不合格
                DEBUG("不停车瞭望");
                if (map.roads[i].active == ROAD_ACTIVE_FORWARD) {
                    AddExamFault(42, rtkTime);
                } else if (map.roads[i].active == ROAD_ACTIVE_TURN_LEFT) {
                    AddExamFault(44, rtkTime);
                } else if (map.roads[i].active == ROAD_ACTIVE_TURN_RIGHT) {
                    AddExamFault(47, rtkTime);
                }
            }
            if (!map.roads[i].activeBreak) {
                // 不按规定减速，不合格
                DEBUG("不按规定减速");
                if (map.roads[i].active == ROAD_ACTIVE_FORWARD) {
                    AddExamFault(41, rtkTime);
                } else if (map.roads[i].active == ROAD_ACTIVE_TURN_LEFT) {
                    AddExamFault(43, rtkTime);
                } else if (map.roads[i].active == ROAD_ACTIVE_TURN_RIGHT) {
                    AddExamFault(46, rtkTime);
                }
            }
        }
    }*/

    // 人行道、公交站刹车点；学校限速区
//    DEBUG("补全特殊区域 size = %d", map.specialAreas.size());
    for (int i = 0; i < map.specialAreas.size(); i++) {
        if (map.specialAreas[i].type == GRID_AREA)
            continue;

        if (map.specialAreas[i].area.size() == 2 && map.specialAreas[i].leftPoints.size() != 2) {
            // 计算点到左侧路边线的垂点
            int road = 0;
            for (road = 0; road < map.roads.size(); ++road) {
                if (map.roads[road].id == map.specialAreas[i].road)
                    break;
            }

            PointF vPoint = CalcProjectionWithRoadEdge(map.roads[road].leftEdge, map.specialAreas[i].area[0]);
//            DEBUG("计算垂点1 (%f, %f)", vPoint.X, vPoint.Y);

            map.specialAreas[i].leftPoints.push_back(vPoint);

            vPoint = CalcProjectionWithRoadEdge(map.roads[road].leftEdge, map.specialAreas[i].area[1]);
//            DEBUG("计算垂点2 (%f, %f)", vPoint.X, vPoint.Y);
            map.specialAreas[i].leftPoints.push_back(vPoint);
        }

        if (map.specialAreas[i].type == ZEBRA_CROSSING || map.specialAreas[i].type == BUS_STATION_AREA) {
//            DEBUG("斑马线");
            Line startLine;

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

            // 车头和斑马线距离不足30米
            if (IntersectionOf(extLine, startLine) == GM_Intersection &&
                IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], startLine) == 1 ) {
                DEBUG("进入减速区 %d", map.specialAreas[i].type);
                if (BreakDone == BREAK_ACTIVE) {
                    map.specialAreas[i].activeBreak = true;
                }
            }

            // 跨线后，检查刹车动作
            if (CrashTheLine(startLine, car, CarModelList)) {
                if (!map.specialAreas[i].activeBreak) {
                    // 不按规定减速，不合格
                    DEBUG("不按规定减速");
                    if (map.specialAreas[i].type == ZEBRA_CROSSING) {
                        AddExamFault(48, rtkTime);
                    } else {
                        AddExamFault(50, rtkTime);
                    }
                } else {
                    DEBUG("按规定减速");
                }

            }
        } else if (map.specialAreas[i].type == SCHOOL_AREA) {
            Polygon school;
//            DEBUG("学校");
            school.num = 4;
            school.point = (PointF *) malloc(school.num * sizeof(PointF));

            school.point[0] = map.specialAreas[i].area[0];
            school.point[1] = map.specialAreas[i].area[1];
            school.point[2] = map.specialAreas[i].leftPoints[1];
            school.point[3] = map.specialAreas[i].leftPoints[0];

            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &school) == GM_Containment) {
                if (ConvertMs2KMh(speed) > PASS_SCHOOL_MAX_SPEED) {
                    if (!map.specialAreas[i].overSpeed) {
                        DEBUG("通过学校区域超速");
                        AddExamFault(49, rtkTime);
                        map.specialAreas[i].overSpeed = true;
                    }
                }
            }

            free(school.point);
        }
    }
//    DEBUG("补全特殊区域 over");
}

#if 0

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

static CarOnTrackInfo_t CarOnTrackInfo;

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

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

    }

    CarOnTrackInfo = newCarOnTrackInfo;


}

/**************************************************
 * 车辆当前所在路段，车头需越过起始线一定距离
 * @param currIndex
 * @param map
 * @param car
 */
static void EnterRoad(int &currIndex, struct road_exam2_map &map, const car_model *car)
{
    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]];
    }

    Polygon roadArea;
    roadArea.num = 0;
    roadArea.point = NULL;

    for (int i = 0; i < map.roads.size(); ++i) {
        if (roadArea.point != NULL) {
            free(roadArea.point);
        }

        for (int x = 0; x < map.roads[i].leftEdge.size(); ++x) {
            roadArea.num += map.roads[i].leftEdge[x].points.size();
        }
        for (int x = 0; x < map.roads[i].rightEdge.size(); ++x) {
            roadArea.num += map.roads[i].rightEdge[x].points.size();
        }
        roadArea.point = (PointF *) malloc(roadArea.num * sizeof(PointF));

        int n = 0;
        for (int j = 0; j < map.roads[i].leftEdge.size(); j++) {
            for (int x = 0; x < map.roads[i].leftEdge[j].points.size(); ++x) {
                if (n > 0 && x == 0 && isEqual2(roadArea.point[n-1].X, map.roads[i].leftEdge[j].points[x].X) &&
                             isEqual2(roadArea.point[n-1].Y, map.roads[i].leftEdge[j].points[x].Y)) {
                    // 第一个点已经存在于上一条线
                } else {
                    roadArea.point[n++] = map.roads[i].leftEdge[j].points[x];
                }
            }
        }
        for (int j = map.roads[i].rightEdge.size(); j > 0; j--) {
            for (int x = map.roads[i].rightEdge[j].points.size(); x > 0; --x) {
                if (n > 0 && x == map.roads[i].rightEdge[j].points.size() &&
                            isEqual2(roadArea.point[n-1].X, map.roads[i].rightEdge[j - 1].points[x-1].X) &&
                            isEqual2(roadArea.point[n-1].Y, map.roads[i].rightEdge[j - 1].points[x-1].Y)) {
                    // 第一个点已经存在于上一条线
                } else {
                    roadArea.point[n++] = map.roads[i].rightEdge[j - 1].points[x - 1];
                }
            }
        }
        roadArea.num = n;

        if (IntersectionOf(car->carXY[car->axial[AXIAL_FRONT]], &roadArea) == GM_Containment) {
            currIndex = i;
            goto CHECK_CAR_ON_ROAD_END;
        }
    }
    currIndex = INVALID_ROAD;

    CHECK_CAR_ON_ROAD_END:
    if (roadArea.point != NULL) {
        free(roadArea.point);
    }
    free(carBody.point);
}

/******************************************************
 * 全车都需离开这个区域
 * @param currIndex
 * @param map
 * @param car
 * @return
 */
static bool ExitRoad(int currIndex, struct road_exam2_map &map, const car_model *car)
{
    // 全车都需不在地图中
    bool ret = false;

    if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
        return ret;

    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, &map.roads[currIndex].area) == GM_None) {
        ret = true;
    }

    free(carBody.point);
    return ret;
}

static bool CrashSeparateLine(int currIndex, struct road_exam2_map &map, const car_model *car)
{
    Line frontAxle, rearAxle;

    if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
        return false;

    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 < map.roads[currIndex].separate.size(); i++) {
        // 分段中的每条线
        for (int j = 0; j < map.roads[currIndex].separate[i].line.size(); j++) {
            Line theLine;
            int p1 = 0;
            for (int p2 = 1; p2 < map.roads[currIndex].separate[i].line[j].num; ++p2) {
                MakeLine(&theLine, &map.roads[currIndex].separate[i].line[j].point[p1], &map.roads[currIndex].separate[i].line[j].point[p2]);
                if (IntersectionOf(theLine, frontAxle) == GM_Intersection ||
                    IntersectionOf(theLine, rearAxle) == GM_Intersection) {
                    return true;
                }
                p1 = p2;
            }
        }
    }
    return false;
}


static void DetectSeparate(int currIndex, struct road_exam2_map &map, const car_model *car)
{
    int segment;
    int lane = -1;

    CarOnTrackInfo_t newInfo;

    if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
        return;

    // 遍历每一分段
    for (int i = 0; i < map.roads[currIndex].separate.size(); i++) {
        int separate_line_num = map.roads[currIndex].separate[i].lines.size();

        struct vrecord_ {
            int valid;
            int p1;
            int p2;
        };

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

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

            struct vrecord_ v;
            v.valid = 0;

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

            vrecord.push_back(v);

            lane = separate_line_num;//
        }



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

                int rel = IntersectionOfLine(map.roads[currIndex].separate[i].lines[x].point[vrecord[x].p1],
                                             map.roads[currIndex].separate[i].lines[x].point[vrecord[x].p2],
                                             car->basePoint);
                if (rel != -1) {
                    newInfo.road = currIndex;
                    newInfo.segment = i;
                    newInfo.lane = x;
                    break;
                }
            }

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

            break;
        }
    }
}



#endif