| | |
| | | START_CAR_DONE |
| | | }; |
| | | |
| | | static const int INVALID_ROAD = -1; |
| | | |
| | | static const int TURN_THRESHOLD = 1; |
| | | static const int TURN_CHECK_INTERVAL = 500; |
| | | const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3; |
| | |
| | | } |
| | | return -1; |
| | | } |
| | | #if 0 |
| | | void TestRoadGeneral(struct road_exam2_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) |
| | | { |
| | | |
| | | } |
| | | |
| | | /************************************************** |
| | | * 车辆当前所在路段,车头需越过起始线一定距离 |
| | | * @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 roadArea; |
| | | |
| | | roadArea.num = map.roads[currIndex].leftEdge.size() + map.roads[currIndex].rightEdge.size();; |
| | | roadArea.point = (PointF *) malloc(roadArea.num * sizeof(PointF));; |
| | | |
| | | int n = 0; |
| | | for (int j = 0; j < map.roads[currIndex].leftEdge.size(); j++) { |
| | | roadArea.point[n++] = map.roads[i].leftEdge[j]; |
| | | } |
| | | for (int j = map.roads[currIndex].rightEdge.size(); j > 0; j--) { |
| | | roadArea.point[n++] = map.roads[i].rightEdge[j - 1]; |
| | | } |
| | | |
| | | 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, &roadArea) == GM_None) { |
| | | ret = true; |
| | | } |
| | | |
| | | free(carBody.point); |
| | | free(roadArea.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; |
| | | } |
| | | |
| | | struct { |
| | | int road; |
| | | int segment; |
| | | int track; |
| | | } CarOnTrackInfo; |
| | | |
| | | static void DetectSeparate(int currIndex, struct road_exam2_map &map, const car_model *car) |
| | | { |
| | | int segment; |
| | | int track = -1; |
| | | |
| | | if (currIndex == INVALID_ROAD || currIndex >= map.roads.size()) |
| | | return; |
| | | |
| | | for (int i = 0; i < map.roads[currIndex].separate.size(); i++) { |
| | | if (map.roads[currIndex].separate[i].character == 0) { |
| | | int separate_line_num = map.roads[currIndex].separate[i].line.size(); |
| | | |
| | | // 遍历当前分段的每一条线 |
| | | for (int j = 0; j < separate_line_num; ++j) { |
| | | Line theLine; |
| | | int p1 = 0; |
| | | bool match_line = false; |
| | | // 单独的一条虚线 |
| | | 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 (p1 == 0 || p2 == map.roads[currIndex].separate[i].line[j].num - 1) { |
| | | // 首尾两端,应采用延长线的方式 |
| | | } |
| | | if (VerticalPointOnLine(car->basePoint, theLine)) { |
| | | match_line = true; |
| | | int rel = IntersectionOfLine(map.roads[currIndex].separate[i].line[j].point[p1], |
| | | map.roads[currIndex].separate[i].line[j].point[p2], |
| | | car->basePoint); |
| | | // 记录所在道路,分道段,分道情况 |
| | | if (rel == 1) { // 在左侧 |
| | | segment = i; |
| | | track = j; |
| | | goto DETECT_SEPARATE_END; |
| | | } else if (rel != -1) { // 在线上 |
| | | segment = i; |
| | | track = j; |
| | | goto DETECT_SEPARATE_END; |
| | | } |
| | | break; |
| | | } |
| | | p1 = p2; |
| | | } |
| | | |
| | | track = separate_line_num; |
| | | } |
| | | } |
| | | } |
| | | |
| | | DETECT_SEPARATE_END: |
| | | ; |
| | | } |
| | | #endif |