From 91ed4c9dc7030f45d249192d430d9b56c43ca535 Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期四, 05 三月 2020 22:55:33 +0800
Subject: [PATCH] 角度修正。
---
lib/src/main/cpp/test_items/driving_curve.cpp | 102 +++++++++++++++++++++++++++++++++++++++-----------
1 files changed, 79 insertions(+), 23 deletions(-)
diff --git a/lib/src/main/cpp/test_items/driving_curve.cpp b/lib/src/main/cpp/test_items/driving_curve.cpp
index f2ceaec..6884863 100644
--- a/lib/src/main/cpp/test_items/driving_curve.cpp
+++ b/lib/src/main/cpp/test_items/driving_curve.cpp
@@ -35,6 +35,7 @@
} calcZone;
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct calc_zone_t *zone);
+static bool ExitArea(const Polygon *map, const Polygon *map2, const car_model *car);
void StartDrivingCurve(int moveDirect, const struct RtkTime *rtkTime)
{
@@ -59,8 +60,8 @@
MakeLine(&axial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);
- DEBUG("START 绾� %d -- %d", calcZone.leftStart, calcZone.rightStart);
- DEBUG("END 绾� %d -- %d", calcZone.leftEnd, 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]);
@@ -73,7 +74,7 @@
tempLeft = calcZone.leftStart - 1;
if (calcZone.rightStart > 0)
tempRight = calcZone.rightStart - 1;
- DEBUG("START 鍚戣捣鐐规煡鎵� %d -- %d", tempLeft, tempRight);
+// DEBUG("START 鍚戣捣鐐规煡鎵� %d -- %d", tempLeft, tempRight);
MakeLine(&start, &map->point[tempLeft], &map2->point[tempRight]);
if (IntersectionOf(start, axial) == GM_Intersection) {
@@ -85,21 +86,45 @@
if (calcZone.leftStart == calcZone.leftEnd && calcZone.rightStart == calcZone.rightEnd) {
// 杞﹁締涓㈠け锛岄噸鏂版悳绱�
- Line cat;
- int l = 0, r = 0;
+ calcZone.leftStart = calcZone.rightStart = 0;
+ calcZone.leftEnd = calcZone.rightEnd = 0;
- while (l < map->num || r < map2->num) {
- MakeLine(&cat, &map->point[l], &map2->point[r]);
+ DEBUG("杞﹁締涓㈠け锛岄噸鏂版悳绱�");
- if (IntersectionOf(cat, axial) == GM_Intersection) {
+ while (calcZone.leftStart < map->num || calcZone.rightStart < map2->num) {
+ MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);
+ if (IntersectionOf(start, axial) == GM_Intersection) {
+ while (calcZone.leftStart < map->num || calcZone.rightStart < map2->num) {
+ MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);
+ if (IntersectionOf(start, axial) == GM_None) {
+ break;
+ }
+ if (calcZone.leftStart < map->num)
+ calcZone.leftStart++;
+ if (calcZone.rightStart < map2->num)
+ calcZone.rightStart++;
+ }
+ break;
}
- if (l < map->num)
- l++;
- if (r < map2->num)
- r++;
+ calcZone.leftEnd = calcZone.leftStart;
+ calcZone.rightEnd = calcZone.rightStart;
+
+ if (calcZone.leftStart < map->num)
+ calcZone.leftStart++;
+ if (calcZone.rightStart < map2->num)
+ calcZone.rightStart++;
}
+
+ if (calcZone.leftStart >= map->num && calcZone.rightStart >= map2->num) {
+ // 绂诲紑鍦哄湴
+ DEBUG("绂诲紑鏇茬嚎鍦哄湴");
+ testing = false;
+ goto TEST_END;
+ }
+
+ break;
}
}
}
@@ -113,7 +138,7 @@
calcZone.leftStart++;
if (calcZone.rightStart < map2->num)
calcZone.rightStart++;
- DEBUG("START 鍚戠粓鐐规煡鎵� %d -- %d", calcZone.leftStart, calcZone.rightStart);
+// DEBUG("START 鍚戠粓鐐规煡鎵� %d -- %d", calcZone.leftStart, calcZone.rightStart);
MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);
} while (IntersectionOf(start, axial) == GM_Intersection);
}
@@ -131,7 +156,7 @@
tempLeft = calcZone.leftEnd + 1;
if (calcZone.rightEnd < map2->num)
tempRight = calcZone.rightEnd + 1;
- DEBUG("END 鍚戠粓鐐规煡鎵� %d -- %d", tempLeft, tempRight);
+// DEBUG("END 鍚戠粓鐐规煡鎵� %d -- %d", tempLeft, tempRight);
MakeLine(&end, &map->point[tempLeft], &map2->point[tempRight]);
if (IntersectionOf(end, axial) == GM_Intersection) {
@@ -152,18 +177,12 @@
calcZone.leftEnd--;
if (calcZone.rightEnd > 0)
calcZone.rightEnd--;
- DEBUG("END 鍚戣捣鐐规煡鎵� %d -- %d", calcZone.leftEnd, calcZone.rightEnd);
+// 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;
- }
if (CrashRedLine(map, map2, car, &calcZone)) {
if (!crashRedLine) {
@@ -197,7 +216,44 @@
}
}
+TEST_END:
return testing ? 1 : 0;
+}
+
+static bool ExitArea(const Polygon *map, const Polygon *map2, const car_model *car)
+{
+// 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
+ bool ret = false;
+
+ Polygon carBody;
+ Polygon bigMap;
+
+ bigMap.num = map->num + map2->num;
+ bigMap.point = (PointF *) malloc(bigMap.num * sizeof(PointF));
+
+ int i = 0;
+ for (; i < map->num; ++i) {
+ bigMap.point[i] = map->point[i];
+ }
+ for (int j = map2->num; j > 0; --j) {
+ bigMap.point[i++] = map2->point[j-1];
+ }
+
+ carBody.num = car->bodyNum;
+ carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
+
+ for (int i = 0; i < carBody.num; ++i) {
+ carBody.point[i] = car->carXY[car->body[i]];
+ }
+
+ if (IntersectionOf(&carBody, &bigMap) == GM_None) {
+ ret = true;
+ }
+
+ free(carBody.point);
+ free(bigMap.point);
+
+ return ret;
}
// 杞﹁疆鏄惁鍘嬭竟绾�
@@ -212,7 +268,7 @@
MakeLine(&rearTireAxial, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
int s = zone->leftStart;
- for (int e = zone->leftStart + 1; e < zone->leftEnd; ++e) {
+ for (int e = zone->leftStart - 1; e >= zone->leftEnd; --e) {
MakeLine(&redLine, &map->point[s], &map->point[e]);
if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
return true;
@@ -224,7 +280,7 @@
}
s = zone->rightStart;
- for (int e = zone->rightStart + 1; e < zone->rightEnd; ++e) {
+ for (int e = zone->rightStart - 1; e >= zone->rightEnd; --e) {
MakeLine(&redLine, &map2->point[s], &map2->point[e]);
if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
return true;
--
Gitblit v1.8.0