From 576d819c4d9aabd1a8d102ae9c3bc13c9165ee1f Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期五, 14 八月 2020 19:15:45 +0800
Subject: [PATCH] 坐标

---
 lib/src/main/cpp/test_items2/through_something.cpp |  308 ++++++++++++++++++++++++++++++--------------------
 1 files changed, 183 insertions(+), 125 deletions(-)

diff --git a/lib/src/main/cpp/test_items2/through_something.cpp b/lib/src/main/cpp/test_items2/through_something.cpp
index 9c70cff..c86aaec 100644
--- a/lib/src/main/cpp/test_items2/through_something.cpp
+++ b/lib/src/main/cpp/test_items2/through_something.cpp
@@ -12,151 +12,209 @@
 #include "../jni_log.h"
 #include "../test_common/car_sensor.h"
 #include "road_exam.h"
+#include "../utils/xconvert.h"
 
 #define DEBUG(fmt, args...)     LOGD("<through_something> <%s>: " fmt, __func__, ##args)
 
-#if 0
 using namespace std;
 
-static const double LASTEST_BREAK_POINT = 30.0;
-static const double NEXT_ROAD_TIP = 100.0;                  // 鍒拌揪璺彛鍓嶆彁绀轰笅涓�涓�庝箞璧�
-static const double DISTANCE_STOP_CAR_TO_STOP_LINE = 5.0;
+static map <int, int> TargetReduceRec;
+static map <int, int> TargetReduceRec2;
+
+#define NOT_ENTER           1
+#define ENTER_Z             2
+#define REDUCE_SPEED        4
+#define STOP_CAR            8
+#define OVER_SPEED          16
+
+static const double DISTANCE_STOP_CAR_TO_STOP_LINE = 3.0;
 static const double PASS_SCHOOL_MAX_SPEED = 30.0;           // kmh
+static const double LASTEST_BREAK_POINT = 30.0;
 
-static int breakActive;
-static int stopActive;
-static bool crashRedLine;
-
-
-
-void StartThroughExam(int index, LIST_ROAD_MAP &RoadMapList)
+static void SetTargetReduceRec(map<int, int> &table, int key, int status)
 {
-    if (index == -1)
+    auto it = table.find(key);
+
+    if (it != table.end()) {
+        table.erase(it);
+    }
+    table.insert(pair<int, int>(key, status));
+}
+
+static int GetTargetReduceRec(map<int, int> &table, int key)
+{
+    auto it = table.find(key);
+
+    if (it != table.end()) {
+        return it->second;
+    }
+    return NOT_ENTER;
+}
+
+static void RemoveTargetReduceRec(map<int, int> &table, int key)
+{
+    auto it = table.find(key);
+
+    if (it != table.end()) {
+        table.erase(it);
+    }
+}
+
+void ResetTarget(road_exam_map &RoadMap)
+{
+    TargetReduceRec.clear();
+    TargetReduceRec2.clear();
+}
+
+void ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime)
+{
+    if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
         return;
-    DEBUG("杩涘叆璺�冮�氳繃something鍦板浘 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type);
-    if (!RoadMapList[index].tts.empty()) {
-        DEBUG("鎾斁TTS %s", RoadMapList[index].tts.c_str());
-        PlayTTS(RoadMapList[index].tts.c_str());
-    } else {
-        DEBUG("娌℃湁TTS");
-    }
-    breakActive = 0;
-    stopActive = 0;
-    crashRedLine = false;
-}
+    // 璺彛
+    for (int i = 0; i < RoadMap.roads[roadIndex].stopLine.size(); ++i) {
+        PointF point;
+        double distance;
 
-void InitThroughSomething(road_exam_map &RoadMap)
-{
-    breakRecord.clear();
-    // 涓烘瘡涓�涓埞杞︽娴嬬偣
-    for (int i = 0; i < RoadMap.specialAreas.size(); ++i) {
+        point.X = RoadMap.roads[roadIndex].stopLine[i].line.X1;
+        point.Y = RoadMap.roads[roadIndex].stopLine[i].line.Y1;
+
+        distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, RoadMap.roads[roadIndex].rightEdge);
+
+        int key = roadIndex * 100 + i;
+        int rec = GetTargetReduceRec(TargetReduceRec, key);
+
+        if (distance > 1e-3 && distance < LASTEST_BREAK_POINT) {
+            if (rec == NOT_ENTER) {
+                SetTargetReduceRec(TargetReduceRec, key, ENTER_Z);
+            }
+            // 璁板綍鍒硅溅
+            if (dobreak && !(rec & REDUCE_SPEED)) {
+                SetTargetReduceRec(TargetReduceRec, key, rec | REDUCE_SPEED);
+            }
+        } else if (distance > 1e-3 && distance < DISTANCE_STOP_CAR_TO_STOP_LINE) {
+            // 璺彛鍋滆溅瑙傚療
+            if (moveDirect == 0 && !(rec & STOP_CAR)) {
+                SetTargetReduceRec(TargetReduceRec, key, rec | STOP_CAR);
+            }
+        } else if (distance > LASTEST_BREAK_POINT + 5 && rec != NOT_ENTER) {
+            RemoveTargetReduceRec(TargetReduceRec, key);
+        }
+    }
+    // 浜鸿閬撱�佸叕浜ょ珯銆佸鏍�
+    for (int i = 0; i < RoadMap.specialAreas.size(); i++) {
+        if (RoadMap.specialAreas[i].type == GRID_AREA || RoadMap.specialAreas[i].road != RoadMap.roads[roadIndex].id)
+            continue;
+
         if (RoadMap.specialAreas[i].type == ZEBRA_CROSSING || RoadMap.specialAreas[i].type == BUS_STATION_AREA) {
-            breakRecord.insert(pair<int, bool>(RoadMap.specialAreas[i].id, false));
-        }
-    }
-    for (int i = 0; i < RoadMap.roads.size(); ++i) {
-        breakRecord.insert(pair<int, bool>(RoadMap.roads[i].id, false));
-    }
-}
+            double distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge);
 
