坐标

yy1717

2020-08-11 03ca970f73af542456326b2fa277090ce0ea532f

 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)