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