坐标

yy1717

2020-05-15 76859aa4b23ea8ebd90bd7553fd70e144bdc96ba

 lib/src/main/cpp/test_items2/through_something.cpp

@@ -18,6 +18,7 @@
using namespace std;

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

@@ -155,17 +156,52 @@
    return index;
}



void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    int BreakDone = ReadCarStatus(BREAK);

    for (int i = 0; i < map.specialAreas.size(); i++) {
        if (map.specialAreas[i].type == ZEBRA_CROSSING || map.specialAreas[i].type == BUS_STATION_AREA) {
            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);
    // 计算车前进轨迹延长线
    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);
    PointF extPoint2 = PointExtend(car->carXY[ car->axial[AXIAL_FRONT] ], NEXT_ROAD_TIP, yaw);
    Line extLine, extLine2;
    MakeLine(&extLine, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint);
    MakeLine(&extLine2, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint2);

    // 路口刹车点
    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) {
                auto itx = breakRecord.find(map.roads[i].id);
                if (itx != breakRecord.end()) {
                    itx->second = true;
                }
            }
        }
        // 跨线后，检查刹车动作
        if (CrashTheLine(map.roads[i].stopLine, car, CarModelList)) {
            auto itx = breakRecord.find(map.roads[i].id);
            if (itx != breakRecord.end()) {
                if (itx->second == false) {
                    // 不按规定减速，不合格
                    DEBUG("不按规定减速");

                }
                itx->second = false;
            }
        }
    }
    // 人行道、公交站刹车点；学校限速区
    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) {
@@ -174,11 +210,19 @@
            }

            PointF vPoint = GetSELine(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[0]);
            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) {
            Line startLine;

            DEBUG("计算垂点 (%f, %f)", vPoint.X, vPoint.Y);

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

            // 车头和斑马线距离不足30米
            if (IntersectionOf(extLine, startLine) == GM_Intersection &&