From 148a951e0d44577997fd2790f889dcdd7dbd6889 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期五, 17 四月 2020 17:53:18 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/road_exam.cpp | 150 +++++++++++++++++++++++++++++++------------------
1 files changed, 95 insertions(+), 55 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index ee1456b..de97518 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -983,9 +983,30 @@
}
return -1;
}
-#if 0
+
+#if 1
+
+typedef struct {
+ int road;
+ int segment;
+ int track;
+} CarOnTrackInfo_t;
+
+static CarOnTrackInfo_t CarOnTrackInfo;
+
void TestRoadGeneral(struct road_exam2_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
+ // 妫�娴嬭溅閬撳彉鏇�
+ CarOnTrackInfo_t newCarOnTrackInfo;
+
+ if (newCarOnTrackInfo.road == CarOnTrackInfo.road &&
+ newCarOnTrackInfo.segment == CarOnTrackInfo.segment &&
+ newCarOnTrackInfo.track != CarOnTrackInfo.track) {
+
+ }
+
+ CarOnTrackInfo = newCarOnTrackInfo;
+
}
@@ -1075,19 +1096,6 @@
if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
return ret;
- Polygon roadArea;
-
- roadArea.num = map.roads[currIndex].leftEdge.size() + map.roads[currIndex].rightEdge.size();;
- roadArea.point = (PointF *) malloc(roadArea.num * sizeof(PointF));;
-
- int n = 0;
- for (int j = 0; j < map.roads[currIndex].leftEdge.size(); j++) {
- roadArea.point[n++] = map.roads[i].leftEdge[j];
- }
- for (int j = map.roads[currIndex].rightEdge.size(); j > 0; j--) {
- roadArea.point[n++] = map.roads[i].rightEdge[j - 1];
- }
-
Polygon carBody;
carBody.num = car->bodyNum;
@@ -1096,12 +1104,11 @@
carBody.point[i] = car->carXY[car->body[i]];
}
- if (IntersectionOf(&carBody, &roadArea) == GM_None) {
+ if (IntersectionOf(&carBody, &map.roads[currIndex].area) == GM_None) {
ret = true;
}
free(carBody.point);
- free(roadArea.point);
return ret;
}
@@ -1134,61 +1141,94 @@
return false;
}
-struct {
- int road;
- int segment;
- int track;
-} CarOnTrackInfo;
static void DetectSeparate(int currIndex, struct road_exam2_map &map, const car_model *car)
{
int segment;
int track = -1;
+ CarOnTrackInfo_t newInfo;
+
if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
return;
+ // 閬嶅巻姣忎竴鍒嗘
for (int i = 0; i < map.roads[currIndex].separate.size(); i++) {
- if (map.roads[currIndex].separate[i].character == 0) {
- int separate_line_num = map.roads[currIndex].separate[i].line.size();
+ int separate_line_num = map.roads[currIndex].separate[i].lines.size();
- // 閬嶅巻褰撳墠鍒嗘鐨勬瘡涓�鏉＄嚎
- for (int j = 0; j < separate_line_num; ++j) {
- Line theLine;
- int p1 = 0;
- bool match_line = false;
- // 鍗曠嫭鐨勪竴鏉¤櫄绾�
- for (int p2 = 1; p2 < map.roads[currIndex].separate[i].line[j].num; ++p2) {
- MakeLine(&theLine, &map.roads[currIndex].separate[i].line[j].point[p1], &map.roads[currIndex].separate[i].line[j].point[p2]);
- if (p1 == 0 || p2 == map.roads[currIndex].separate[i].line[j].num - 1) {
- // 棣栧熬涓ょ锛屽簲閲囩敤寤堕暱绾跨殑鏂瑰紡
+ struct vrecord_ {
+ int valid;
+ int p1;
+ int p2;
+ };
+
+ vector<struct vrecord_> vrecord;
+ vrecord.clear();
+
+ bool match1 = false;
+ // 閬嶅巻褰撳墠鍒嗘鐨勬瘡涓�鏉＄嚎
+ for (int j = 0; j < separate_line_num; ++j) {
+ Line theLine;
+ int p1 = 0;
+
+ struct vrecord_ v;
+ v.valid = 0;
+
+ // 鍗曠嫭鐨勪竴鏉¤櫄绾�
+ for (int p2 = 1; p2 < map.roads[currIndex].separate[i].lines[j].num; ++p2) {
+ MakeLine(&theLine, &map.roads[currIndex].separate[i].lines[j].point[p1],
+ &map.roads[currIndex].separate[i].lines[j].point[p2]);
+ if (VerticalPointOnLine(car->basePoint, theLine)) {
+ v.valid = 1;
+ v.p1 = p1;
+ v.p2 = p2;
+ match1 = true;
+ break;
+ }
+ p1 = p2;
+ }
+
+ vrecord.push_back(v);
+
+ track = separate_line_num;//
+ }
+
+
+
+ if (match1) {
+ for (int x = 0; x < vrecord.size(); ++x) {
+ if (vrecord[i].valid == 0) {
+ // 棣栧熬涓ゆ绾跨殑寤剁敵蹇呮湁涓�涓瀭鐐�
+ if (DistanceOf(car->carXY[ car->axial[AXIAL_FRONT] ], map.roads[currIndex].separate[i].lines[x].point[0]) <
+ DistanceOf(car->carXY[ car->axial[AXIAL_FRONT] ], map.roads[currIndex].separate[i].lines[x].point[1])) {
+ vrecord[x].p1 = 0;
+ vrecord[x].p2 = 1;
+ } else {
+ vrecord[x].p1 = map.roads[currIndex].separate[i].lines[x].num - 2;
+ vrecord[x].p2 = map.roads[currIndex].separate[i].lines[x].num - 1;
}
- if (VerticalPointOnLine(car->basePoint, theLine)) {
- match_line = true;
- int rel = IntersectionOfLine(map.roads[currIndex].separate[i].line[j].point[p1],
- map.roads[currIndex].separate[i].line[j].point[p2],
- car->basePoint);
- // 璁板綍鎵�鍦ㄩ亾璺紝鍒嗛亾娈碉紝鍒嗛亾鎯呭喌
- if (rel == 1) { // 鍦ㄥ乏渚�
- segment = i;
- track = j;
- goto DETECT_SEPARATE_END;
- } else if (rel != -1) { // 鍦ㄧ嚎涓�
- segment = i;
- track = j;
- goto DETECT_SEPARATE_END;
- }
- break;
- }
- p1 = p2;
}
- track = separate_line_num;
+ int rel = IntersectionOfLine(map.roads[currIndex].separate[i].lines[x].point[vrecord[x].p1],
+ map.roads[currIndex].separate[i].lines[x].point[vrecord[x].p2],
+ car->basePoint);
+ if (rel != -1) {
+ newInfo.road = currIndex;
+ newInfo.segment = i;
+ newInfo.track = x;
+ break;
+ }
}
+
+ newInfo.road = currIndex;
+ newInfo.segment = i;
+ newInfo.track = vrecord.size();
+
+ break;
}
}
-
- DETECT_SEPARATE_END:
- ;
}
+
+
+
#endif
--
Gitblit v1.8.0