坐标

yy1717

2020-08-07 e43f00fbe051dc8f9dfa5a19143c38613b64ecad

坐标

 lib/src/main/cpp/driver_test.cpp

@@ -372,7 +372,7 @@
            reportSeatbeltEject = false;

            if (type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
                exam_dummy_light = 0;
                exam_dummy_light = 1;           //0
            }
            if (type == TEST_TYPE_AREA) {
                InitAreaExam();

 lib/src/main/cpp/driver_test.h

@@ -133,8 +133,15 @@
    std::vector<PointF> points;
} segment_t;

typedef struct {
    PointF start;
    PointF end;
    std::vector<int> direct;
} lane_direct_t;

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


 lib/src/main/cpp/native-lib.cpp

@@ -26,7 +26,7 @@
const int RTK_PLATFORM_PORT = 12125;
const uint8_t phone[] = {0x20,0x19,0x10,0x15,0x00,0x00,0x00,0x01};

const char *VIRTUAL_RTK_IP = "192.168.1.16";
const char *VIRTUAL_RTK_IP = "192.168.16.100";
const int VIRTUAL_RTK_PORT = 9001;

static pthread_mutex_t tts_mutex = PTHREAD_MUTEX_INITIALIZER;

 lib/src/main/cpp/rtk_module/rtk.cpp

@@ -214,7 +214,7 @@
        }*/

        if (RxBufLen > 0) {
#if 1
#if 0
            const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
            if(ptr != RxBuf) {
                memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));
@@ -475,7 +475,7 @@

    DEBUG("RTK Module failure!!");

    PlayTTS("RTK模块无法通讯");

    CheckPjkParam();
//    PlayTTS("RTK模块无法通讯");
//
//    CheckPjkParam();
}

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

@@ -628,3 +628,8 @@

    return ext;
}

bool IsSamePoint(PointF p1, PointF p2)
{
    return isEqual(p1.X, p2.X) && isEqual(p1.Y, p2.Y);
}

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

@@ -64,4 +64,6 @@
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);
bool IsSamePoint(PointF p1, PointF p2);

#endif //GUI_GEOMETRY_H

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

@@ -119,6 +119,18 @@
static double odoPrevSpeed;
static int odoCnt;

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

typedef struct {
    int edgeIndex;
    int pointIndex;
    PointF point;
} projection_t;

static const int MAX_ENGINE_RPM = 2500;
static const double START_CAR_MOVE_DISTANCE = 10.0;
static const double START_CAR_CHECK_DOOR_DISTANCE = 1.0;
@@ -128,7 +140,6 @@
static const uint32_t CHANGE_LANE_MIN_INTERVAL = D_SEC(10);
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;
@@ -157,6 +168,9 @@
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 int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car);

void InitRoadExam(road_exam_map &RoadMap)
{
@@ -222,6 +236,523 @@

    AppTimer_delete(TurnSignalError13ColdTimer);
    AppTimer_delete(TurnSignalError14ColdTimer);
}

/*********************************************************************
 * 计算某点到道路边线的投影点
 * @param edge
 * @param road
 * @param point
 * @return
 */
static PointF CalcProjectionWithRoadEdge(vector<edge_t> &edge, PointF point)
{
    PointF p1, p2;
    PointF proj;

    vector<PointF> projArray;

    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)) {
                projArray.push_back(vp);
            }

            p1 = p2;
        }
    }

    if (projArray.size() == 0) {
        // 没有任何垂点，找到最近的点
        proj = edge[0].points[0];
        for (int i = 0; i < edge.size(); ++i) {
            for (int j = 1; j < edge[i].points.size(); ++j) {
                if (DistanceOf(point, edge[i].points[j]) < DistanceOf(point, proj)) {
                    proj = edge[i].points[j];
                }
            }
        }
    } else {
        // 最近的垂点
        proj = projArray[0];
        for (int i = 1; i < projArray.size(); ++i) {
            if (DistanceOf(point, projArray[i]) < DistanceOf(point, proj)) {
                proj = projArray[i];
            }
        }
    }

    return proj;
}

