From efc508204e781a1ce1d3f55c2b986198c9e32509 Mon Sep 17 00:00:00 2001 From: yy1717 <fctom1215@outlook.com> Date: 星期二, 18 八月 2020 17:35:26 +0800 Subject: [PATCH] 坐标 --- lib/src/main/cpp/test_items2/road_exam.cpp | 35 +++++++++++++++++++++++++++++++++++ 1 files changed, 35 insertions(+), 0 deletions(-) diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp index a1e3357..8bb5cea 100644 --- a/lib/src/main/cpp/test_items2/road_exam.cpp +++ b/lib/src/main/cpp/test_items2/road_exam.cpp @@ -51,6 +51,7 @@ int road; int sep; int no; + int total; int guide; } lane_t; @@ -173,6 +174,8 @@ static bool AllCmp(road_exam_map &map); static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car); + +void AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car); void InitRoadExam(road_exam_map &RoadMap) { @@ -768,6 +771,7 @@ lane.road = roadIndex; lane.sep = i; lane.no = CLP.size(); + lane.total = CLP.size() + 1; for (int x = 0; x < CLP.size(); ++x) { if (CLP[x].pos == 'L') { @@ -779,6 +783,12 @@ return true; } } + + lane.road = roadIndex; + lane.sep = 0; + lane.no = 0; + lane.total = 0; + lane.guide = 0; return false; } @@ -1380,6 +1390,8 @@ oldid = Lane.guide; DetectLane(RoadMap, car, currExamMapIndex, rtkTime); + +// DEBUG("Lane淇℃伅 road %d sep %d total %d no %d guide %d", Lane.road, Lane.sep, Lane.total, Lane.no, Lane.guide); if (Lane.guide > 0 && currExamMapIndex >= 0) { int stop_line; @@ -1394,6 +1406,10 @@ if (Lane.guide != oldid) { DEBUG("瀵煎悜绫诲瀷鍒囨崲 %d", Lane.guide); + } + + if (currExamMapIndex >= 0) { + AnalysisRoad(RoadMap, currExamMapIndex, Lane, car); } // 妫�娴嬪帇绾跨姸鎬� @@ -1792,3 +1808,22 @@ } return true; } + +void AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car) +{ + double distance = 0; + + if (roadIndex < 0 || roadIndex >= RoadMap.roads.size()) + return; + + for (int i = 0; i < RoadMap.roads[roadIndex].rightEdge.size(); ++i) { + for (int j = 1; j < RoadMap.roads[roadIndex].rightEdge[i].points.size(); ++j) { + PointF point = RoadMap.roads[roadIndex].rightEdge[i].points[j]; + distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, RoadMap.roads[roadIndex].rightEdge); + if (distance > 1e-3) { + DEBUG("鐩撮亾鍓╀綑璺濈 %f", distance); + return; + } + } + } +} -- Gitblit v1.8.0