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 ++++++++++++++++++++++++++++++++---------
 1 files changed, 65 insertions(+), 19 deletions(-)

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