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