From 576d819c4d9aabd1a8d102ae9c3bc13c9165ee1f Mon Sep 17 00:00:00 2001 From: yy1717 <fctom1215@outlook.com> Date: 星期五, 14 八月 2020 19:15:45 +0800 Subject: [PATCH] 坐标 --- lib/src/main/cpp/test_items2/through_something.cpp | 308 ++++++++++++++++++++++++++++++-------------------- 1 files changed, 183 insertions(+), 125 deletions(-) diff --git a/lib/src/main/cpp/test_items2/through_something.cpp b/lib/src/main/cpp/test_items2/through_something.cpp index 9c70cff..c86aaec 100644 --- a/lib/src/main/cpp/test_items2/through_something.cpp +++ b/lib/src/main/cpp/test_items2/through_something.cpp @@ -12,151 +12,209 @@ #include "../jni_log.h" #include "../test_common/car_sensor.h" #include "road_exam.h" +#include "../utils/xconvert.h" #define DEBUG(fmt, args...) LOGD("<through_something> <%s>: " fmt, __func__, ##args) -#if 0 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 map <int, int> TargetReduceRec; +static map <int, int> TargetReduceRec2; + +#define NOT_ENTER 1 +#define ENTER_Z 2 +#define REDUCE_SPEED 4 +#define STOP_CAR 8 +#define OVER_SPEED 16 + +static const double DISTANCE_STOP_CAR_TO_STOP_LINE = 3.0; static const double PASS_SCHOOL_MAX_SPEED = 30.0; // kmh +static const double LASTEST_BREAK_POINT = 30.0; -static int breakActive; -static int stopActive; -static bool crashRedLine; - - - -void StartThroughExam(int index, LIST_ROAD_MAP &RoadMapList) +static void SetTargetReduceRec(map<int, int> &table, int key, int status) { - if (index == -1) + auto it = table.find(key); + + if (it != table.end()) { + table.erase(it); + } + table.insert(pair<int, int>(key, status)); +} + +static int GetTargetReduceRec(map<int, int> &table, int key) +{ + auto it = table.find(key); + + if (it != table.end()) { + return it->second; + } + return NOT_ENTER; +} + +static void RemoveTargetReduceRec(map<int, int> &table, int key) +{ + auto it = table.find(key); + + if (it != table.end()) { + table.erase(it); + } +} + +void ResetTarget(road_exam_map &RoadMap) +{ + TargetReduceRec.clear(); + TargetReduceRec2.clear(); +} + +void ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime) +{ + if (roadIndex < 0 || roadIndex >= RoadMap.roads.size()) return; - DEBUG("杩涘叆璺�冮�氳繃something鍦板浘 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type); - if (!RoadMapList[index].tts.empty()) { - DEBUG("鎾斁TTS %s", RoadMapList[index].tts.c_str()); - PlayTTS(RoadMapList[index].tts.c_str()); - } else { - DEBUG("娌℃湁TTS"); - } - breakActive = 0; - stopActive = 0; - crashRedLine = false; -} + // 璺彛 + for (int i = 0; i < RoadMap.roads[roadIndex].stopLine.size(); ++i) { + PointF point; + double distance; -void InitThroughSomething(road_exam_map &RoadMap) -{ - breakRecord.clear(); - // 涓烘瘡涓�涓埞杞︽娴嬬偣 - for (int i = 0; i < RoadMap.specialAreas.size(); ++i) { + point.X = RoadMap.roads[roadIndex].stopLine[i].line.X1; + point.Y = RoadMap.roads[roadIndex].stopLine[i].line.Y1; + + distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, RoadMap.roads[roadIndex].rightEdge); + + int key = roadIndex * 100 + i; + int rec = GetTargetReduceRec(TargetReduceRec, key); + + if (distance > 1e-3 && distance < LASTEST_BREAK_POINT) { + if (rec == NOT_ENTER) { + SetTargetReduceRec(TargetReduceRec, key, ENTER_Z); + } + // 璁板綍鍒硅溅 + if (dobreak && !(rec & REDUCE_SPEED)) { + SetTargetReduceRec(TargetReduceRec, key, rec | REDUCE_SPEED); + } + } else if (distance > 1e-3 && distance < DISTANCE_STOP_CAR_TO_STOP_LINE) { + // 璺彛鍋滆溅瑙傚療 + if (moveDirect == 0 && !(rec & STOP_CAR)) { + SetTargetReduceRec(TargetReduceRec, key, rec | STOP_CAR); + } + } else if (distance > LASTEST_BREAK_POINT + 5 && rec != NOT_ENTER) { + RemoveTargetReduceRec(TargetReduceRec, key); + } + } + // 浜鸿閬撱�佸叕浜ょ珯銆佸鏍� + 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 (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)); - } -} + double distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge); -int ExecuteThroughExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, - LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) -{ - PointF p1, p2; + int key = i; + int rec = GetTargetReduceRec(TargetReduceRec2, key); - double distance2StopLine = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], - RoadMapList[index].stopLine); - p1.X = RoadMapList[index].stopLine.X1; - p1.Y = RoadMapList[index].stopLine.Y1; - p2.X = RoadMapList[index].stopLine.X2; - p2.Y = RoadMapList[index].stopLine.Y2; - - if (IntersectionOfLine(p1, p2, car->carXY[car->axial[AXIAL_FRONT]]) == -1) { - distance2StopLine = -distance2StopLine; - } - - // 璺濈鍋滄绾�30绫冲唴鏄惁鏈夊埞杞﹀姩浣� - if (breakActive == 0) { - if (distance2StopLine > 0 && distance2StopLine <= LASTEST_BREAK_POINT) { - if (ReadCarStatus(BREAK) == BREAK_ACTIVE) { - breakActive = 1; + if (distance > 1e-3 && distance < LASTEST_BREAK_POINT) { + if (rec == NOT_ENTER) { + SetTargetReduceRec(TargetReduceRec2, key, ENTER_Z); + } + // 璁板綍鍒硅溅锛屽仠杞� + if (dobreak && !(rec & REDUCE_SPEED)) { + SetTargetReduceRec(TargetReduceRec2, key, rec | REDUCE_SPEED); + } + } else if (distance > LASTEST_BREAK_POINT + 5 && rec != NOT_ENTER) { + RemoveTargetReduceRec(TargetReduceRec2, key); } - } + } else if (RoadMap.specialAreas[i].type == SCHOOL_AREA) { + double distance1 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge); + double distance2 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[1], RoadMap.roads[roadIndex].rightEdge); + int key = i; + int rec = GetTargetReduceRec(TargetReduceRec2, key); - if (distance2StopLine < 0 && breakActive != 1){ - breakActive = -1; - // 涓嶆寜瑙勫畾鍑忛�燂紝涓嶅悎鏍� - DEBUG("涓嶈俯涓嬪埞杞︼紝涓嶅悎鏍�"); - switch (RoadMapList[index].type) { - case THROUGH_INTERSECTION_MAP: - AddExamFault(41, rtkTime); - break; - case TURN_LEFT_MAP: - AddExamFault(43, rtkTime); - break; - case TURN_RIGHT_MAP: - AddExamFault(46, rtkTime); - break; - case THROUGH_ZEBRA_CROSSING_MAP: - AddExamFault(48, rtkTime); - break; - case THROUGH_SCHOOL_MAP: + if (distance1 < -1e-3 && distance2 > 1e-3) { + if (rec == NOT_ENTER) { + SetTargetReduceRec(TargetReduceRec2, key, ENTER_Z); + } + if (ConvertMs2KMh(speed) > PASS_SCHOOL_MAX_SPEED && !(rec & OVER_SPEED)) { + SetTargetReduceRec(TargetReduceRec2, key, rec | OVER_SPEED); + + DEBUG("閫氳繃瀛︽牎鍖哄煙瓒呴��"); AddExamFault(49, rtkTime); - break; - case THROUGH_BUS_STATION_MAP: - AddExamFault(50, rtkTime); - break; - default: - break; + } + } else if (rec != NOT_ENTER) { + RemoveTargetReduceRec(TargetReduceRec2, key); } } } - - // 鏈夊仠姝㈡爣璁扮殑锛屾槸鍚﹀仠杞︼紙鍋滄绾�5绫冲唴鏈夊仠杞﹀姩浣滐級 - if (RoadMapList[index].flagStop != 0 && stopActive == 0) { - if (distance2StopLine < 0.0) { - // 涓嶅仠杞︾灜鏈涳紝涓嶅悎鏍� - DEBUG("涓嶅仠杞︾灜鏈�"); - switch (RoadMapList[index].type) { - case THROUGH_INTERSECTION_MAP: - AddExamFault(42, rtkTime); - break; - case TURN_LEFT_MAP: - AddExamFault(44, rtkTime); - break; - case TURN_RIGHT_MAP: - AddExamFault(47, rtkTime); - break; - default: - break; - } - stopActive = -1; - } else if (distance2StopLine < DISTANCE_STOP_CAR_TO_STOP_LINE) { - if (moveDirect == 0) { - stopActive = 1; - } - } - } - - if (CrashSonRedLine(index, RoadMapList, car, CarModelList)) { - if (!crashRedLine) { - DEBUG("涓嶆寜鑰冭瘯鍛樻寚浠よ椹�"); - crashRedLine = true; - // 涓嶆寜鑰冭瘯鍛樻寚浠よ椹� - AddExamFault(3, rtkTime); - } - } else { - crashRedLine = false; - } - - if (ExitSonArea(index, RoadMapList, car)) { - DEBUG("绂诲紑閫氳繃something鍖哄煙"); - return -1; - } - - return index; } -#endif +void ExitTarget(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, const struct RtkTime *rtkTime) +{ + RECHECK: + for (auto it = TargetReduceRec.begin(); it != TargetReduceRec.end(); ++it) { + int road = it->first / 100; + int x = it->first % 100; + if (CrashTheLine(RoadMap.roads[road].stopLine[x].line, car, CarModelList)) { + DEBUG("瓒婅繃璺彛 road %d 璺彛 %d", road, x); + if (!(it->second & REDUCE_SPEED)) { + // 涓嶆寜瑙勫畾鍑忛�燂紝涓嶅悎鏍� + DEBUG("涓嶆寜瑙勫畾鍑忛��"); + if (RoadMap.roads[road].stopLine[x].active == ROAD_ACTIVE_FORWARD) { + AddExamFault(41, rtkTime); + } else if (RoadMap.roads[road].stopLine[x].active == + ROAD_ACTIVE_TURN_LEFT) { + AddExamFault(43, rtkTime); + } else if (RoadMap.roads[road].stopLine[x].active == + ROAD_ACTIVE_TURN_RIGHT) { + AddExamFault(46, rtkTime); + } + } + if (!(it->second & STOP_CAR) && RoadMap.roads[road].stopLine[x].stopFlag) { + // 涓嶅仠杞︾灜鏈涳紝涓嶅悎鏍� + DEBUG("涓嶅仠杞︾灜鏈�"); + if (RoadMap.roads[road].stopLine[x].active == ROAD_ACTIVE_FORWARD) { + AddExamFault(42, rtkTime); + } else if (RoadMap.roads[road].stopLine[x].active == + ROAD_ACTIVE_TURN_LEFT) { + AddExamFault(44, rtkTime); + } else if (RoadMap.roads[road].stopLine[x].active == + ROAD_ACTIVE_TURN_RIGHT) { + AddExamFault(47, rtkTime); + } + } + + RemoveTargetReduceRec(TargetReduceRec, it->first); + goto RECHECK; + } + } + + RECHECK2: + for (auto it = TargetReduceRec2.begin(); it != TargetReduceRec2.end(); ++it) { + int x = it->first; + Line line; + int roadIndex = 0; + + for (; roadIndex < RoadMap.roads.size(); ++roadIndex) { + if (RoadMap.roads[roadIndex].id == RoadMap.specialAreas[x].road) + break; + } + + PointF point2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].leftEdge, + RoadMap.specialAreas[x].area[1]); + MakeLine(&line, &RoadMap.specialAreas[x].area[1], &point2); + + if (CrashTheLine(line, car, CarModelList)) { + if (RoadMap.specialAreas[x].type == ZEBRA_CROSSING && + !(it->second & REDUCE_SPEED)) { + DEBUG("涓嶆寜瑙勫畾鍑忛��"); + AddExamFault(48, rtkTime); + } + if (RoadMap.specialAreas[x].type == BUS_STATION_AREA && + !(it->second & REDUCE_SPEED)) { + DEBUG("涓嶆寜瑙勫畾鍑忛��"); + AddExamFault(50, rtkTime); + } + RemoveTargetReduceRec(TargetReduceRec2, it->first); + goto RECHECK2; + } + } +} -- Gitblit v1.8.0