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