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/road_exam.cpp |  596 +++++------------------------------------------------------
 1 files changed, 51 insertions(+), 545 deletions(-)

diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index d75355d..5196886 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -83,13 +83,8 @@
 const int OVERTAKE_HOLD_TIME = D_SEC(3);                // 鍦ㄨ秴杞﹂亾琛岄┒鐨勪竴娈垫椂闂�
 const double EXAM_RANGE = 3000.0;                       // 鑷冲皯椹鹃┒璺濈
 
-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 const double PASS_SCHOOL_MAX_SPEED = 30.0;           // kmh
 
-static const int FIND_POSITION = -2;
-static const int INVALID_POSITION = -1;
 
 static bool occurCrashRedLine;
 static bool occurCrashGreenLine;
@@ -125,7 +120,6 @@
 static int startCar, stopCar;
 static int currExamMapIndex;
 static trigger_line_t *currRoadItem;
-static int nextRoadId;
 static PointF roadItemStartPoint;
 static struct drive_timer roadItemStartTime;
 static bool overtake = false;
@@ -168,12 +162,9 @@
 
 
 static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
-static void ClearAll(road_exam_map &map);
-static bool AllCmp(road_exam_map &map);
-static void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList,
-                             double speed, int moveDirect, const struct RtkTime *rtkTime);
 
-static PointF CalcProjectionWithRoadEdge(vector<edge_t> &edge, PointF point);
+static bool AllCmp(road_exam_map &map);
+
 static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car);
 
 void InitRoadExam(road_exam_map &RoadMap)
@@ -215,15 +206,14 @@
     checkStartCarSignal = startCarLeftTurnSignal = false;
 
     Lane.road = Lane.sep = Lane.no = -1;
+    Lane.guide = 0;
 
-    nextRoadId = -1;
     checkTurn = false;
-    ClearAll(RoadMap);
 
     CrashLineType = -1;
 
     ResetOdo();
-
+    ResetTarget(RoadMap);
     // 鍒濆鍖栬�冮」
 }
 
@@ -242,7 +232,7 @@
  * @param point
  * @return
  */
-static PointF CalcProjectionWithRoadEdge(vector<edge_t> &edge, PointF point)
+PointF CalcProjectionWithRoadEdge(vector<edge_t> &edge, PointF point)
 {
     PointF p1, p2;
     PointF proj;
@@ -346,7 +336,7 @@
     return theProj;
 }
 
-static double CalcDistanceReference(PointF point, PointF refPoint, vector<edge_t> &edge)
+double CalcDistanceReference(PointF point, PointF refPoint, vector<edge_t> &edge)
 {
     double distance = 0;
 
@@ -657,7 +647,7 @@
     for (int i = 0; i < RoadMap.roads[roadIndex].separate[sepIndex].lane_direct.size(); ++i) {
         double d1 = CalcDistanceReference(point, RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].start, RoadMap.roads[roadIndex].rightEdge);
         double d2 = CalcDistanceReference(point, RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].end, RoadMap.roads[roadIndex].rightEdge);
-        if (fabs(d1) < 1e-6 && d1 < 0.0 && d2 > 1e-6 && laneNo < RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].direct.size()) {
+        if (d1 < -1e-3 && d2 > 1e-3 && laneNo < RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].direct.size()) {
             return RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].direct[laneNo];
         }
     }
@@ -1063,144 +1053,51 @@
     }
 }
 
