| | |
| | | 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; |
| | |
| | | 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; |
| | |
| | | |
| | | |
| | | 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) |
| | |
| | | checkStartCarSignal = startCarLeftTurnSignal = false; |
| | | |
| | | Lane.road = Lane.sep = Lane.no = -1; |
| | | Lane.guide = 0; |
| | | |
| | | nextRoadId = -1; |
| | | checkTurn = false; |
| | | ClearAll(RoadMap); |
| | | |
| | | CrashLineType = -1; |
| | | |
| | | ResetOdo(); |
| | | |
| | | ResetTarget(RoadMap); |
| | | // 初始化考项 |
| | | } |
| | | |
| | |
| | | * @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; |
| | |
| | | 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; |
| | | |
| | |
| | | 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]; |
| | | } |
| | | } |
| | |
| | | } |
| | | } |
| | | |
| | | static map <int, int> TargetReduceRec; |
| | | static int NearbyCrossingGuide(int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList) |
| | | { |
| | | int guide = 0; |
| | | double distance; |
| | | |
| | | enum { |
| | | NOT_REDUCE_SPEED, |
| | | REDUCE_SPEED, |
| | | STOP_CAR, |
| | | OVER_SPEED |
| | | struct nearby_crossing { |
| | | int guide; |
| | | double distance; |
| | | }; |
| | | |
| | | static void ChangeTargetReduceRec(int roadIndex, int index, int status) |
| | | { |
| | | int key = roadIndex * 100 + index; |
| | | vector<struct nearby_crossing> set; |
| | | |
| | | 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) { |
| | | for (int i = 0; i < road.stopLine.size(); ++i) { |
| | | PointF point; |
| | | double distance; |
| | | |
| | | 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; |
| | | } |
| | | |
| | | 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; |
| | | for (int i = 1; i < set.size(); ++i) { |
| | | if (distance > set[i].distance) { |
| | | distance = set[i].distance; |
| | | guide = set[i].guide; |
| | | } |
| | | } |
| | | } |
| | | |
| | | 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) |
| | | { |
| | |
| | | } |
| | | } 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); |
| | |
| | | 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); |
| | |
| | | } |
| | | 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) { |
| | | // 车头和路口距离不足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 |