static projection_t CalcProjectionWithRoadEdgeEx(vector<edge_t> &edge, PointF point)
{
    PointF p1, p2;
    Line line;
    projection_t theProj;

    vector<projection_t> projSet;

    // 计算垂点
    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)) {
                projection_t proj;
                proj.edgeIndex = i;
                proj.pointIndex = j - 1;
                proj.point = vp;
                projSet.push_back(proj);
            }

            p1 = p2;
        }
    }

    if (projSet.size() == 0) {
        // 没有任何垂点，找到最近的点
        theProj.edgeIndex = 0;
        theProj.pointIndex = 0;
        theProj.point = edge[0].points[0];

        for (int i = 0; i < edge.size(); ++i) {
            for (int j = 1; j < edge[i].points.size(); ++j) {
                if (DistanceOf(point, edge[i].points[j]) < DistanceOf(point, theProj.point)) {
                    theProj.edgeIndex = i;
                    theProj.pointIndex = j - 1;
                    theProj.point = edge[i].points[j];
                }
            }
        }
    } else {
        // 最近的垂点
        theProj = projSet[0];
        for (int i = 1; i < projSet.size(); ++i) {
            if (DistanceOf(point, projSet[i].point) < DistanceOf(point, theProj.point)) {
                theProj = projSet[i];
            }
        }
    }

    return theProj;
}

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

    projection_t projection, refProjection;

    projection = CalcProjectionWithRoadEdgeEx(edge, point);
    refProjection = CalcProjectionWithRoadEdgeEx(edge, refPoint);

    if (projection.edgeIndex != refProjection.edgeIndex || projection.pointIndex != refProjection.pointIndex) {
        bool ahead = false;
        if (projection.edgeIndex != refProjection.edgeIndex) {
            if (projection.edgeIndex > refProjection.edgeIndex) {
                projection_t temp;
                temp = projection;
                projection = refProjection;
                refProjection = temp;
                ahead = true;
            }
            int a = projection.edgeIndex;
            int b = projection.pointIndex + 1;

            if (b < edge[a].points.size()) {
                distance += DistanceOf(projection.point, edge[projection.edgeIndex].points[b]);
            }

            for (; a < refProjection.edgeIndex; ++a) {
                for (; b < edge[a].points.size() - 1; ++b) {
                    distance += DistanceOf(edge[a].points[b], edge[a].points[b+1]);
                }
                b = 0;
            }

            for (int i = 0; i < refProjection.pointIndex; i++) {
                distance += DistanceOf(edge[a].points[i], edge[a].points[i+1]);
            }

            distance += DistanceOf(refProjection.point, edge[refProjection.edgeIndex].points[refProjection.pointIndex]);
        } else {
            if (projection.pointIndex > refProjection.pointIndex) {
                projection_t temp;
                temp = projection;
                projection = refProjection;
                refProjection = temp;
                ahead = true;
            }
            int a = projection.edgeIndex;
            int b = projection.pointIndex + 1;

            if (b < edge[a].points.size()) {
                distance += DistanceOf(projection.point, edge[projection.edgeIndex].points[b]);
            }

            for (; b < refProjection.pointIndex; b++) {
                distance += DistanceOf(edge[a].points[b], edge[a].points[b+1]);
            }

            distance += DistanceOf(refProjection.point, edge[refProjection.edgeIndex].points[refProjection.pointIndex]);
        }
        if (ahead) {
            distance *= -1.0;
        }
    } else {
        distance = DistanceOf(projection.point, refProjection.point);

        if (DistanceOf(projection.point, edge[projection.edgeIndex].points[projection.pointIndex]) >
                DistanceOf(refProjection.point, edge[refProjection.edgeIndex].points[refProjection.pointIndex])) {
            distance *= -1.0;
        }
    }

    return distance;
}

/***************************************************************
 * 车辆所在道路，根据车辆的中轴前点
 * @param currRoadIndex
 * @param RoadMap
 * @param car
 * @return
 */
static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car)
{
    int n = 0;
    int index = currRoadIndex;

    if (index < 0) {
        index = 0;
    }

    while (n++ < RoadMap.roads.size()) {
        bool changeSegment = false;
        vector<PointF> roadOutLine;
        Polygon area;

        for (int i = 0; i < RoadMap.roads[index].leftEdge.size(); ++i) {
            for (int j = 0; j < RoadMap.roads[index].leftEdge[i].points.size(); ++j) {
                if (changeSegment && roadOutLine.size() > 0 && IsSamePoint(roadOutLine.back(), RoadMap.roads[index].leftEdge[i].points[j])) {
                    continue;
                }
                changeSegment = false;
                roadOutLine.push_back(RoadMap.roads[index].leftEdge[i].points[j]);
            }
            changeSegment = true;
        }

        for (int i = 0; i < RoadMap.roads[index].rightEdge.size(); ++i) {
            for (int j = RoadMap.roads[index].rightEdge[i].points.size() - 1; j >= 0; --j) {
                if (changeSegment && roadOutLine.size() > 0 && IsSamePoint(roadOutLine.back(), RoadMap.roads[index].leftEdge[i].points[j])) {
                    continue;
                }
                changeSegment = false;
                roadOutLine.push_back(RoadMap.roads[index].rightEdge[i].points[j]);
            }
            changeSegment = true;
        }

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

        for (int i = 0; i < area.num; ++i) {
            area.point[i] = roadOutLine[i];
        }

        if (IntersectionOf(car->carXY[car->axial[AXIAL_FRONT]], &area) == GM_Containment) {
            free(area.point);
            break;
        }

        free(area.point);

        index = (index + 1) % RoadMap.roads.size();
    }

    if (n >= RoadMap.roads.size()) {
        index = -1;
    }

    return index;
}