-static map <int, int> TargetReduceRec;
-
-enum {
-    NOT_REDUCE_SPEED,
-    REDUCE_SPEED,
-    STOP_CAR,
-    OVER_SPEED
-};
-
-static void ChangeTargetReduceRec(int roadIndex, int index, int status)
+static int NearbyCrossingGuide(int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
 {
-    int key = roadIndex * 100 + index;
+    int guide = 0;
+    double distance;
 
-    auto it = CrossingHint.find(key);
-
-    if (it != CrossingHint.end()) {
-        CrossingHint.erase(it);
-    }
-    CrossingHint.insert(pair<int, int>(key, status));
-}
-
-static int GetTargetReduceRec(int roadIndex, int index)
-{
-    int key = roadIndex * 100 + index;
-
-    auto it = CrossingHint.find(key);
-
-    if (it != CrossingHint.end()) {
-        return it->second;
-    }
-    return -1;
-}
-
-void ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, double speed, int moveDirect)
-{
-    car_sensor_value_t brk = ReadCarSensorValue(BREAK);
-
-    if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
-        return;
-
-    for (int n = 0; n < RoadMap.roads[roadIndex].stopLine.size(); ++n) {
-        PointF point;
+    struct nearby_crossing {
+        int guide;
         double distance;
+    };
+
+    vector<struct nearby_crossing> set;
+
+    for (int i = 0; i < road.stopLine.size(); ++i) {
+        PointF point;
 
         point.X = road.stopLine[i].line.X1;
         point.Y = road.stopLine[i].line.Y1;
 
-        distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, RoadMap.roads[roadIndex].rightEdge);
+        distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point,
+                                         road.rightEdge);
 
-        if (distance > 1e-3 && distance < LASTEST_BREAK_POINT) {
-            // 璁板綍鍒硅溅锛屽仠杞�
-            if (brk.value == BREAK_ACTIVE) {
+        if (distance > 1e-3) {
+            struct nearby_crossing temp;
 
-            }
-        } else if (distance > LASTEST_BREAK_POINT + 5) {
+            temp.guide = road.stopLine[i].active;
+            temp.distance = distance;
 
+            set.push_back(temp);
         }
     }
 
-    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 (set.size() > 0) {
+        distance = set[0].distance;
+        guide = set[0].guide;
 
-        if (map.specialAreas[i].area.size() == 2 && map.specialAreas[i].leftPoints.size() != 2) {
-            // 璁$畻鐐瑰埌宸︿晶璺竟绾跨殑鍨傜偣
-            int road = 0;
-            for (road = 0; road < map.roads.size(); ++road) {
-                if (map.roads[road].id == map.specialAreas[i].road)
-                    break;
+        for (int i = 1; i < set.size(); ++i) {
+            if (distance > set[i].distance) {
+                distance = set[i].distance;
+                guide = set[i].guide;
             }
-
-            PointF vPoint = CalcProjectionWithRoadEdge(map.roads[road].leftEdge, map.specialAreas[i].area[0]);
-//            DEBUG("璁$畻鍨傜偣1 (%f, %f)", vPoint.X, vPoint.Y);
-
-            map.specialAreas[i].leftPoints.push_back(vPoint);
-
-            vPoint = CalcProjectionWithRoadEdge(map.roads[road].leftEdge, map.specialAreas[i].area[1]);
-//            DEBUG("璁$畻鍨傜偣2 (%f, %f)", vPoint.X, vPoint.Y);
-            map.specialAreas[i].leftPoints.push_back(vPoint);
-        }
-
-        if (map.specialAreas[i].type == ZEBRA_CROSSING || map.specialAreas[i].type == BUS_STATION_AREA) {
-//            DEBUG("鏂戦┈绾�");
-            Line startLine;
-
-            MakeLine(&startLine, &map.specialAreas[i].area[0], &map.specialAreas[i].leftPoints[0]);
-
-            // 杞﹀ご鍜屾枒椹嚎璺濈涓嶈冻30绫�
-            if (IntersectionOf(extLine, startLine) == GM_Intersection &&
-                IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], startLine) == 1 ) {
-                DEBUG("杩涘叆鍑忛�熷尯 %d", map.specialAreas[i].type);
-                if (BreakDone == BREAK_ACTIVE) {
-                    map.specialAreas[i].activeBreak = true;
-                }
-            }
-
-            // 璺ㄧ嚎鍚庯紝妫�鏌ュ埞杞﹀姩浣�
-            if (CrashTheLine(startLine, car, CarModelList)) {
-                if (!map.specialAreas[i].activeBreak) {
-                    // 涓嶆寜瑙勫畾鍑忛�燂紝涓嶅悎鏍�
-                    DEBUG("涓嶆寜瑙勫畾鍑忛��");
-                    if (map.specialAreas[i].type == ZEBRA_CROSSING) {
-                        AddExamFault(48, rtkTime);
-                    } else {
-                        AddExamFault(50, rtkTime);
-                    }
-                } else {
-                    DEBUG("鎸夎瀹氬噺閫�");
-                }
-
-            }
-        } else if (map.specialAreas[i].type == SCHOOL_AREA) {
-            Polygon school;
-//            DEBUG("瀛︽牎");
-            school.num = 4;
-            school.point = (PointF *) malloc(school.num * sizeof(PointF));
-
-            school.point[0] = map.specialAreas[i].area[0];
-            school.point[1] = map.specialAreas[i].area[1];
-            school.point[2] = map.specialAreas[i].leftPoints[1];
-            school.point[3] = map.specialAreas[i].leftPoints[0];
-
-            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &school) == GM_Containment) {
-                if (ConvertMs2KMh(speed) > PASS_SCHOOL_MAX_SPEED) {
-                    if (!map.specialAreas[i].overSpeed) {
-                        DEBUG("閫氳繃瀛︽牎鍖哄煙瓒呴��");
-                        AddExamFault(49, rtkTime);
-                        map.specialAreas[i].overSpeed = true;
-                    }
-                }
-            }
-
-            free(school.point);
         }
     }
