From 24428a1b69afcf113883e9cede1159443201a1b8 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期四, 02 四月 2020 18:57:07 +0800
Subject: [PATCH] 进出科目二区域调整

---
 lib/src/main/cpp/driver_test.cpp |  157 +++++++++++++++++++++++++++++++++-------------------
 1 files changed, 99 insertions(+), 58 deletions(-)

diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 0af9a65..9f47645 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -40,12 +40,6 @@
 
 using namespace std;
 
-#define MAP_TYPE_PARK_BUTTOM        1
-#define MAP_TYPE_STOP_START         2
-#define MAP_TYPE_PART_EDGE         3
-#define MAP_TYPE_CURVE         4
-#define MAP_TYPE_TURN_90         5
-
 #define RTK_INVALID         0
 #define RTK_SINGLE_POINT   1
 #define RTK_DIFF            2
@@ -117,7 +111,8 @@
 static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel);
 static bool UpdateCarCoord(double &spd, int &mov, int &idx);
 
-static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum);
+static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList);
+static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, const struct map_list *mapList, int mapNum);
 static bool ExitMap(const car_model *car, int mapId, const struct map_list *mapList, int mapNum);
 static int GetMapId(int index, const struct map_list *mapList, int mapNum);
 static int GetMapType(int index, const struct map_list *mapList, int mapNum);
@@ -650,7 +645,7 @@
                 RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);
 
         brief.qf = RtkBuffer[index].qf;
-        brief.map_id = GetMapId(CurrExamMapIndex, MapList, MapNum);
+        brief.map_id = -1;//GetMapId(CurrExamMapIndex, MapList, MapNum);
         brief.move = move;
         brief.speed = speed * 3.6;
         brief.heading = RtkBuffer[index].heading;
