| | |
| | | // |
| | | // Created by YY on 2019/10/23. |
| | | // |
| | | // |
| | | // 0 ____________________________________7 |
| | | // |
| | | // |
| | | // 1 _____________2 5_________________6 |
| | | // 8| |9 |
| | | // | | |
| | | // | | |
| | | // | | |
| | | // 3--------4 |
| | | // |
| | | |
| | | #include "park_bottom.h" |
| | | #include "../common/apptimer.h" |
| | |
| | | TEST_FINISH |
| | | }; |
| | | |
| | | static int testStatus; |
| | | typedef enum { |
| | | NONE_CTRL_LINE, |
| | | LEFT_CTRL_LINE, |
| | | RIGHT_CTRL_LINE |
| | | } ctrl_line_t; |
| | | |
| | | static bool reverseCar = false; |
| | | static int mapIndex = 0; |
| | | |
| | | const uint32_t CHECK_PARK_DELAY = 400; |
| | | |
| | | static uint32_t stopTimepoint; |
| | | static int prevMoveDirect, storeMoveDirectBeforeStop; |
| | | static int prevMoveDirect; |
| | | static bool occurCrashRedLine; |
| | | static bool checkPartStatus; |
| | | |
| | | static uint32_t firstReverseTimepoint; |
| | | static bool reportExamTimeout, reportParkFail; |
| | | static bool crossCtrlLineSw; |
| | | |
| | | static int parkCount; |
| | | static char carray[3]; |
| | | static int darray[3]; |
| | |
| | | |
| | | static int gearAtStop; |
| | | static int currGear; |
| | | static char prevCrossedCtrlLine; |
| | | static ctrl_line_t prevCrossedCtrlLine; |
| | | static double odo; |
| | | static int exitAreaCfm; |
| | | static char CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car); |
| | | static bool EnterParking(const Polygon *map, const car_model *car); |
| | | static bool CrashRedLine(const Polygon *map, const car_model *car, int &who); |
| | | static bool ExitParkArea(const Polygon *map, const car_model *car); |
| | | static bool AllTireExitParkArea(const Polygon *map, const car_model *car); |
| | | |
| | | void StartParkBottom(int index, int moveDirect, const struct RtkTime *rtkTime) |
| | | static ctrl_line_t CrossCtrlLine(prime_t &prime); |
| | | static bool EnterParking(prime_t &prime); |
| | | static bool CrashRedLine(prime_t &prime); |
| | | |
| | | |
| | | void StartParkBottom(prime_t &prime) |
| | | { |
| | | DEBUG("StartParkBottom"); |
| | | testStatus = TESTING; |
| | | |
| | | reverseCar = false; |
| | | mapIndex = index; |
| | | memset(carray, 0, sizeof(carray)); |
| | | memset(darray, 0, sizeof(darray)); |
| | | memset(parkStatus, 0, sizeof(parkStatus)); |
| | | prevMoveDirect = moveDirect; |
| | | prevMoveDirect = prime.pMotion->move; |
| | | |
| | | checkPartStatus = false; |
| | | firstReverseTimepoint = 0; |
| | | reportExamTimeout = false; |
| | | |
| | | parkCount = 0; |
| | | crossCtrlLineSw = false; |
| | | reportParkFail = false; |
| | | |
| | | occurCrashRedLine = false; |
| | | |
| | | currGear = ReadCarStatus(GEAR); |
| | | prevCrossedCtrlLine = 0; |
| | | prevCrossedCtrlLine = NONE_CTRL_LINE; |
| | | stopTimepoint = 0; |
| | | exitAreaCfm = 0; |
| | | |
| | | odo = ReadOdo(); |
| | | |
| | | PlayTTS("您已进入倒车入库区域", NULL); |
| | | } |
| | | |
| | | int TestParkBottom(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime) |
| | | void TestParkBottom(prime_t &prime) |
| | | { |
| | | char crossCtrlLine = 0; |
| | | uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); |
| | | int who = 0; |
| | | ctrl_line_t crossCtrlLine = NONE_CTRL_LINE; |
| | | uint32_t tp = AppTimer_GetTickCount(); |
| | | |
| | | vector<double> dtox; |
| | | vector<Line> line_set; |
| | | Line distance_line; |
| | |
| | | odo = ReadOdo(); |
| | | } |
| | | |
| | | if (testStatus == TESTING && gear_change) { |
| | | if (gear_change) { |
| | | if (currGear == GEAR_R) { |
| | | // 挂倒挡,检测是否过控制线 |
| | | DEBUG("开始挂倒挡"); |
| | | if (!reverseCar) { |
| | | DEBUG("开始首轮入库"); |
| | | reverseCar = true; |
| | | MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 1); |
| | | MA_EnterMap(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].id, MAP_TYPE_PARK_BUTTOM, 1); |
| | | firstReverseTimepoint = tp; // 开始210秒计时 |
| | | } |
| | | crossCtrlLine = CrossCtrlLine(map, car, carPrev); |
| | | crossCtrlLine = CrossCtrlLine(prime); |
| | | |
| | | if (parkCount >= 2) { |
| | | DEBUG("开始次轮入库"); |
| | | parkCount = 0; |
| | | MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 1); |
| | | MA_EnterMap(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].id, MAP_TYPE_PARK_BUTTOM, 1); |
| | | firstReverseTimepoint = tp; // 开始210秒计时 |
| | | } |
| | | |
| | | if (crossCtrlLine == 0) { |
| | | if (crossCtrlLine == NONE_CTRL_LINE) { |
| | | // 倒车前,前轮未驶过控制线 |
| | | AddExamFault(20104, rtkTime); |
| | | AddExamFault(20104); |
| | | DEBUG("倒车前,前轮未驶过控制线"); |
| | | } else if (crossCtrlLine == prevCrossedCtrlLine) { |
| | | // 重复跨越同一控制线,不按规定线路,顺序形式,不合格 |
| | | AddExamFault(20101, rtkTime); |
| | | AddExamFault(20101); |
| | | DEBUG("不按规定线路,顺序形式, 同 %c 侧", prevCrossedCtrlLine); |
| | | } else { |
| | | prevCrossedCtrlLine = crossCtrlLine; |
| | |
| | | |
| | | DEBUG("库位检查次数 = %d", parkCount); |
| | | |
| | | if (EnterParking(map, car)) { |
| | | if (EnterParking(prime)) { |
| | | DEBUG("倒库成功"); |
| | | } else { |
| | | AddExamFault(20103, rtkTime); |
| | | AddExamFault(20103); |
| | | DEBUG("倒库不入"); |
| | | } |
| | | } |
| | | } |
| | | |
| | | if (testStatus == TESTING && parkCount < 2 && AllTireExitParkArea(map, car)) { |
| | | if (++exitAreaCfm >= 4) { // 避免信号漂移造成的误判 |
| | | testStatus = TEST_FAIL; |
| | | AddExamFault(10103, rtkTime); |
| | | |
| | | if (ExitParkArea(prime)) { |
| | | // 离开场地 |
| | | DEBUG("离开场地"); |
| | | if (parkCount < 2) { |
| | | AddExamFault(10103); |
| | | DEBUG("直接驶离测试区,不按考试员指令驾驶"); |
| | | } |
| | | } else { |
| | | exitAreaCfm = 0; |
| | | } |
| | | |
| | | if (ExitParkArea(map, car)) { |
| | | DEBUG("离开场地"); |
| | | // 离开场地 |
| | | testStatus = TEST_FINISH; |
| | | /*if ((parkStatus[0] != 1 || parkStatus[1] != 1) && !reportParkFail && reverseCar) { |
| | | // 倒库不入,不合格 |
| | | reportParkFail = true; |
| | | AddExamFault(20103, rtkTime); |
| | | DEBUG("倒库不入"); |
| | | }*/ |
| | | prime.curr_exam_map.type = 0; |
| | | goto TEST_END; |
| | | } |
| | | |
| | | // 距离检测 |
| | | MakeLine(&distance_line, &map->point[0], &map->point[7]); |
| | | line_set.push_back(distance_line); |
| | | MakeLine(&distance_line, &map->point[1], &map->point[2]); |
| | | line_set.push_back(distance_line); |
| | | MakeLine(&distance_line, &map->point[2], &map->point[3]); |
| | | line_set.push_back(distance_line); |
| | | MakeLine(&distance_line, &map->point[3], &map->point[4]); |
| | | line_set.push_back(distance_line); |
| | | MakeLine(&distance_line, &map->point[4], &map->point[5]); |
| | | line_set.push_back(distance_line); |
| | | MakeLine(&distance_line, &map->point[5], &map->point[6]); |
| | | line_set.push_back(distance_line); |
| | | DistanceOfTire2X(dtox, car, line_set); |
| | | MA_SendDistance(dtox[0], dtox[1]); |
| | | |
| | | if (CrashRedLine(map, car, who)) { |
| | | if (!occurCrashRedLine /*&& reverseCar*/) { |
| | | if (BodyCollidingLine(prime) >= 0) { |
| | | if (!occurCrashRedLine) { |
| | | occurCrashRedLine = true; |
| | | // 车身出线,不合格 |
| | | AddExamFault(10116, rtkTime); |
| | | DEBUG("车轮压线"); |
| | | /*if (who == 1) { |
| | | PlayTTS("压左库位线", NULL); |
| | | } else if (who == 2) { |
| | | PlayTTS("压右库位线", NULL); |
| | | }*/ |
| | | AddExamFault(10116); |
| | | DEBUG("车身出线"); |
| | | } |
| | | } else { |
| | | occurCrashRedLine = false; |
| | | } |
| | | |
| | | if (moveDirect != prevMoveDirect) { |
| | | if (moveDirect == 0) { |
| | | if (prime.pMotion->move != prevMoveDirect) { |
| | | if (prime.pMotion->move == STOP) { |
| | | stopTimepoint = tp; |
| | | gearAtStop = (currGear == GEAR_R ? 1 : 0); |
| | | DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss); |
| | | DEBUG("停车了"); |
| | | DEBUG("停车时挡位 = %d", gearAtStop); |
| | | } else if (prevMoveDirect == 0 && stopTimepoint > 0) { |
| | | DEBUG("继续行驶 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss); |
| | | } else if (prevMoveDirect == STOP && stopTimepoint > 0) { |
| | | DEBUG("继续行驶"); |
| | | |
| | | DEBUG("停车时间 %ld", tp - stopTimepoint); |
| | | DEBUG("再次移动时挡位 = %d", currGear == GEAR_R ? 1 : 0); |
| | |
| | | if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_bottom_pause_criteria) |
| | | && gearAtStop == (currGear == GEAR_R ? 1 : 0)) { |
| | | // 停车超2秒,每次扣5分 |
| | | AddExamFault(20106, rtkTime); |
| | | AddExamFault(20106); |
| | | DEBUG("中途停车"); |
| | | } |
| | | } |
| | | |
| | | prevMoveDirect = moveDirect; |
| | | prevMoveDirect = prime.pMotion->move; |
| | | } |
| | | |
| | | /* crossCtrlLine = CrossCtrlLine(map, car, carPrev); |
| | | if (crossCtrlLine > 0 && !crossCtrlLineSw) { |
| | | crossCtrlLineSw = true; |
| | | if (parkCount == 0) { |
| | | carray[0] = crossCtrlLine; |
| | | } else if (parkCount == 1) { |
| | | if (carray[0] == crossCtrlLine) { |
| | | // 不按规定线路,顺序形式,不合格 |
| | | AddExamFault(20101, rtkTime); |
| | | DEBUG("不按规定线路,顺序形式"); |
| | | } |
| | | carray[1] = crossCtrlLine; |
| | | } else if (parkCount == 2) { |
| | | if (carray[0] != crossCtrlLine) { |
| | | // 不按规定线路,顺序形式,不合格 |
| | | AddExamFault(20101, rtkTime); |
| | | DEBUG("不按规定线路,顺序形式"); |
| | | } else { |
| | | // 离开测试区,停止计时 |
| | | DEBUG("离开测试区,停止计时"); |
| | | testStatus = false; |
| | | goto TEST_END; |
| | | } |
| | | carray[2] = crossCtrlLine; |
| | | } |
| | | } |
| | | |
| | | if (testStatus && darray[0] > 0 && tp - firstReverseTimepoint >= examParam.park_bottom_limit_time) { |
| | | // 完成超时,不合格 |
| | | if (!reportExamTimeout) { |
| | | reportExamTimeout = true; |
| | | AddExamFault(20105, rtkTime); |
| | | DEBUG("项目超时"); |
| | | } |
| | | } |
| | | |
| | | if (moveDirect != prevMoveDirect) { |
| | | if (moveDirect == 0) { |
| | | stopTimepoint = tp; |
| | | storeMoveDirectBeforeStop = prevMoveDirect; |
| | | if (prevMoveDirect == -1) { |
| | | checkPartStatus = true; // 每次倒车停止,触发入库检查 |
| | | } |
| | | |
| | | DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss); |
| | | } else { |
| | | DEBUG("继续行驶 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss); |
| | | |
| | | DEBUG("停车时间 %ld", tp - stopTimepoint); |
| | | |
| | | if (moveDirect == storeMoveDirectBeforeStop) { |
| | | // 同方向再启动,继续判断是否停车超时 |
| | | if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_bottom_pause_criteria) && reverseCar) { |
| | | // 停车超2秒,每次扣5分 |
| | | AddExamFault(20106, rtkTime); |
| | | DEBUG("中途停车"); |
| | | } |
| | | } else if (moveDirect == -1) { |
| | | // 切换为倒车 |
| | | if (!reverseCar) { |
| | | reverseCar = true; |
| | | MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 1); |
| | | } |
| | | if (darray[parkCount] == 0) { |
| | | if (!crossCtrlLineSw) { |
| | | // 倒车前,前轮未驶过控制线 |
| | | AddExamFault(20104, rtkTime); |
| | | DEBUG("倒车前,前轮未驶过控制线"); |
| | | } |
| | | darray[parkCount] = 1; |
| | | firstReverseTimepoint = tp; // 首次倒车,才意味着此项目开始 |
| | | } |
| | | } else { |
| | | // 切换为前进 |
| | | DEBUG("切换为前进"); |
| | | |
| | | if (tp - stopTimepoint >= CHECK_PARK_DELAY) { |
| | | if (crossCtrlLineSw) { |
| | | if (parkStatus[parkCount] != 1) { |
| | | // 倒库不入,不合格 |
| | | reportParkFail = true; |
| | | AddExamFault(20103, rtkTime); |
| | | DEBUG("倒库不入"); |
| | | } |
| | | } |
| | | |
| | | crossCtrlLineSw = false; |
| | | |
| | | if (parkCount < 2) |
| | | parkCount++; |
| | | } |
| | | } |
| | | } |
| | | prevMoveDirect = moveDirect; |
| | | } else if (moveDirect == -1) { |
| | | if (darray[parkCount] == 0) { |
| | | // 切换为倒车 |
| | | if (!crossCtrlLineSw) { |
| | | // 倒车前,前轮未驶过控制线 |
| | | AddExamFault(20104, rtkTime); |
| | | DEBUG("倒车前,前轮未驶过控制线"); |
| | | } |
| | | darray[parkCount] = 1; |
| | | firstReverseTimepoint = tp; |
| | | } |
| | | } else if (moveDirect == 0 && crossCtrlLineSw) { |
| | | if (tp - stopTimepoint >= CHECK_PARK_DELAY && checkPartStatus) { |
| | | if (EnterParking(map, car)) { |
| | | parkStatus[parkCount] = 1; |
| | | } |
| | | checkPartStatus = false; |
| | | } |
| | | }*/ |
| | | |
| | | TEST_END: |
| | | if (testStatus == TEST_FINISH) { |
| | | DEBUG("倒库结束"); |
| | | MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 0); |
| | | return 0; |
| | | DEBUG("倒库结束"); |
| | | MA_EnterMap(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].id, MAP_TYPE_PARK_BUTTOM, 0); |
| | | } |
| | | |
| | | static int BodyCollidingLine(prime_t &prime) |
| | | { |
| | | vector<Line> lines; |
| | | |
| | | Polygon car_body; |
| | | |
| | | car_body.num = prime.pModel->body.size(); |
| | | car_body.point = new PointF[car_body.num]; |
| | | |
| | | for (int i = 0; i < car_body.num; ++i) { |
| | | car_body.point[i] = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[i]]; |
| | | } |
| | | return 1; |
| | | |
| | | Line line; |
| | | MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[0], |
| | | prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[7]); |
| | | lines.push_back(line); |
| | | MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[1], |
| | | prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2]); |
| | | lines.push_back(line); |
| | | MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2], |
| | | prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3]); |
| | | lines.push_back(line); |
| | | MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3], |
| | | prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4]); |
| | | lines.push_back(line); |
| | | MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4], |
| | | prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5]); |
| | | lines.push_back(line); |
| | | MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5], |
| | | prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[6]); |
| | | lines.push_back(line); |
| | | |
| | | int idx = 0; |
| | | |
| | | for (auto line: lines) { |
| | | if (IntersectionOf(line, &car_body) != GM_None) { |
| | | break; |
| | | } |
| | | idx++; |
| | | } |
| | | delete []car_body.point; |
| | | return idx < lines.size()? idx : -1; |
| | | } |
| | | |
| | | // 检测2前轮是否正向越过左右控制线 |
| | | static char CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car) |
| | | static ctrl_line_t CrossCtrlLine(prime_t &prime) |
| | | { |
| | | // 过右控制线 |
| | | if ((IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1) && |
| | | (IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]) == -1) && |
| | | (IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->axial[AXIAL_REAR]]) == 1)) { |
| | | return 'R'; |
| | | Line left_trace, right_trace; |
| | | Line left_ctrl, right_ctrl; |
| | | |
| | | MAKE_LINE(left_trace, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], |
| | | prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]]); |
| | | MAKE_LINE(right_trace, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]], |
| | | prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]); |
| | | |
| | | MAKE_LINE(left_ctrl, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[1], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[0]); |
| | | MAKE_LINE(right_ctrl, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[6], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[7]); |
| | | |
| | | |
| | | if (IntersectionOf(left_trace, left_ctrl) == GM_Intersection && |
| | | IntersectionOf(right_trace, left_ctrl) == GM_Intersection && |
| | | IntersectionOfLine(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], left_ctrl) == RELATION_LEFT) { |
| | | return LEFT_CTRL_LINE; |
| | | } |
| | | |
| | | // 过左控制线 |
| | | if ((IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == 1) && |
| | | (IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]) == 1) && |
| | | (IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->axial[AXIAL_REAR]]) == -1)) { |
| | | return 'L'; |
| | | if (IntersectionOf(left_trace, right_ctrl) == GM_Intersection && |
| | | IntersectionOf(right_trace, right_ctrl) == GM_Intersection && |
| | | IntersectionOfLine(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], right_ctrl) == RELATION_RIGHT) { |
| | | return RIGHT_CTRL_LINE; |
| | | } |
| | | |
| | | return 0; |
| | | return NONE_CTRL_LINE; |
| | | } |
| | | |
| | | static bool EnterParking(const Polygon *map, const car_model *car) { |
| | | // 需要库入口线宽计算在内 |
| | | static bool EnterParking(prime_t &prime) { |
| | | bool succ = false; |
| | | |
| | | Polygon parking; |
| | | Polygon park_area; |
| | | Polygon car_body; |
| | | |
| | | car_body.num = car->bodyNum; |
| | | car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num); |
| | | car_body.num = prime.pModel->body.size(); |
| | | car_body.point = new PointF[car_body.num]; |
| | | for (int i = 0; i < car_body.num; ++i) { |
| | | car_body.point[i] = car->carXY[car->body[i]]; |
| | | car_body.point[i] = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[i]]; |
| | | } |
| | | |
| | | MakePolygon(&parking, {map->point[2], map->point[3], map->point[4], map->point[5]}); |
| | | PointF p8 = PointExtend(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2], |
| | | prime.pMap->park_button_map[prime.curr_exam_map.map_idx].line_width, |
| | | YawOf(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3])); |
| | | |
| | | if (IntersectionOf(&car_body, &parking) == GM_Containment) { |
| | | PointF p9 = PointExtend(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5], |
| | | prime.pMap->park_button_map[prime.curr_exam_map.map_idx].line_width, |
| | | YawOf(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4])); |
| | | |
| | | MakePolygon(&park_area, {p8, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4], p9}); |
| | | |
| | | if (IntersectionOf(&car_body, &park_area) == GM_Containment) { |
| | | succ = true; |
| | | } |
| | | |
| | | CleanPolygon(&parking); |
| | | CleanPolygon(&park_area); |
| | | free(car_body.point); |
| | | |
| | | DEBUG("检查倒库状态 %s", succ ? "成功" : "失败"); |
| | |
| | | return succ; |
| | | } |
| | | |
| | | static bool CrashRedLine(const Polygon *map, const car_model *car, int &who) |
| | | // 4个车轮和车头点不在场地中 |
| | | bool ExitParkArea(prime_t &prime) |
| | | { |
| | | bool ret = false; |
| | | Polygon polygon; |
| | | |
| | | Line red_line; |
| | | const int red_lines[][2] = {{0, 7}, {1, 2}, {2, 3}, {3, 4}, {4, 5}, {5, 6}}; |
| | | |
| | | Polygon car_body; |
| | | |
| | | car_body.num = car->bodyNum; |
| | | car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num); |
| | | for (int i = 0; i < car_body.num; ++i) { |
| | | car_body.point[i] = car->carXY[car->body[i]]; |
| | | polygon.num = prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map.size(); |
| | | polygon.point = new PointF[polygon.num]; |
| | | for (int i = 0; i < polygon.num; ++i) { |
| | | polygon.point[i] = prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[i]; |
| | | } |
| | | |
| | | for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) { |
| | | MakeLine(&red_line, &(map->point[red_lines[i][0]]), &(map->point[red_lines[i][1]])); |
| | | if (IntersectionOf(red_line, &car_body) != GM_None) { |
| | | if (i == 2 || i == 1) { |
| | | who = 1; |
| | | } else if (i == 4 || i == 5) { |
| | | who = 2; |
| | | } |
| | | int num = 0; |
| | | |
| | | ret = true; |
| | | break; |
| | | } |
| | | if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], |
| | | &polygon) == GM_None) { |
| | | num++; |
| | | } |
| | | if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]], |
| | | &polygon) == GM_None) { |
| | | num++; |
| | | } |
| | | if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]], |
| | | &polygon) == GM_None) { |
| | | num++; |
| | | } |
| | | if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]], |
| | | &polygon) == GM_None) { |
| | | num++; |
| | | } |
| | | if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[prime.pModel->axial[AXIAL_FRONT]]], |
| | | &polygon) == GM_None) { |
| | | num++; |
| | | } |
| | | |
| | | free(car_body.point); |
| | | return ret; |
| | | } |
| | | delete []polygon.point; |
| | | |
| | | static bool ExitParkArea(const Polygon *map, const car_model *car) |
| | | { |
| | | // 全车都需不在地图中 |
| | | bool ret = false; |
| | | |
| | | 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) == GM_None) { |
| | | ret = true; |
| | | } |
| | | |
| | | free(carBody.point); |
| | | |
| | | return ret; |
| | | } |
| | | |
| | | // 双前轮和双后轮不在区域 |
| | | static bool AllTireExitParkArea(const Polygon *map, const car_model *car) |
| | | { |
| | | int tireExitNum = 0; |
| | | |
| | | if (IntersectionOf(car->carXY[ car->left_front_tire[TIRE_OUTSIDE] ], map) == GM_None) { |
| | | tireExitNum++; |
| | | } |
| | | |
| | | if (IntersectionOf(car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ], map) == GM_None) { |
| | | tireExitNum++; |
| | | } |
| | | |
| | | if (IntersectionOf(car->carXY[ car->left_rear_tire[TIRE_OUTSIDE] ], map) == GM_None) { |
| | | tireExitNum++; |
| | | } |
| | | |
| | | if (IntersectionOf(car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ], map) == GM_None) { |
| | | tireExitNum++; |
| | | } |
| | | |
| | | if (tireExitNum >= 4) { |
| | | return true; |
| | | } |
| | | return false; |
| | | return num == 5? true : false; |
| | | } |