+
+    return guide;
 }
-
-
 
 void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
 {
@@ -1384,20 +1281,19 @@
         }
     } else {
         // 鍓嶈繘
-
     }
-
-
-    // 妫�娴嬮�氳繃璺彛銆佷汉琛岄亾绛夊尯鍩熸椂锛岄噴鏀惧埞杞︽垨鍑忛��
-    CheckBreakActive(RoadMap, car, CarModelList,
-                                 speed, moveDirect, rtkTime);
 
     // 妫�娴嬬寮�姝よ矾娈碉紝鍏ㄨ溅闇�涓嶅湪鑼冨洿鍐�
     int oldid = currExamMapIndex;
     currExamMapIndex = CalcRoadIndex(currExamMapIndex, RoadMap, car);
     if (currExamMapIndex >= 0) {
+        car_sensor_value_t brk = ReadCarSensorValue(BREAK);
         HintCrossing(currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList);
+
+        // 妫�娴嬮�氳繃璺彛銆佷汉琛岄亾绛夊尯鍩熸椂锛岄噴鏀惧埞杞︽垨鍑忛��
+        ApproachTarget(RoadMap, car, currExamMapIndex, (brk.value == BREAK_ACTIVE), speed, moveDirect, rtkTime);
     }
+    ExitTarget(RoadMap, car, CarModelList, rtkTime);
 
     if (oldid != currExamMapIndex) {
         DEBUG("閬撹矾ID鍒囨崲 %d", currExamMapIndex);
@@ -1411,7 +1307,19 @@
         DEBUG("鍘嬬嚎绫诲瀷鍒囨崲 %d", CrashLineType);
     }
 
+
+    oldid = Lane.guide;
     DetectLane(RoadMap, car, currExamMapIndex, rtkTime);
+    if (Lane.guide > 0 && currExamMapIndex >= 0) {
+        if (!(NearbyCrossingGuide(currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList) & Lane.guide)) {
+            DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒");
+            AddExamFault(9, rtkTime);
+        }
+    }
+
+    if (Lane.guide != oldid) {
+        DEBUG("瀵煎悜绫诲瀷鍒囨崲 %d", Lane.guide);
+    }
 
     // 妫�娴嬪帇绾跨姸鎬�
     TestRoadStartCar(car, speed, moveDirect, rtkTime);
@@ -1702,405 +1610,3 @@
     }
     return true;
 }