/************************************************
 * 车轮压实线，前后轴交叉，前后轮迹交叉
 * @param mode
 * @param RoadMap
 * @param car
 * @param CarModelList
 * @return
 */
static bool CrashRedLine(int mode, int roadIndex, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    Line frontTireAxle, rearTireAxle;
    Line frontLeftTireTrack, frontRightTireTrack;
    Line rearLeftTireTrack, rearRightTireTrack;
    bool track = false;

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

    if (CarModelList.size() >= 2) {
        list<car_model *>::iterator iter = CarModelList.begin();

        PointF p11, p12, p21, p22, p31, p32, p41, p42;

        p11 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]];
        p21 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_front_tire[TIRE_OUTSIDE]];
        p31 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_rear_tire[TIRE_OUTSIDE]];
        p41 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_rear_tire[TIRE_OUTSIDE]];

        struct RtkTime time1 = ((car_model *)(*iter))->tm;

        ++iter;

        p12 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]];
        p22 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_front_tire[TIRE_OUTSIDE]];
        p32 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_rear_tire[TIRE_OUTSIDE]];
        p42 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_rear_tire[TIRE_OUTSIDE]];

        struct RtkTime time2 = ((car_model *)(*iter))->tm;

        if (TimeGetDiff(time1.hh, time1.mm, time1.ss, time1.mss, time2.hh, time2.mm, time2.ss, time2.mss) <= D_SEC(1)) {
            MakeLine(&frontLeftTireTrack, &p11, &p12);
            MakeLine(&frontRightTireTrack, &p21, &p22);
            MakeLine(&rearLeftTireTrack, &p31, &p32);
            MakeLine(&rearRightTireTrack, &p41, &p42);
            track = true;
        }
    }

    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) {
                p1 = RoadMap.roads[n].leftEdge[m].points[0];
                for (int l = 1; l < RoadMap.roads[n].leftEdge[m].points.size(); ++l) {
                    p2 = RoadMap.roads[n].leftEdge[m].points[l];

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

        for (int m = 0; m < RoadMap.roads[n].rightEdge.size(); ++m) {
            if (RoadMap.roads[n].rightEdge[m].character == LINE_SOLID && RoadMap.roads[n].rightEdge[m].points.size() >= 2) {
                p1 = RoadMap.roads[n].rightEdge[m].points[0];
                for (int l = 1; l < RoadMap.roads[n].rightEdge[m].points.size(); ++l) {
                    p2 = RoadMap.roads[n].rightEdge[m].points[l];

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

        for (int m = 0; m < RoadMap.roads[n].separate.size(); ++m) {
            // 一组车道
            for (int l = 0; l < RoadMap.roads[n].separate[m].lines.size(); ++l) {
                // 多根分道线
                for (int a = 0; a < RoadMap.roads[n].separate[m].lines[l].size(); ++a) {
                    // 一根分道线中线型相同的
                    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];

                        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;
                        }
                    }
                    // 比较最近的连续的n个左侧点和上一个右侧点
                    if (character == LINE_HALF_SOLID_LEFT) {
                        // 不能从左移动到右

                    }
                }
            }
        }
    }

    return false;
}

/********************************************
 * 计算某点在哪个车道
 * @param point
 * @param RoadMap
 * @param roadIndex
 * @return
 */
