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/through_something.cpp | 34 ++++++++
lib/src/main/cpp/test_items2/road_exam.cpp | 150 +++++++++++++++++++++++-------------
lib/src/main/cpp/test_common/Geometry.h | 3
lib/src/main/cpp/driver_test.h | 2
lib/src/main/cpp/test_common/Geometry.cpp | 29 +++++++
5 files changed, 161 insertions(+), 57 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index 3ca9f19..7d3d221 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -135,7 +135,7 @@
typedef struct {
int character; // 灞炴�с�婂疄绾裤�佽櫄绾裤��
- std::vector<Polygon> line; // 姣忎竴鏉＄嚎
+ std::vector<Polygon> lines; // 姣忎竴鏉＄嚎
} separate_t;
struct road_t {
diff --git a/lib/src/main/cpp/test_common/Geometry.cpp b/lib/src/main/cpp/test_common/Geometry.cpp
index ca4cd36..8c90d31 100644
--- a/lib/src/main/cpp/test_common/Geometry.cpp
+++ b/lib/src/main/cpp/test_common/Geometry.cpp
@@ -442,6 +442,18 @@
}
}
+int IntersectionOfLine(PointF p, Line line)
+{
+ PointF p1, p2;
+
+ p1.X = line.X1;
+ p1.Y = line.Y1;
+ p2.X = line.X2;
+ p2.Y = line.Y2;
+
+ IntersectionOfLine(p1, p2, p);
+}
+
/***************************************************************
* 寰楀埌p3浜巔1,p2缁勬垚鐨勭洿绾夸笂鐨勫瀭鐐�
* @param p1
@@ -568,3 +580,20 @@
return p3;
}
+
+/*******************************************************
+ * ori鐐瑰湪yaw鏂瑰悜涓婂欢闀跨殑璺濈
+ * @param ori
+ * @param length
+ * @param yaw
+ * @return
+ */
+PointF PointExtend(PointF ori, double length, double yaw)
+{
+ PointF ext;
+
+ ext.X = ori.X + length * sin(toRadians(yaw));
+ ext.Y = ori.Y + length * cos(toRadians(yaw));
+
+ return ext;
+}
diff --git a/lib/src/main/cpp/test_common/Geometry.h b/lib/src/main/cpp/test_common/Geometry.h
index ebfd47c..cfe73be 100644
--- a/lib/src/main/cpp/test_common/Geometry.h
+++ b/lib/src/main/cpp/test_common/Geometry.h
@@ -57,8 +57,9 @@
bool PartInsidePolygon(const Polygon *t1, const Polygon *t2);
bool OutsidePolygon(const Polygon *t1, const Polygon *t2);
int IntersectionOfLine(PointF p1, PointF p2, PointF p3);
+int IntersectionOfLine(PointF p, Line line);
PointF GetVerticalPoint(PointF p1, PointF p2, PointF p3);
bool VerticalPointOnLine(PointF point, Line line);
PointF Calc3Point(PointF p1, PointF p2, double L, char dir);
-
+PointF PointExtend(PointF ori, double length, double yaw);
#endif //GUI_GEOMETRY_H
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
diff --git a/lib/src/main/cpp/test_items2/through_something.cpp b/lib/src/main/cpp/test_items2/through_something.cpp
index b67e989..9461842 100644
--- a/lib/src/main/cpp/test_items2/through_something.cpp
+++ b/lib/src/main/cpp/test_items2/through_something.cpp
@@ -132,3 +132,37 @@
return index;
}
+
+static void ThroughZebraCrossing(struct road_exam2_map &map, const car_model *car)
+{
+ for (int i = 0; i < map.specialAreas.size(); i++) {
+ if (map.specialAreas[i].character == 'zebra') {
+ double distance = 0.0;
+
+ 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);
+
+ if (IntersectionOf(extLine, map.specialAreas[i].startLine) == GM_Intersection &&
+ IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], map.specialAreas[i].startLine) == 1 ) {
+ jj = true;
+ if (ReadCarStatus(BREAK) == BREAK_ACTIVE) {
+ breakActive = 1;
+ }
+ } else {
+ jj = false;
+ }
+
+
+
+
+
+ if (distance < LASTEST_BREAK_POINT) {
+ if (ReadCarStatus(BREAK) == BREAK_ACTIVE) {
+ breakActive = 1;
+ }
+ }
+ }
+ }
+}
--
Gitblit v1.8.0