From 25bdbfbf6f4c9f1530be74fc194ad1dd0a13e8c9 Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期日, 08 三月 2020 23:12:49 +0800
Subject: [PATCH] 角度修正。
---
lib/src/main/cpp/test_items/driving_curve.cpp | 289 ++++++++++++++++++++++++++++++---------------------------
1 files changed, 154 insertions(+), 135 deletions(-)
diff --git a/lib/src/main/cpp/test_items/driving_curve.cpp b/lib/src/main/cpp/test_items/driving_curve.cpp
index 6884863..7a33a68 100644
--- a/lib/src/main/cpp/test_items/driving_curve.cpp
+++ b/lib/src/main/cpp/test_items/driving_curve.cpp
@@ -15,10 +15,6 @@
#define DEBUG(fmt, args...) LOGD("<driving_curve> <%s>: " fmt, __func__, ##args)
-enum {
- DRIVING_ON_CURVE
-};
-
const uint32_t STOP_CAR_TIME = D_SEC(2);
static bool testing = false;
@@ -27,15 +23,16 @@
static bool reportStopCarTimeout;
static int prevMoveDirect;
static bool crashRedLine;
-static struct calc_zone_t {
+static struct scan_window_t {
int leftStart;
int leftEnd;
int rightStart;
int rightEnd;
-} calcZone;
+} scanWindow;
-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);
+static bool UpdateStartLine(struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
+static bool UpdateEndLine(bool mode, struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
+static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone);
void StartDrivingCurve(int moveDirect, const struct RtkTime *rtkTime)
{
@@ -50,141 +47,31 @@
reportStopCarTimeout = false;
crashRedLine = false;
- calcZone.leftStart = calcZone.leftEnd = calcZone.rightStart = calcZone.rightEnd = 0;
+ scanWindow.leftStart = scanWindow.leftEnd = scanWindow.rightStart = scanWindow.rightEnd = 0;
}
int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
- Line start, end;
- Line axial;
+ Polygon tireRect;
- MakeLine(&axial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);
+ MakePolygon(&tireRect, {car->carXY[car->left_front_tire[TIRE_OUTSIDE]],
+ car->carXY[car->right_front_tire[TIRE_OUTSIDE]],
+ car->carXY[car->right_rear_tire[TIRE_OUTSIDE]],
+ car->carXY[car->left_rear_tire[TIRE_OUTSIDE]]});
-// 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) {
- // 鍚戣捣鐐规煡鎵�
- int tempLeft = calcZone.leftStart, tempRight = calcZone.rightStart;
-
- while (calcZone.leftStart > 0 || calcZone.rightStart > 0) {
- if (calcZone.leftStart > 0)
- tempLeft = calcZone.leftStart - 1;
- if (calcZone.rightStart > 0)
- 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) {
- // 杞﹁締涓㈠け锛岄噸鏂版悳绱�
- calcZone.leftStart = calcZone.rightStart = 0;
- calcZone.leftEnd = calcZone.rightEnd = 0;
-
- DEBUG("杞﹁締涓㈠け锛岄噸鏂版悳绱�");
-
- 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;
- }
-
- 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;
- }
- }
- }
- } else {
- // 鍚戠粓鐐规煡鎵�
- do {
- if (calcZone.leftStart >= map->num && calcZone.rightStart >= map2->num) {
- break;
- }
- if (calcZone.leftStart < map->num)
- calcZone.leftStart++;
- if (calcZone.rightStart < map2->num)
- 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);
+ // 鏇存柊杞﹀ご鎵弿绾�
+ if (!UpdateStartLine(&scanWindow, map, map2, &tireRect)) {
+ DEBUG("绂诲紑鍦哄湴");
+ testing = false;
+ goto TEST_END;
}
- MakeLine(&end, &map->point[calcZone.leftEnd], &map2->point[calcZone.rightEnd]);
+ // 鏇存柊杞﹀熬鎵弿绾�
+ UpdateEndLine(false, &scanWindow, map, map2, &tireRect);
- if (IntersectionOf(end, axial) == GM_None) {
- // 鍚戠粓鐐规煡鎵�
- 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)
- tempLeft = calcZone.leftEnd + 1;
- if (calcZone.rightEnd < map2->num)
- tempRight = calcZone.rightEnd + 1;
-// DEBUG("END 鍚戠粓鐐规煡鎵� %d -- %d", tempLeft, tempRight);
- MakeLine(&end, &map->point[tempLeft], &map2->point[tempRight]);
+ DEBUG("scanWindow leftStart %d leftEnd %d rightStart %d rightEnd %d", scanWindow.leftStart, scanWindow.leftEnd, scanWindow.rightStart, scanWindow.rightEnd);
- if (IntersectionOf(end, axial) == GM_Intersection) {
- // 淇濇寔涔嬪墠鐨勭嚎
- break;
- } else {
- calcZone.leftEnd = tempLeft;
- calcZone.rightEnd = tempRight;
- }
- }
- } else {
- // 鍚戣捣鐐规煡鎵�
- do {
- if (calcZone.leftEnd == 0 && calcZone.rightEnd == 0) {
- break;
- }
- if (calcZone.leftEnd > 0)
- calcZone.leftEnd--;
- if (calcZone.rightEnd > 0)
- 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 (CrashRedLine(map, map2, car, &calcZone)) {
+ if (CrashRedLine(map, map2, car, &scanWindow)) {
if (!crashRedLine) {
crashRedLine = true;
// 杞﹁疆鍘嬭竟绾匡紝涓嶅悎鏍�
@@ -217,10 +104,142 @@
}
TEST_END:
+ CleanPolygon(&tireRect);
return testing ? 1 : 0;
}
-static bool ExitArea(const Polygon *map, const Polygon *map2, const car_model *car)
+static bool UpdateStartLine(struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect)
+{
+ Line start;
+
+ bool update = true;
+ int direct = 0;
+ int tempLeft = scanWindow.leftStart, tempRight = scanWindow.rightStart;
+
+ while (update) {
+ update = false;
+ MakeLine(&start, &map->point[scanWindow.leftStart], &map2->point[scanWindow.rightStart]);
+
+ if (IntersectionOf(start, tireRect) == GM_None) {
+ if (direct != 1) {
+ direct = -1;
+ // 鍏ュ満鏂瑰悜鎵弿
+ tempLeft = scanWindow.leftStart;
+ tempRight = scanWindow.rightStart;
+
+ if (scanWindow.leftStart > 0) {
+ update = true;
+ scanWindow.leftStart--;
+ }
+ if (scanWindow.rightStart > 0) {
+ update = true;
+ scanWindow.rightStart--;
+ }
+
+ if (scanWindow.leftStart <= scanWindow.leftEnd && scanWindow.rightStart <= scanWindow.rightEnd) {
+ DEBUG("杞﹁締涓㈠け锛岄噸鏂版悳绱� %d %d", scanWindow.leftStart, scanWindow.rightStart);
+
+ // 杞﹁締涓㈠け锛岄噸鏂版悳绱�
+ update = false;
+ scanWindow.leftEnd = scanWindow.rightEnd = 0;
+
+ if (UpdateEndLine(true, &scanWindow, map, map2, tireRect)) {
+ DEBUG("鍖归厤鎴愬姛 %d %d", scanWindow.leftStart, scanWindow.leftEnd);
+ direct = 0;
+ update = true;
+ } else {
+ DEBUG("鍖归厤澶辫触");
+ return false;
+ }
+ }
+ }
+ } else {
+ if (direct != -1) {
+ // 鍑哄満鏂瑰悜鎵弿
+ direct = 1;
+ if (scanWindow.leftStart < map->num - 1) {
+ update = true;
+ scanWindow.leftStart++;
+ }
+ if (scanWindow.rightStart < map2->num - 1) {
+ update = true;
+ scanWindow.rightStart++;
+ }
+ } else {
+ scanWindow.leftStart = tempLeft;
+ scanWindow.rightStart = tempRight;
+ }
+ }
+ }
+
+ return true;
+}
+
+static bool UpdateEndLine(bool mode, struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect)
+{
+ bool update = true;
+ bool crash = false;
+
+ int direct = 0;
+ int tempLeft = zone->leftEnd;
+ int tempRight = zone->rightEnd;
+
+ Line end;
+
+ while (update) {
+ update = false;
+ MakeLine(&end, &map->point[zone->leftEnd], &map2->point[zone->rightEnd]);
+
+ if (IntersectionOf(end, tireRect) == GM_None) {
+ if (direct != -1) {
+ // 鍑哄満鏂瑰悜鎵弿
+ direct = 1;
+ tempLeft = zone->leftEnd;
+ tempRight = zone->rightEnd;
+
+ if (zone->leftEnd < map->num - 1) {
+ update = true;
+ zone->leftEnd++;
+ }
+ if (zone->rightEnd < map2->num - 1) {
+ update = true;
+ zone->rightEnd++;
+ }
+ }
+ } else {
+ if (!crash) {
+ crash = true;
+ if (mode) {
+ zone->leftStart = zone->leftEnd;
+ zone->rightStart = zone->rightEnd;
+
+
+ DEBUG("绗竴娆℃帴瑙� %d %d %d", zone->leftStart, zone->leftEnd, tempLeft);
+ }
+ }
+
+ if (direct != 1) {
+ // 鍏ュ満鏂瑰悜鎵弿
+ direct = -1;
+ if (zone->leftEnd > 0) {
+ update = true;
+ zone->leftEnd--;
+ }
+ if (zone->rightEnd > 0) {
+ update = true;
+ zone->rightEnd--;
+ }
+ } else {
+ zone->leftEnd = tempLeft;
+ zone->rightEnd = tempRight;
+ }
+ }
+ }
+
+ return crash;
+}
+
+bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model *car)
{
// 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
bool ret = false;
@@ -257,7 +276,7 @@
}
// 杞﹁疆鏄惁鍘嬭竟绾�
-static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct calc_zone_t *zone)
+static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone)
{
bool ret = false;
--
Gitblit v1.8.0