static bool GetLane(lane_t &theLane, PointF point, road_exam_map &RoadMap, int roadIndex)
{
    Line leftProjLine, rightProjLine;
    Line sep;
    PointF 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);

    MakeLine(&leftProjLine, &point, &p1);
    MakeLine(&rightProjLine, &point, &p2);

    for (int i = 0; i < RoadMap.roads[roadIndex].separate.size(); ++i) {
        struct car_lane_pos {
            int lane_no;
            char pos;            // 点在车道的左右位置

            bool operator == (const int &x) {
                return(this->lane_no == x);
            }
        };

        vector<struct car_lane_pos> CLP;
        bool oneMoreIntersection = false;

        for (int j = 0;  j < RoadMap.roads[roadIndex].separate[i].lines.size(); ++j) {              // 每一根线分组
            bool intersection = false;
            for (int k = 0; !intersection && k < RoadMap.roads[roadIndex].separate[i].lines[j].size(); ++k) {        // 同一线型的线分组

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

                    if (IntersectionOf(sep, leftProjLine) == GM_Intersection) {
                        intersection = true;
                        struct car_lane_pos temp;

                        temp.lane_no = j;
                        temp.pos = 'R';
                        CLP.push_back(temp);
                        break;
                    }
                    if (IntersectionOf(sep, rightProjLine) == GM_Intersection) {
                        intersection = true;
                        struct car_lane_pos temp;

                        temp.lane_no = j;
                        temp.pos = 'L';
                        CLP.push_back(temp);
                        break;
                    }
                    p1 = p2;
                }
            }
            if (!intersection) {
                struct car_lane_pos temp;
                temp.lane_no = j;
                temp.pos = 0;
                CLP.push_back(temp);
            } else {
                oneMoreIntersection = true;
            }
        }

        if (oneMoreIntersection) {
            // 考虑到采集偏差可能每一根线无法对齐，只要其中一根相交，其它不相交的也参与
            for (vector<struct car_lane_pos>::iterator it = CLP.begin(); it != CLP.end(); ++it) {
                if ((*it).pos == 0) {
                    int m = RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no].size();
                    int n = RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points.size();

                    if (DistanceOf(RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][0].points[0], point) >
                        DistanceOf(RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points[n-1], point)) {
                        // 车辆接近车道尾部
                        MakeLine(&sep, &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points[n-2], &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points[n-1]);
                    } else {
                        // 车辆接近车道头部
                        MakeLine(&sep, &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][0].points[0], &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][0].points[1]);
                    }

                    struct car_lane_pos temp;

                    temp.lane_no = (*it).lane_no;

                    if (IntersectionOfLine(point, sep) == -1) {
                        temp.pos = 'R';
                    } else {
                        temp.pos = 'L';
                    }
                    (*it) = temp;
                }
            }

            theLane.road = roadIndex;
            theLane.sep = i;
            theLane.lane = CLP.size();

            for (int x = 0; x < CLP.size(); ++x) {
                if (CLP[x].pos == 'L') {
                    theLane.lane = x;
                    break;
                }
            }

            return true;
        }
    }

    return false;
}

static void TurnSignalError13ColdTimer(union sigval sig)
@@ -331,6 +862,32 @@

void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    uint32_t cts = AppTimer_GetTickCount();
    int ri = CalcRoadIndex(-1, RoadMap, car);
    bool crash = CrashRedLine(0, RoadMap, car, CarModelList);
    lane_t laneInfo;
    double redist = -1;

    laneInfo.road = -1;
    laneInfo.sep = -1;
    laneInfo.lane = -1;

    if (ri >= 0) {
        GetLane(laneInfo, car->carXY[car->axial[AXIAL_FRONT]], RoadMap, ri);

        int m = RoadMap.roads[ri].rightEdge.size();
        int n = RoadMap.roads[ri].rightEdge[m-1].points.size();

        PointF base;

        base.X = 428922.2985; base.Y = 3292119.5457;

        redist = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], base,
                              RoadMap.roads[ri].rightEdge);
    }

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

    // 行驶距离，不包含倒车
    if (odoCnt == 0 && moveDirect == 1) {
        odoPrevSpeed = speed;
@@ -1424,56 +1981,6 @@
    return false;
}

/*********************************************************************
 * 计算某点到道路左边线的最近垂点
 * @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;
}

/***************************************************
 * 接近路口时，提示下一步怎么走
@@ -1515,7 +2022,7 @@
                RoadMap.triggerLines[i].leftPoints.clear();

                for (int j = 0; j < RoadMap.triggerLines[i].points.size(); ++j) {
                    RoadMap.triggerLines[i].leftPoints.push_back(GetSELine(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[j]));
                    RoadMap.triggerLines[i].leftPoints.push_back(CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[j]));
                }

                for (int j = 0; j < RoadMap.triggerLines[i].points.size(); ++j) {
@@ -1630,12 +2137,12 @@
                    break;
            }

            PointF vPoint = GetSELine(map.roads[road].leftEdge, map.specialAreas[i].area[0]);
            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 = GetSELine(map.roads[road].leftEdge, map.specialAreas[i].area[1]);
            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);
        }