-
-/************************************************************************
- * 寮�濮嬫柊鐨勮�冭瘯鍚庯紝娓呴櫎鍦板浘鎵�鏈夌殑鍒硅溅銆佸仠杞﹁褰�
- * @param map
- */
-static void ClearAll(road_exam_map &map)
-{
-//    for (int i = 0; i < map.roads.size(); ++i) {
-//        map.roads[i].activeStop = map.roads[i].activeBreak = false;
-//        map.roads[i].arrivedTail = false;
-//        map.roads[i].errorLane = false;
-//    }
-//    for (int i = 0; i < map.specialAreas.size(); i++) {
-//        map.specialAreas[i].overSpeed = map.specialAreas[i].activeBreak = false;
-//    }
-//    for (int i = 0; i < map.triggerLines.size(); ++i) {
-//        map.triggerLines[i].cmp = false;
-//    }
-}
-
-static void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList,
-                             double speed, int moveDirect, const struct RtkTime *rtkTime)
-{
-    int BreakDone = ReadCarStatus(BREAK);
-
-    // 璁$畻杞﹀墠杩涜建杩瑰欢闀跨嚎
-    double yaw = YawOf(car->carXY[ car->axial[AXIAL_FRONT] ], car->carXY[ car->axial[AXIAL_REAR] ]);
-    PointF extPoint = PointExtend(car->carXY[ car->axial[AXIAL_FRONT] ], LASTEST_BREAK_POINT, yaw);
-
-    Line extLine;
-    MakeLine(&extLine, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint);
-
-    // 璺彛鍒硅溅鐐�
-/*    for (int i = 0; i < map.roads.size(); ++i) {
-        // 杞﹀ご鍜岃矾鍙h窛绂讳笉瓒�30绫�
-        if (IntersectionOf(extLine, map.roads[i].stopLine) == GM_Intersection &&
-            IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], map.roads[i].stopLine) == 1 ) {
-            DEBUG("杩涘叆鍑忛�熷尯");
-            if (BreakDone == BREAK_ACTIVE) {
-                map.roads[i].activeBreak = true;
-            }
-            if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]],
-                           map.roads[i].stopLine) < DISTANCE_STOP_CAR_TO_STOP_LINE && moveDirect == 0) {
-                map.roads[i].activeStop = true;
-            }
-        }
-        // 璺ㄧ嚎鍚庯紝妫�鏌ュ埞杞﹀姩浣�
-        if (CrashTheLine(map.roads[i].stopLine, car, CarModelList)) {
-            if (map.roads[i].stopFlag != 0 && !map.roads[i].activeStop) {
-                // 涓嶅仠杞︾灜鏈涳紝涓嶅悎鏍�
-                DEBUG("涓嶅仠杞︾灜鏈�");
-                if (map.roads[i].active == ROAD_ACTIVE_FORWARD) {
-                    AddExamFault(42, rtkTime);
-                } else if (map.roads[i].active == ROAD_ACTIVE_TURN_LEFT) {
-                    AddExamFault(44, rtkTime);
-                } else if (map.roads[i].active == ROAD_ACTIVE_TURN_RIGHT) {
-                    AddExamFault(47, rtkTime);
-                }
-            }
-            if (!map.roads[i].activeBreak) {
-                // 涓嶆寜瑙勫畾鍑忛�燂紝涓嶅悎鏍�
-                DEBUG("涓嶆寜瑙勫畾鍑忛��");
-                if (map.roads[i].active == ROAD_ACTIVE_FORWARD) {
-                    AddExamFault(41, rtkTime);
-                } else if (map.roads[i].active == ROAD_ACTIVE_TURN_LEFT) {
-                    AddExamFault(43, rtkTime);
-                } else if (map.roads[i].active == ROAD_ACTIVE_TURN_RIGHT) {
-                    AddExamFault(46, rtkTime);
-                }
-            }
-        }
-    }*/
-
-    // 浜鸿閬撱�佸叕浜ょ珯鍒硅溅鐐癸紱瀛︽牎闄愰�熷尯
-//    DEBUG("琛ュ叏鐗规畩鍖哄煙 size = %d", map.specialAreas.size());
-    for (int i = 0; i < map.specialAreas.size(); i++) {
-        if (map.specialAreas[i].type == GRID_AREA)
-            continue;
-
-        if (map.specialAreas[i].area.size() == 2 && map.specialAreas[i].leftPoints.size() != 2) {
-            // 璁$畻鐐瑰埌宸︿晶璺竟绾跨殑鍨傜偣
-            int road = 0;
-            for (road = 0; road < map.roads.size(); ++road) {
-                if (map.roads[road].id == map.specialAreas[i].road)
-                    break;
-            }
-
-            PointF vPoint = CalcProjectionWithRoadEdge(map.roads[road].leftEdge, map.specialAreas[i].area[0]);
-//            DEBUG("璁$畻鍨傜偣1 (%f, %f)", vPoint.X, vPoint.Y);
-
-            map.specialAreas[i].leftPoints.push_back(vPoint);
-
-            vPoint = CalcProjectionWithRoadEdge(map.roads[road].leftEdge, map.specialAreas[i].area[1]);
-//            DEBUG("璁$畻鍨傜偣2 (%f, %f)", vPoint.X, vPoint.Y);
-            map.specialAreas[i].leftPoints.push_back(vPoint);
-        }
-
-        if (map.specialAreas[i].type == ZEBRA_CROSSING || map.specialAreas[i].type == BUS_STATION_AREA) {
-//            DEBUG("鏂戦┈绾�");
-            Line startLine;
-
-            MakeLine(&startLine, &map.specialAreas[i].area[0], &map.specialAreas[i].leftPoints[0]);
-
-            // 杞﹀ご鍜屾枒椹嚎璺濈涓嶈冻30绫�
-            if (IntersectionOf(extLine, startLine) == GM_Intersection &&
-                IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], startLine) == 1 ) {
-                DEBUG("杩涘叆鍑忛�熷尯 %d", map.specialAreas[i].type);
-                if (BreakDone == BREAK_ACTIVE) {
-                    map.specialAreas[i].activeBreak = true;
-                }
-            }
-
-            // 璺ㄧ嚎鍚庯紝妫�鏌ュ埞杞﹀姩浣�
-            if (CrashTheLine(startLine, car, CarModelList)) {
-                if (!map.specialAreas[i].activeBreak) {
-                    // 涓嶆寜瑙勫畾鍑忛�燂紝涓嶅悎鏍�
-                    DEBUG("涓嶆寜瑙勫畾鍑忛��");
-                    if (map.specialAreas[i].type == ZEBRA_CROSSING) {
-                        AddExamFault(48, rtkTime);
-                    } else {
-                        AddExamFault(50, rtkTime);
-                    }
-                } else {
-                    DEBUG("鎸夎瀹氬噺閫�");
-                }
-
-            }
-        } else if (map.specialAreas[i].type == SCHOOL_AREA) {
-            Polygon school;
-//            DEBUG("瀛︽牎");
-            school.num = 4;
-            school.point = (PointF *) malloc(school.num * sizeof(PointF));
-
-            school.point[0] = map.specialAreas[i].area[0];
-            school.point[1] = map.specialAreas[i].area[1];
-            school.point[2] = map.specialAreas[i].leftPoints[1];
-            school.point[3] = map.specialAreas[i].leftPoints[0];
-
-            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &school) == GM_Containment) {
-                if (ConvertMs2KMh(speed) > PASS_SCHOOL_MAX_SPEED) {
-                    if (!map.specialAreas[i].overSpeed) {
-                        DEBUG("閫氳繃瀛︽牎鍖哄煙瓒呴��");
-                        AddExamFault(49, rtkTime);
-                        map.specialAreas[i].overSpeed = true;
-                    }
-                }
-            }
-
-            free(school.point);
-        }
-    }
-//    DEBUG("琛ュ叏鐗规畩鍖哄煙 over");
-}
-
-#if 0
-
-typedef struct {
-    int road;
-    int segment;
-    int lane;
-} CarOnTrackInfo_t;
-
-static CarOnTrackInfo_t CarOnTrackInfo;
-
-void TestRoadGeneral(struct road_exam2_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
-{
-    // 妫�娴嬭溅閬撳彉鏇�
-    CarOnTrackInfo_t newCarOnTrackInfo;
-
-    if (newCarOnTrackInfo.road == CarOnTrackInfo.road &&
-            newCarOnTrackInfo.segment == CarOnTrackInfo.segment &&
-            newCarOnTrackInfo.lane != CarOnTrackInfo.lane) {
-
-    }
-
-    CarOnTrackInfo = newCarOnTrackInfo;
-
-
-}
-
-/**************************************************
- * 杞﹁締褰撳墠鎵�鍦ㄨ矾娈碉紝杞﹀ご闇�瓒婅繃璧峰绾夸竴瀹氳窛绂�
- * @param currIndex
- * @param map
- * @param car
- */
-static void EnterRoad(int &currIndex, struct road_exam2_map &map, const car_model *car)
-{
-    Polygon carBody;
-
-    carBody.num = car->bodyNum;
-    carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
-    for (int i = 0; i < carBody.num; ++i) {
-        carBody.point[i] = car->carXY[car->body[i]];
-    }
-
-    Polygon roadArea;
-    roadArea.num = 0;
-    roadArea.point = NULL;
-
-    for (int i = 0; i < map.roads.size(); ++i) {
-        if (roadArea.point != NULL) {
-            free(roadArea.point);
-        }
-
-        for (int x = 0; x < map.roads[i].leftEdge.size(); ++x) {
-            roadArea.num += map.roads[i].leftEdge[x].points.size();
-        }
-        for (int x = 0; x < map.roads[i].rightEdge.size(); ++x) {
-            roadArea.num += map.roads[i].rightEdge[x].points.size();
-        }
-        roadArea.point = (PointF *) malloc(roadArea.num * sizeof(PointF));
-
-        int n = 0;
-        for (int j = 0; j < map.roads[i].leftEdge.size(); j++) {
-            for (int x = 0; x < map.roads[i].leftEdge[j].points.size(); ++x) {
-                if (n > 0 && x == 0 && isEqual2(roadArea.point[n-1].X, map.roads[i].leftEdge[j].points[x].X) &&
-                             isEqual2(roadArea.point[n-1].Y, map.roads[i].leftEdge[j].points[x].Y)) {
-                    // 绗竴涓偣宸茬粡瀛樺湪浜庝笂涓�鏉$嚎
-                } else {
-                    roadArea.point[n++] = map.roads[i].leftEdge[j].points[x];
-                }
-            }
-        }
-        for (int j = map.roads[i].rightEdge.size(); j > 0; j--) {
-            for (int x = map.roads[i].rightEdge[j].points.size(); x > 0; --x) {
-                if (n > 0 && x == map.roads[i].rightEdge[j].points.size() &&
-                            isEqual2(roadArea.point[n-1].X, map.roads[i].rightEdge[j - 1].points[x-1].X) &&
-                            isEqual2(roadArea.point[n-1].Y, map.roads[i].rightEdge[j - 1].points[x-1].Y)) {
-                    // 绗竴涓偣宸茬粡瀛樺湪浜庝笂涓�鏉$嚎
-                } else {
-                    roadArea.point[n++] = map.roads[i].rightEdge[j - 1].points[x - 1];
-                }
-            }
-        }
-        roadArea.num = n;
-
-        if (IntersectionOf(car->carXY[car->axial[AXIAL_FRONT]], &roadArea) == GM_Containment) {
-            currIndex = i;
-            goto CHECK_CAR_ON_ROAD_END;
-        }
-    }
-    currIndex = INVALID_ROAD;
-
-    CHECK_CAR_ON_ROAD_END:
-    if (roadArea.point != NULL) {
-        free(roadArea.point);
-    }
-    free(carBody.point);
-}
-
-/******************************************************
- * 鍏ㄨ溅閮介渶绂诲紑杩欎釜鍖哄煙
- * @param currIndex
- * @param map
- * @param car
- * @return
- */
-static bool ExitRoad(int currIndex, struct road_exam2_map &map, const car_model *car)
-{
-    // 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
-    bool ret = false;
-
-    if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
-        return ret;
-
-    Polygon carBody;
-
-    carBody.num = car->bodyNum;
-    carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
-    for (int i = 0; i < carBody.num; ++i) {
-        carBody.point[i] = car->carXY[car->body[i]];
-    }
-
-    if (IntersectionOf(&carBody, &map.roads[currIndex].area) == GM_None) {
-        ret = true;
-    }
-
-    free(carBody.point);
-    return ret;
-}
-
-static bool CrashSeparateLine(int currIndex, struct road_exam2_map &map, const car_model *car)
-{
-    Line frontAxle, rearAxle;
-
-    if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
-        return false;
-
-    MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
-    MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
-
-    // 鍒嗘
-    for (int i = 0; i < map.roads[currIndex].separate.size(); i++) {
-        // 鍒嗘涓殑姣忔潯绾�
-        for (int j = 0; j < map.roads[currIndex].separate[i].line.size(); j++) {
-            Line theLine;
-            int p1 = 0;
-            for (int p2 = 1; p2 < map.roads[currIndex].separate[i].line[j].num; ++p2) {
-                MakeLine(&theLine, &map.roads[currIndex].separate[i].line[j].point[p1], &map.roads[currIndex].separate[i].line[j].point[p2]);
-                if (IntersectionOf(theLine, frontAxle) == GM_Intersection ||
-                    IntersectionOf(theLine, rearAxle) == GM_Intersection) {
-                    return true;
-                }
-                p1 = p2;
-            }
-        }
-    }
-    return false;
-}
-
-
-static void DetectSeparate(int currIndex, struct road_exam2_map &map, const car_model *car)
-{
-    int segment;
-    int lane = -1;
-
-    CarOnTrackInfo_t newInfo;
-
-    if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
-        return;
-
-    // 閬嶅巻姣忎竴鍒嗘
-    for (int i = 0; i < map.roads[currIndex].separate.size(); i++) {
-        int separate_line_num = map.roads[currIndex].separate[i].lines.size();
-
-        struct vrecord_ {
-            int valid;
-            int p1;
-            int p2;
-        };
-
-        vector<struct vrecord_> vrecord;
-        vrecord.clear();
-
-        bool match1 = false;
-        // 閬嶅巻褰撳墠鍒嗘鐨勬瘡涓�鏉$嚎
-        for (int j = 0; j < separate_line_num; ++j) {
-            Line theLine;
-            int p1 = 0;
-
-            struct vrecord_ v;
-            v.valid = 0;
-
-            // 鍗曠嫭鐨勪竴鏉¤櫄绾�
-            for (int p2 = 1; p2 < map.roads[currIndex].separate[i].lines[j].num; ++p2) {
-                MakeLine(&theLine, &map.roads[currIndex].separate[i].lines[j].point[p1],
-                         &map.roads[currIndex].separate[i].lines[j].point[p2]);
-                if (VerticalPointOnLine(car->basePoint, theLine)) {
-                    v.valid = 1;
-                    v.p1 = p1;
-                    v.p2 = p2;
-                    match1 = true;
-                    break;
-                }
-                p1 = p2;
-            }
-
-            vrecord.push_back(v);
-
-            lane = separate_line_num;//
-        }
-
-
-
-        if (match1) {
-            for (int x = 0; x < vrecord.size(); ++x) {
-                if (vrecord[i].valid == 0) {
-                    // 棣栧熬涓ゆ绾跨殑寤剁敵蹇呮湁涓�涓瀭鐐�
-                    if (DistanceOf(car->carXY[ car->axial[AXIAL_FRONT] ], map.roads[currIndex].separate[i].lines[x].point[0]) <
-                        DistanceOf(car->carXY[ car->axial[AXIAL_FRONT] ], map.roads[currIndex].separate[i].lines[x].point[1])) {
-                        vrecord[x].p1 = 0;
-                        vrecord[x].p2 = 1;
-                    } else {
-                        vrecord[x].p1 = map.roads[currIndex].separate[i].lines[x].num - 2;
-                        vrecord[x].p2 = map.roads[currIndex].separate[i].lines[x].num - 1;
-                    }
-                }
-
-                int rel = IntersectionOfLine(map.roads[currIndex].separate[i].lines[x].point[vrecord[x].p1],
-                                             map.roads[currIndex].separate[i].lines[x].point[vrecord[x].p2],
-                                             car->basePoint);
-                if (rel != -1) {
-                    newInfo.road = currIndex;
-                    newInfo.segment = i;
-                    newInfo.lane = x;
-                    break;
-                }
-            }
-
-            newInfo.road = currIndex;
-            newInfo.segment = i;
-            newInfo.lane = vrecord.size();
-
-            break;
-        }
-    }
-}
-
-
-
-#endif

--
Gitblit v1.8.0