@@ -712,18 +707,18 @@
     }
     if (CurrExamMapIndex < 0) {
         if (CurrEnterMapIndex < 0) {
-            CurrEnterMapIndex = EnterMap(CarModel, MapList, MapNum);
+            CurrEnterMapIndex = EnterMap(CarModel, CarModelList, MapList, MapNum);
             if (CurrEnterMapIndex >= 0) {
-                DEBUG("鍙戦�佽繘鍏ュ満鍦版姤鍛� %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
-                MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1);
+//                DEBUG("鍙戦�佽繘鍏ュ満鍦版姤鍛� %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
+//                MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1);
 
                 CurrExamMapIndex = CurrEnterMapIndex;
                 CurrExamStatus = EXAM_AREA_START;
             }
         } else {
             if (ExitMap(CarModel, CurrEnterMapIndex, MapList, MapNum)) {
-                DEBUG("鍙戦�佺寮�鍦哄湴鎶ュ憡 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
-                MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0);
+//                DEBUG("鍙戦�佺寮�鍦哄湴鎶ュ憡 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
+//                MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0);
                 CurrEnterMapIndex = -1;
             }
         }
@@ -894,33 +889,33 @@
                     DEBUG("杩涘叆鍊掕溅鍏ュ簱鍦哄湴");
                     MA_SendDebugInfo("杩涘叆鍊掕溅鍏ュ簱鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
 
-                    StartParkBottom(move, rtkTime);
+                    StartParkBottom(CurrExamMapIndex, move, rtkTime);
                     CurrExamStatus = EXAM_AREA_RUN;
                     break;
                 case MAP_TYPE_STOP_START:
                     DEBUG("杩涘叆涓婂潯璧锋鍦哄湴");
                     MA_SendDebugInfo("杩涘叆涓婂潯璧锋鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
-                    StartSAS(move, rtkTime);
+                    StartSAS(CurrExamMapIndex, move, rtkTime);
                     CurrExamStatus = EXAM_AREA_RUN;
                     break;
                 case MAP_TYPE_PART_EDGE:
                     DEBUG("杩涘叆渚ф柟浣嶅仠杞﹀満鍦�");
                     MA_SendDebugInfo("杩涘叆渚ф柟浣嶅仠杞﹀満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
-                    StartParkEdge(move, rtkTime);
+                    StartParkEdge(CurrExamMapIndex, move, rtkTime);
 
                     CurrExamStatus = EXAM_AREA_RUN;
                     break;
                 case MAP_TYPE_CURVE:
                     DEBUG("杩涘叆鏇茬嚎琛岄┒鍦哄湴");
                     MA_SendDebugInfo("杩涘叆鏇茬嚎琛岄┒鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
-                    StartDrivingCurve(move, rtkTime);
+                    StartDrivingCurve(CurrExamMapIndex, move, rtkTime);
 
                     CurrExamStatus = EXAM_AREA_RUN;
                     break;
                 case MAP_TYPE_TURN_90:
                     DEBUG("杩涘叆鐩磋杞集鍦哄湴");
                     MA_SendDebugInfo("杩涘叆鐩磋杞集鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
-                    StartTurnA90(move, azimuth, rtkTime);
+                    StartTurnA90(CurrExamMapIndex, move, azimuth, rtkTime);
                     CurrExamStatus = EXAM_AREA_RUN;
                     break;
                 default:break;
@@ -1145,15 +1140,79 @@
     return true;
 }
 
-static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum)
+static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList)
 {
-    // 杞︾殑鏈�鍓嶇偣鏄惁杩涘叆鍦板浘
+    bool trigger = false;
+
+    if (CarModelList.size() < 5)
+        return trigger;
+
+    Polygon trace, trace2;
+    int pn = 0;
+
+    trace2.num = trace.num = 5;
+    trace.point = (PointF *) malloc(sizeof(PointF) * trace.num);
+    trace2.point = (PointF *) malloc(sizeof(PointF) * trace2.num);
+
+    list<car_model *>::iterator iter = CarModelList.begin();
+
+    car_model *c1 = *iter;
+
+    trace.point[pn] = c1->carXY[c1->left_front_tire[TIRE_OUTSIDE]];
+    trace2.point[pn++] = c1->carXY[c1->left_rear_tire[TIRE_OUTSIDE]];
+
+    ++iter;
+
+    while (iter != CarModelList.end() && pn < trace.num) {
+        car_model *c2 = *iter;
+
+        uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);
+
+        if (tdiff >= D_SEC(1)) {
+            trace.point[pn] = c2->carXY[c2->left_front_tire[TIRE_OUTSIDE]];
+            trace2.point[pn++] = c2->carXY[c2->left_rear_tire[TIRE_OUTSIDE]];
+            c1 = c2;
+        }
+        ++iter;
+    }
+
+    PointF p1, p2;
+
+    p1.X = triggerLine.X1;
+    p1.Y = triggerLine.Y1;
+    p2.X = triggerLine.X2;
+    p2.Y = triggerLine.Y2;
+
+    int pp = 0;
+    for (int p = 1; p < pn; ++p) {
+        Line trace_line, trace2_line;
+
+        MakeLine(&trace_line, &trace.point[pp], &trace.point[p]);
+        MakeLine(&trace2_line, &trace2.point[pp], &trace2.point[p]);
+
+        if ((IntersectionOf(trace_line, triggerLine) == GM_Intersection || IntersectionOf(trace2_line, triggerLine) == GM_Intersection) &&
+            IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 &&
+            DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], triggerLine) > 0.1) {
+            // 纰板埌瑙﹀彂绾�
+            DEBUG("纰版挒瑙﹀彂绾� 寮曞彂鍦板浘");
+            trigger = true;
+            goto SEARCH_TRIGGER_LINE_END;
+        }
+    }
+
+SEARCH_TRIGGER_LINE_END:
+    free(trace.point);
+
+    return trigger;
+}
+
+static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, const struct map_list *mapList, int mapNum)
+{
     for (int i = 0; i < mapNum && car != NULL; ++i) {
+        // 杞﹀墠杞垨鍚庤疆杞ㄨ抗瓒婅繃瑙﹀彂绾�
         if (mapList[i].type == MAP_TYPE_STOP_START) {
             // 鏋勯�犺櫄鎷熺殑宸︿笂瑙掔偣
             double x9, y9, xo, yo;
-
-            bool enter = false;
 
             xo = (mapList[i].map.point[0].X + mapList[i].map.point[7].X) / 2;
             yo = (mapList[i].map.point[0].Y + mapList[i].map.point[7].Y) / 2;
@@ -1161,31 +1220,18 @@
             x9 = 2*xo - mapList[i].map.point[8].X;
             y9 = 2*yo - mapList[i].map.point[8].Y;
 
-            Polygon map;
+            Line triggerLine;
 
-            map.num = 4;
-            map.point = (PointF *) malloc(map.num * sizeof(PointF));
+            triggerLine.X1 = mapList[i].map.point[0].X;
+            triggerLine.Y1 = mapList[i].map.point[0].Y;
+            triggerLine.X2 = x9;
+            triggerLine.Y2 = y9;
 
-            map.point[0] = mapList[i].map.point[0];
-            map.point[1] = mapList[i].map.point[8];
-            map.point[2] = mapList[i].map.point[7];
-            map.point[3].X = x9;
-            map.point[3].Y = y9;
-
-            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &map) == GM_Containment) {
-                Line enterLine1;
-
-                MakeLine(&enterLine1, &(map.point[0]), &(map.point[3]));
-
-                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
-                    enter = true;
-            }
-
-            free(map.point);
-
-            if (enter) return i;
+            if (CrashTriggerLine(triggerLine, car, CarModelList))
+                return i;
         }
         if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
+            // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴
             if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
                 Line enterLine1, enterLine2;
 
@@ -1198,6 +1244,7 @@
             }
         }
         if (mapList[i].type == MAP_TYPE_PART_EDGE) {
+            // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴
             if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
                 Line enterLine;
 
@@ -1208,26 +1255,20 @@
             }
         }
         if (mapList[i].type == MAP_TYPE_TURN_90) {
-            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
-                Line enterLine1;
+            // 杞﹀墠杞垨鍚庤疆杞ㄨ抗瓒婅繃瑙﹀彂绾�
+            Line triggerLine;
 
-                MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
+            MakeLine(&triggerLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
 
-                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
-                    return i;
-            }
+            if (CrashTriggerLine(triggerLine, car, CarModelList))
+                return i;
         }
         if (mapList[i].type == MAP_TYPE_CURVE) {
-            Line startLine;
-            Line carAxial;
+            Line triggerLine;
 
-            MakeLine(&startLine, &mapList[i].map.point[0], &mapList[i].map2.point[0]);
-            MakeLine(&carAxial, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]]);
-
-            if (IntersectionOf(startLine, carAxial) == GM_Intersection) {
-                if (DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], startLine) > 0.1)
-                    return i;
-            }
+            MakeLine(&triggerLine, &mapList[i].map2.point[0], &mapList[i].map.point[0]);
+            if (CrashTriggerLine(triggerLine, car, CarModelList))
+                return i;
         }
     }
     return -1;

--
Gitblit v1.8.0