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