From 76859aa4b23ea8ebd90bd7553fd70e144bdc96ba Mon Sep 17 00:00:00 2001 From: yy1717 <fctom1215@outlook.com> Date: 星期五, 15 五月 2020 15:53:20 +0800 Subject: [PATCH] 坐标 --- lib/src/main/cpp/test_items2/through_something.cpp | 62 ++++++++++++++++++++++++++---- 1 files changed, 53 insertions(+), 9 deletions(-) diff --git a/lib/src/main/cpp/test_items2/through_something.cpp b/lib/src/main/cpp/test_items2/through_something.cpp index d9349e9..5a87311 100644 --- a/lib/src/main/cpp/test_items2/through_something.cpp +++ b/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) { + // 杞﹀ご鍜岃矾鍙h窛绂讳笉瓒�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 && -- Gitblit v1.8.0