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