-int ExecuteThroughExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
-        LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
-{
-    PointF p1, p2;
+            int key =  i;
+            int rec = GetTargetReduceRec(TargetReduceRec2, key);
 
-    double distance2StopLine = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]],
-                                          RoadMapList[index].stopLine);
-    p1.X = RoadMapList[index].stopLine.X1;
-    p1.Y = RoadMapList[index].stopLine.Y1;
-    p2.X = RoadMapList[index].stopLine.X2;
-    p2.Y = RoadMapList[index].stopLine.Y2;
-
-    if (IntersectionOfLine(p1, p2, car->carXY[car->axial[AXIAL_FRONT]]) == -1) {
-        distance2StopLine = -distance2StopLine;
-    }
-
-    // 璺濈鍋滄绾�30绫冲唴鏄惁鏈夊埞杞﹀姩浣�
-    if (breakActive == 0) {
-        if (distance2StopLine > 0 && distance2StopLine <= LASTEST_BREAK_POINT) {
-            if (ReadCarStatus(BREAK) == BREAK_ACTIVE) {
-                breakActive = 1;
+            if (distance > 1e-3 && distance < LASTEST_BREAK_POINT) {
+                if (rec == NOT_ENTER) {
+                    SetTargetReduceRec(TargetReduceRec2, key, ENTER_Z);
+                }
+                // 璁板綍鍒硅溅锛屽仠杞�
+                if (dobreak && !(rec & REDUCE_SPEED)) {
+                    SetTargetReduceRec(TargetReduceRec2, key, rec | REDUCE_SPEED);
+                }
+            } else if (distance > LASTEST_BREAK_POINT + 5 && rec != NOT_ENTER) {
+                RemoveTargetReduceRec(TargetReduceRec2, key);
             }
-        }
+        } else if (RoadMap.specialAreas[i].type == SCHOOL_AREA) {
+            double distance1 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge);
+            double distance2 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[1], RoadMap.roads[roadIndex].rightEdge);
+            int key =  i;
+            int rec = GetTargetReduceRec(TargetReduceRec2, key);
 
-        if (distance2StopLine < 0 && breakActive != 1){
-            breakActive = -1;
-            // 涓嶆寜瑙勫畾鍑忛�燂紝涓嶅悎鏍�
-            DEBUG("涓嶈俯涓嬪埞杞︼紝涓嶅悎鏍�");
-            switch (RoadMapList[index].type) {
-                case THROUGH_INTERSECTION_MAP:
-                    AddExamFault(41, rtkTime);
-                    break;
-                case TURN_LEFT_MAP:
-                    AddExamFault(43, rtkTime);
-                    break;
-                case TURN_RIGHT_MAP:
-                    AddExamFault(46, rtkTime);
-                    break;
-                case THROUGH_ZEBRA_CROSSING_MAP:
-                    AddExamFault(48, rtkTime);
-                    break;
-                case THROUGH_SCHOOL_MAP:
+            if (distance1 < -1e-3 && distance2 > 1e-3) {
+                if (rec == NOT_ENTER) {
+                    SetTargetReduceRec(TargetReduceRec2, key, ENTER_Z);
+                }
+                if (ConvertMs2KMh(speed) > PASS_SCHOOL_MAX_SPEED && !(rec & OVER_SPEED)) {
+                    SetTargetReduceRec(TargetReduceRec2, key, rec | OVER_SPEED);
+
+                    DEBUG("閫氳繃瀛︽牎鍖哄煙瓒呴��");
                     AddExamFault(49, rtkTime);
-                    break;
-                case THROUGH_BUS_STATION_MAP:
-                    AddExamFault(50, rtkTime);
-                    break;
-                default:
-                    break;
+                }
+            } else if (rec != NOT_ENTER) {
+                RemoveTargetReduceRec(TargetReduceRec2, key);
             }
         }
     }
