From 7147f8046917a14e71f2330709e73aadb9844d30 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期四, 05 三月 2020 17:39:49 +0800
Subject: [PATCH] 更新地图
---
lib/src/main/cpp/test_items/driving_curve.cpp | 84 +++++++++++++++++++++------
lib/src/main/cpp/driver_test.cpp | 48 ++++++++--------
2 files changed, 89 insertions(+), 43 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index db93cc9..4ee19db 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -851,18 +851,18 @@
// if (mapList[i].type == MAP_TYPE_CURVE) {
//
// }
- if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
- if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
- Line enterLine1, enterLine2;
-
- MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
- MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
-
- if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
- DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
- return i;
- }
- }
+// if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
+// if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
+// Line enterLine1, enterLine2;
+//
+// MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
+// MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
+//
+// if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
+// DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
+// return i;
+// }
+// }
// if (mapList[i].type == MAP_TYPE_PART_EDGE) {
// if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
// Line enterLine;
@@ -883,18 +883,18 @@
// return i;
// }
// }
-// if (mapList[i].type == MAP_TYPE_CURVE) {
-// Line startLine;
-// Line carAxial;
-//
-// MakeLine(&startLine, &mapList[i].map.point[0], &mapList[i].map2.point[0]);
-// MakeLine(&carAxial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);
-//
-// if (IntersectionOf(startLine, carAxial) == GM_Intersection) {
-// if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], startLine) > 0.1)
-// return i;
-// }
-// }
+ if (mapList[i].type == MAP_TYPE_CURVE) {
+ Line startLine;
+ Line carAxial;
+
+ MakeLine(&startLine, &mapList[i].map.point[0], &mapList[i].map2.point[0]);
+ MakeLine(&carAxial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);
+
+ if (IntersectionOf(startLine, carAxial) == GM_Intersection) {
+ if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], startLine) > 0.1)
+ return i;
+ }
+ }
}
return -1;
}
diff --git a/lib/src/main/cpp/test_items/driving_curve.cpp b/lib/src/main/cpp/test_items/driving_curve.cpp
index a2bd5d0..f2ceaec 100644
--- a/lib/src/main/cpp/test_items/driving_curve.cpp
+++ b/lib/src/main/cpp/test_items/driving_curve.cpp
@@ -59,21 +59,50 @@
MakeLine(&axial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);
- MakeLine(&start, &map->point[calcZone.leftStart], &map->point[calcZone.rightStart]);
- MakeLine(&end, &map->point[calcZone.leftEnd], &map->point[calcZone.rightEnd]);
+ DEBUG("START 绾� %d -- %d", calcZone.leftStart, calcZone.rightStart);
+ DEBUG("END 绾� %d -- %d", calcZone.leftEnd, calcZone.rightEnd);
+
+ MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);
if (IntersectionOf(start, axial) == GM_None) {
// 鍚戣捣鐐规煡鎵�
- do {
- if (calcZone.leftStart == 0 && calcZone.rightStart == 0) {
- break;
- }
+ int tempLeft = calcZone.leftStart, tempRight = calcZone.rightStart;
+
+ while (calcZone.leftStart > 0 || calcZone.rightStart > 0) {
if (calcZone.leftStart > 0)
- calcZone.leftStart--;
+ tempLeft = calcZone.leftStart - 1;
if (calcZone.rightStart > 0)
- calcZone.rightStart--;
- MakeLine(&start, &map->point[calcZone.leftStart], &map->point[calcZone.rightStart]);
- } while (IntersectionOf(start, axial) == GM_None);
+ tempRight = calcZone.rightStart - 1;
+ DEBUG("START 鍚戣捣鐐规煡鎵� %d -- %d", tempLeft, tempRight);
+ MakeLine(&start, &map->point[tempLeft], &map2->point[tempRight]);
+
+ if (IntersectionOf(start, axial) == GM_Intersection) {
+ // 淇濇寔涔嬪墠鐨勭嚎
+ break;
+ } else {
+ calcZone.leftStart = tempLeft;
+ calcZone.rightStart = tempRight;
+
+ if (calcZone.leftStart == calcZone.leftEnd && calcZone.rightStart == calcZone.rightEnd) {
+ // 杞﹁締涓㈠け锛岄噸鏂版悳绱�
+ Line cat;
+ int l = 0, r = 0;
+
+ while (l < map->num || r < map2->num) {
+ MakeLine(&cat, &map->point[l], &map2->point[r]);
+
+ if (IntersectionOf(cat, axial) == GM_Intersection) {
+
+ }
+
+ if (l < map->num)
+ l++;
+ if (r < map2->num)
+ r++;
+ }
+ }
+ }
+ }
} else {
// 鍚戠粓鐐规煡鎵�
do {
@@ -84,22 +113,35 @@
calcZone.leftStart++;
if (calcZone.rightStart < map2->num)
calcZone.rightStart++;
- MakeLine(&start, &map->point[calcZone.leftStart], &map->point[calcZone.rightStart]);
- } while (IntersectionOf(start, axial) != GM_None);
+ DEBUG("START 鍚戠粓鐐规煡鎵� %d -- %d", calcZone.leftStart, calcZone.rightStart);
+ MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);
+ } while (IntersectionOf(start, axial) == GM_Intersection);
}
+
+ MakeLine(&end, &map->point[calcZone.leftEnd], &map2->point[calcZone.rightEnd]);
if (IntersectionOf(end, axial) == GM_None) {
// 鍚戠粓鐐规煡鎵�
- do {
+ int tempLeft = calcZone.leftEnd, tempRight = calcZone.rightEnd;
+ while (calcZone.leftEnd < map->num || calcZone.rightEnd < map2->num) {
if (calcZone.leftEnd >= map->num && calcZone.rightEnd >= map2->num) {
break;
}
if (calcZone.leftEnd < map->num)
- calcZone.leftEnd++;
+ tempLeft = calcZone.leftEnd + 1;
if (calcZone.rightEnd < map2->num)
- calcZone.rightEnd++;
- MakeLine(&end, &map->point[calcZone.leftEnd], &map->point[calcZone.rightEnd]);
- } while (IntersectionOf(end, axial) == GM_None);
+ tempRight = calcZone.rightEnd + 1;
+ DEBUG("END 鍚戠粓鐐规煡鎵� %d -- %d", tempLeft, tempRight);
+ MakeLine(&end, &map->point[tempLeft], &map2->point[tempRight]);
+
+ if (IntersectionOf(end, axial) == GM_Intersection) {
+ // 淇濇寔涔嬪墠鐨勭嚎
+ break;
+ } else {
+ calcZone.leftEnd = tempLeft;
+ calcZone.rightEnd = tempRight;
+ }
+ }
} else {
// 鍚戣捣鐐规煡鎵�
do {
@@ -110,12 +152,16 @@
calcZone.leftEnd--;
if (calcZone.rightEnd > 0)
calcZone.rightEnd--;
- MakeLine(&end, &map->point[calcZone.leftEnd], &map->point[calcZone.rightEnd]);
- } while (IntersectionOf(end, axial) == GM_None);
+ DEBUG("END 鍚戣捣鐐规煡鎵� %d -- %d", calcZone.leftEnd, calcZone.rightEnd);
+ MakeLine(&end, &map->point[calcZone.leftEnd], &map2->point[calcZone.rightEnd]);
+ } while (IntersectionOf(end, axial) == GM_Intersection);
}
+
+ DEBUG("calcZone leftStart %d leftEnd %d rightStart %d rightEnd %d", calcZone.leftStart, calcZone.leftEnd, calcZone.rightStart, calcZone.rightEnd);
if (calcZone.leftStart <= calcZone.leftEnd || calcZone.rightStart <= calcZone.rightEnd) {
// 绂诲紑鍦哄湴
+ DEBUG("绂诲紑鏇茬嚎鍦哄湴");
testing = false;
}
--
Gitblit v1.8.0