From fb7b0660a319a9c54ff35c3944548348fae11b60 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期一, 24 八月 2020 18:53:23 +0800
Subject: [PATCH] 坐标

---
 lib/src/main/cpp/test_items2/through_something.cpp |   40 ++++++++++++++++++++++++++++++++--------
 1 files changed, 32 insertions(+), 8 deletions(-)

diff --git a/lib/src/main/cpp/test_items2/through_something.cpp b/lib/src/main/cpp/test_items2/through_something.cpp
index 7372634..f75d5db 100644
--- a/lib/src/main/cpp/test_items2/through_something.cpp
+++ b/lib/src/main/cpp/test_items2/through_something.cpp
@@ -66,10 +66,12 @@
     TargetReduceRec2.clear();
 }
 
-void ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime)
+double ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime)
 {
+    vector<double> nearbyTarget;        // 瀛︽牎銆佸叕浜ょ珯銆佷汉琛岄亾鍖哄煙鐨勮窛绂�
+
     if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
-        return;
+        return 100000;
     // 璺彛
     for (int i = 0; i < RoadMap.roads[roadIndex].stopLine.size(); ++i) {
         PointF point;
@@ -106,12 +108,19 @@
             continue;
 
         if (RoadMap.specialAreas[i].type == ZEBRA_CROSSING || RoadMap.specialAreas[i].type == BUS_STATION_AREA) {
-            double distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge);
+            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 (distance > 1e-3 && distance < LASTEST_BREAK_POINT) {
+            if (distance1 < -1e-3 && distance2 > 1e-3) {
+                nearbyTarget.push_back(0);
+            } else if (distance1 > 1e-3 && distance2 > 1e-3) {
+                nearbyTarget.push_back(distance1);
+            }
+
+            if (distance1 > 1e-3 && distance1 < LASTEST_BREAK_POINT) {
                 if (rec == NOT_ENTER) {
                     SetTargetReduceRec(TargetReduceRec2, key, ENTER_Z);
                 }
@@ -119,30 +128,45 @@
                 if (dobreak && !(rec & REDUCE_SPEED)) {
                     SetTargetReduceRec(TargetReduceRec2, key, rec | REDUCE_SPEED);
                 }
-            } else if (distance > LASTEST_BREAK_POINT + 5 && rec != NOT_ENTER) {
+            } else if (distance1 > 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 (distance1 < -1e-3 && distance2 > 1e-3) {
+                nearbyTarget.push_back(0);
                 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("閫氳繃瀛︽牎鍖哄煙瓒呴��");
+                    DEBUG("閫氳繃瀛︽牎鍖哄煙瓒呴�� %f kmh", ConvertMs2KMh(speed));
                     AddExamFault(49, rtkTime);
                 }
-            } else if (rec != NOT_ENTER) {
-                RemoveTargetReduceRec(TargetReduceRec2, key);
+            } else if (distance1 < -1e-3 && distance2 < -1e-3) {
+                if (rec != NOT_ENTER) {
+                    RemoveTargetReduceRec(TargetReduceRec2, key);
+                }
+            } else {
+                nearbyTarget.push_back(distance1);
+                if (rec != NOT_ENTER) {
+                    RemoveTargetReduceRec(TargetReduceRec2, key);
+                }
             }
         }
     }
+
+    if (nearbyTarget.size() > 0) {
+        sort(nearbyTarget.begin(), nearbyTarget.end());
+        return nearbyTarget[0];
+    }
+    return 100000;
 }
 
 void ExitTarget(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, const struct RtkTime *rtkTime)

--
Gitblit v1.8.0