-
-    // 鏈夊仠姝㈡爣璁扮殑锛屾槸鍚﹀仠杞︼紙鍋滄绾�5绫冲唴鏈夊仠杞﹀姩浣滐級
-    if (RoadMapList[index].flagStop != 0 && stopActive == 0) {
-        if (distance2StopLine < 0.0) {
-            // 涓嶅仠杞︾灜鏈涳紝涓嶅悎鏍�
-            DEBUG("涓嶅仠杞︾灜鏈�");
-            switch (RoadMapList[index].type) {
-                case THROUGH_INTERSECTION_MAP:
-                    AddExamFault(42, rtkTime);
-                    break;
-                case TURN_LEFT_MAP:
-                    AddExamFault(44, rtkTime);
-                    break;
-                case TURN_RIGHT_MAP:
-                    AddExamFault(47, rtkTime);
-                    break;
-                default:
-                    break;
-            }
-            stopActive = -1;
-        } else if (distance2StopLine < DISTANCE_STOP_CAR_TO_STOP_LINE) {
-            if (moveDirect == 0) {
-                stopActive = 1;
-            }
-        }
-    }
-
-     if (CrashSonRedLine(index, RoadMapList, car, CarModelList)) {
-         if (!crashRedLine) {
-             DEBUG("涓嶆寜鑰冭瘯鍛樻寚浠よ椹�");
-             crashRedLine = true;
-             // 涓嶆寜鑰冭瘯鍛樻寚浠よ椹�
-             AddExamFault(3, rtkTime);
-         }
-     } else {
-         crashRedLine = false;
-     }
-
-    if (ExitSonArea(index, RoadMapList, car)) {
-        DEBUG("绂诲紑閫氳繃something鍖哄煙");
-        return -1;
-    }
-
-    return index;
 }
-#endif
 
+void ExitTarget(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, const struct RtkTime *rtkTime)
+{
+    RECHECK:
+    for (auto it = TargetReduceRec.begin(); it != TargetReduceRec.end(); ++it) {
+            int road = it->first / 100;
+            int x = it->first % 100;
 
+            if (CrashTheLine(RoadMap.roads[road].stopLine[x].line, car, CarModelList)) {
+                DEBUG("瓒婅繃璺彛 road %d 璺彛 %d", road, x);
+                if (!(it->second & REDUCE_SPEED)) {
+                    // 涓嶆寜瑙勫畾鍑忛�燂紝涓嶅悎鏍�
+                    DEBUG("涓嶆寜瑙勫畾鍑忛��");
 
+                    if (RoadMap.roads[road].stopLine[x].active == ROAD_ACTIVE_FORWARD) {
+                        AddExamFault(41, rtkTime);
+                    } else if (RoadMap.roads[road].stopLine[x].active ==
+                               ROAD_ACTIVE_TURN_LEFT) {
+                        AddExamFault(43, rtkTime);
+                    } else if (RoadMap.roads[road].stopLine[x].active ==
+                               ROAD_ACTIVE_TURN_RIGHT) {
+                        AddExamFault(46, rtkTime);
+                    }
+                }
+                if (!(it->second & STOP_CAR) && RoadMap.roads[road].stopLine[x].stopFlag) {
+                    // 涓嶅仠杞︾灜鏈涳紝涓嶅悎鏍�
+                    DEBUG("涓嶅仠杞︾灜鏈�");
+                    if (RoadMap.roads[road].stopLine[x].active == ROAD_ACTIVE_FORWARD) {
+                        AddExamFault(42, rtkTime);
+                    } else if (RoadMap.roads[road].stopLine[x].active ==
+                               ROAD_ACTIVE_TURN_LEFT) {
+                        AddExamFault(44, rtkTime);
+                    } else if (RoadMap.roads[road].stopLine[x].active ==
+                               ROAD_ACTIVE_TURN_RIGHT) {
+                        AddExamFault(47, rtkTime);
+                    }
+                }
+
+                RemoveTargetReduceRec(TargetReduceRec, it->first);
+                goto RECHECK;
+            }
+    }
+
+    RECHECK2:
+    for (auto it = TargetReduceRec2.begin(); it != TargetReduceRec2.end(); ++it) {
+            int x = it->first;
+            Line line;
+            int roadIndex = 0;
+
+            for (; roadIndex < RoadMap.roads.size(); ++roadIndex) {
+                if (RoadMap.roads[roadIndex].id == RoadMap.specialAreas[x].road)
+                    break;
+            }
+
+            PointF point2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].leftEdge,
+                                                       RoadMap.specialAreas[x].area[1]);
+            MakeLine(&line, &RoadMap.specialAreas[x].area[1], &point2);
+
+            if (CrashTheLine(line, car, CarModelList)) {
+                if (RoadMap.specialAreas[x].type == ZEBRA_CROSSING &&
+                    !(it->second & REDUCE_SPEED)) {
+                    DEBUG("涓嶆寜瑙勫畾鍑忛��");
+                    AddExamFault(48, rtkTime);
+                }
+                if (RoadMap.specialAreas[x].type == BUS_STATION_AREA &&
+                    !(it->second & REDUCE_SPEED)) {
+                    DEBUG("涓嶆寜瑙勫畾鍑忛��");
+                    AddExamFault(50, rtkTime);
+                }
+                RemoveTargetReduceRec(TargetReduceRec2, it->first);
+                goto RECHECK2;
+            }
+        }
+}

--
Gitblit v1.8.0