新的科目三

yy1717

2020-05-11 a073dc3c983b4c56c5da92642c9ad11995bdb844

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

@@ -2,6 +2,9 @@
// Created by YY on 2020/3/20.
//

#include <map>
#include <algorithm>

#include "through_something.h"
#include "../driver_test.h"
#include "../test_common/Geometry.h"
@@ -12,12 +15,17 @@

#define DEBUG(fmt, args...)     LOGD("<through_something> <%s>: " fmt, __func__, ##args)

using namespace std;

static const double LASTEST_BREAK_POINT = 30.0;
static const double DISTANCE_STOP_CAR_TO_STOP_LINE = 5.0;
static const double PASS_SCHOOL_MAX_SPEED = 30.0;           // kmh

static int breakActive;
static int stopActive;
static bool crashRedLine;

static map<int, bool> breakRecord;              // 记录车辆驶过各个停止线前刹车动作

void StartThroughExam(int index, LIST_ROAD_MAP &RoadMapList)
{
@@ -33,6 +41,20 @@
    breakActive = 0;
    stopActive = 0;
    crashRedLine = false;
}

void InitThroughSomething(road_exam_map &RoadMap)
{
    breakRecord.clear();
    // 为每一个刹车检测点
    for (int i = 0; i < RoadMap.specialAreas.size(); ++i) {
        if (RoadMap.specialAreas[i].type == ZEBRA_CROSSING || RoadMap.specialAreas[i].type == BUS_STATION_AREA) {
            breakRecord.insert(pair<int, bool>(RoadMap.specialAreas[i].id, false));
        }
    }
    for (int i = 0; i < RoadMap.roads.size(); ++i) {
        breakRecord.insert(pair<int, bool>(RoadMap.roads[i].id, false));
    }
}

int ExecuteThroughExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
@@ -133,36 +155,57 @@
    return index;
}

static void ThroughZebraCrossing(struct road_exam2_map &map, const car_model *car)
void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    for (int i = 0; i < map.specialAreas.size(); i++) {
        if (map.specialAreas[i].character == 'zebra') {
            double distance = 0.0;
    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);

            if (IntersectionOf(extLine, map.specialAreas[i].startLine) == GM_Intersection &&
                    IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], map.specialAreas[i].startLine) == 1 ) {
                jj = true;
                if (ReadCarStatus(BREAK) == BREAK_ACTIVE) {
                    breakActive = 1;
                }
            } else {
                jj = false;
            // 计算点到左侧路边线的垂点
            int road = 0;
            for (road = 0; road < map.roads.size(); ++road) {
                if (map.roads[road].id == map.specialAreas[i].road)
                    break;
            }

            PointF vPoint = GetSELine(map.roads[road].leftEdge, map.specialAreas[i].area[0]);
            Line startLine;

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

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


            if (distance < LASTEST_BREAK_POINT) {
                if (ReadCarStatus(BREAK) == BREAK_ACTIVE) {
                    breakActive = 1;
            // 车头和斑马线距离不足30米
            if (IntersectionOf(extLine, startLine) == GM_Intersection &&
                    IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], startLine) == 1 ) {
                DEBUG("进入减速区");
                if (BreakDone == BREAK_ACTIVE) {
                    auto itx = breakRecord.find(map.specialAreas[i].id);
                    if (itx != breakRecord.end()) {
                        itx->second = true;
                    }
                }
            }

            // 跨线后，检查刹车动作
            if (CrashTheLine(startLine, car, CarModelList)) {
                auto itx = breakRecord.find(map.specialAreas[i].id);
                if (itx != breakRecord.end()) {
                    if (itx->second == false) {
                        // 不按规定减速，不合格
                        DEBUG("不按规定减速");
                    }
                    itx->second = false;
                }
            }
        } else if (map.specialAreas[i].type == SCHOOL_AREA) {

        }
    }
}