| | |
| | | // | | |
| | | // | | |
| | | // | | |
| | | // ________________| |___________1 |
| | | // ________________|9 8|___________1 |
| | | // 6 5 2 |
| | | // |
| | | // |
| | |
| | | |
| | | #include <vector> |
| | | #include <cstdlib> |
| | | #include <tuple> |
| | | |
| | | #define DEBUG(fmt, args...) LOGD("<park_edge> <%s>: " fmt, __func__, ##args) |
| | | |
| | | using namespace std; |
| | | |
| | | enum { |
| | | TESTING, |
| | | TEST_FAIL, // 因触发某些规则,在车身未完全立场情况下,提前终止部分测试 |
| | | TEST_FINISH |
| | | }; |
| | | static bool overpass_parkingspace; |
| | | static bool check_parking_space; |
| | | |
| | | const uint32_t CHECK_PARK_DELAY = 400; |
| | | |
| | | static bool reportExamTimeout; |
| | | static bool reportParkFail; |
| | | static bool occurCrashRedLine; |
| | | static bool firstMoveBack, checkLight; |
| | | |
| | | static uint32_t stopTimepoint = 0; |
| | | static bool occurCrashRedLine1, occurCrashRedLine2, occurCrashRedLine3; |
| | | static int prevMoveStatus, storeMoveStatusBeforeStop; |
| | | static int parkStatus; |
| | | static int gearAtStop; |
| | | static bool occurMoveBack, checkPark, parkSuccess, checkLight; |
| | | static uint32_t moveBackTimePoint; |
| | | static int testStatus; |
| | | static int exitAreaCfm; |
| | | static int currGear; |
| | | static double odo; |
| | | static void ParkTimeout(apptimer_var_t val); |
| | | static bool CrashRedLine(prime_t &prime); |
| | | static bool CheckParkspace(prime_t &prime); |
| | | static bool OverpassParkspace(prime_t &prime); |
| | | static void MotionChange(move_status_t mv); |
| | | |
| | | static bool CrashRedLine1(const Polygon *map, const car_model_t *car); |
| | | static bool CrashRedLine2(const Polygon *map, const car_model_t *car); |
| | | static bool CrashRedLine3(const Polygon *map, const car_model_t *car); |
| | | static bool EnterParking(const Polygon *map, const car_model_t *car); |
| | | static bool ExitParkArea(const Polygon *map, const car_model_t *car); |
| | | static bool ExitParkArea2(const Polygon *map, const car_model_t *car); |
| | | enum { |
| | | PE_PREPARE_PARK, |
| | | PE_PARKING, |
| | | PE_EXIT |
| | | }; |
| | | |
| | | void StartParkEdge(prime_t &prime) |
| | | { |
| | | DEBUG("进入侧方停车场地"); |
| | | |
| | | testStatus = TESTING; |
| | | |
| | | occurCrashRedLine1 = occurCrashRedLine2 = occurCrashRedLine3 = false; // 这个科目规定特殊点,发生一次扣10分,而不直接淘汰 |
| | | reportExamTimeout = false; |
| | | reportParkFail = false; |
| | | prevMoveStatus = prime.pMotion->move; |
| | | parkSuccess = false; |
| | | parkStatus = 0; |
| | | occurMoveBack = false; |
| | | checkPark = false; |
| | | occurCrashRedLine = false; // 这个科目规定特殊点,发生一次扣10分,而不直接淘汰 |
| | | firstMoveBack = false; |
| | | checkLight = false; |
| | | gearAtStop = -1; |
| | | stopTimepoint = 0; |
| | | odo = ReadOdo(); |
| | | currGear = ReadCarStatus(GEAR); |
| | | exitAreaCfm = 0; |
| | | |
| | | check_parking_space = false; |
| | | prime.examing_area.stage = PE_PREPARE_PARK; |
| | | PlayTTS("您已进入侧方停车区域", NULL); |
| | | // 仅当发生倒车,才意味着项目开始 |
| | | /*if (moveStatus == -1) { |
| | | occurMoveBack = true; |
| | | moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); |
| | | }*/ |
| | | } |
| | | |
| | | int TestParkEdge(prime_t &prime) |
| | | void StopParkEdge(prime_t &prime) |
| | | { |
| | | vector<double> dtox; |
| | | vector<Line> line_set; |
| | | Line distance_line; |
| | | if (prime.examing_area.type != MAP_TYPE_PARK_EDGE) |
| | | return; |
| | | DEBUG("离开侧方停车场地"); |
| | | |
| | | bool gear_change = false; |
| | | int gear = ReadCarStatus(GEAR); |
| | | |
| | | if (gear == GEAR_R) { |
| | | if (currGear != GEAR_R) { |
| | | gear_change = true; |
| | | currGear = GEAR_R; |
| | | } |
| | | } else if (currGear == GEAR_R) { |
| | | gear_change = true; |
| | | currGear = gear; |
| | | if (!check_parking_space) { |
| | | DEBUG("直接驶离测试区,不按考试员指令驾驶"); |
| | | AddExamFault(10103); |
| | | } |
| | | prime.examing_area.type = MAP_TYPE_NONE; |
| | | |
| | | if (gear_change) { |
| | | // 检查上一次挡位的行驶距离,过小就放弃,避开学员原地挂挡重试 |
| | | double run_distance = ReadOdo() - odo; |
| | | |
| | | DEBUG("2次挡位运行距离 %f", run_distance); |
| | | |
| | | if (run_distance < 1) { |
| | | gear_change = false; |
| | | DEBUG("2次挡位运行距离过小,忽略"); |
| | | } |
| | | odo = ReadOdo(); |
| | | } |
| | | |
| | | // 首次挂倒挡, 才意味着项目开始 |
| | | if (testStatus == TESTING && gear_change) { |
| | | if (currGear == GEAR_R) { |
| | | if (!occurMoveBack) { |
| | | DEBUG("首次侧方停车"); |
| | | } else { |
| | | DEBUG("再次侧方停车"); |
| | | } |
| | | |
| | | { |
| | | checkPark = false; |
| | | occurMoveBack = true; |
| | | moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, |
| | | rtkTime->mss * 10); // 开始计时 |
| | | MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 1); |
| | | } |
| | | } else { |
| | | if (occurMoveBack && !checkPark) { |
| | | // 检查车身入库情况 |
| | | DEBUG("检查车身入库情况"); |
| | | checkPark = true; |
| | | checkLight = false; |
| | | if (EnterParking(map, car)) { |
| | | parkStatus = 1; |
| | | parkSuccess = true; |
| | | } else { |
| | | // 停止后,车身出线,不合格 |
| | | AddExamFault(20401, rtkTime); |
| | | parkSuccess = false; |
| | | DEBUG("移库不入"); |
| | | } |
| | | } |
| | | } |
| | | } |
| | | |
| | | if (CrashRedLine1(map, car)) { |
| | | if (!occurCrashRedLine1 /*&& occurMoveBack*/) { |
| | | // 车轮压边线,每次扣10分 |
| | | AddExamFault(20403, rtkTime); |
| | | DEBUG("车轮压边线"); |
| | | occurCrashRedLine1 = true; |
| | | } |
| | | } else { |
| | | occurCrashRedLine1 = false; |
| | | } |
| | | |
| | | if (CrashRedLine2(map, car)) { |
| | | if (!occurCrashRedLine2 /*&& occurMoveBack*/) { |
| | | // 车身压库位线,每次扣10分 |
| | | AddExamFault(20404, rtkTime); |
| | | DEBUG("车身压库位线"); |
| | | occurCrashRedLine2 = true; |
| | | } |
| | | } else { |
| | | occurCrashRedLine2 = false; |
| | | } |
| | | |
| | | if (CrashRedLine3(map, car)) { |
| | | if (!occurCrashRedLine3 && !occurMoveBack && moveStatus == 1) { |
| | | // 车身压库位线,每次扣10分 |
| | | AddExamFault(20407, rtkTime); |
| | | DEBUG("车身压库位线"); |
| | | occurCrashRedLine3 = true; |
| | | } |
| | | } else { |
| | | occurCrashRedLine3 = false; |
| | | } |
| | | |
| | | if (ExitParkArea2(map, car)) { |
| | | /*if (!parkSuccess && occurMoveBack && !reportParkFail) { |
| | | // 直接驶离测试区,认为移库不入 |
| | | AddExamFault(10103, rtkTime); |
| | | reportParkFail = true; |
| | | DEBUG("直接驶离测试区,不按考试员指令驾驶"); |
| | | }*/ |
| | | if (occurMoveBack && !checkPark) { |
| | | // 倒车直接驶离测试区 |
| | | AddExamFault(10103, rtkTime); |
| | | DEBUG("直接驶离测试区,不按考试员指令驾驶"); |
| | | } |
| | | |
| | | testStatus = TEST_FINISH; |
| | | 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 (testStatus == TESTING && !occurMoveBack && ExitParkArea(map, car)) { |
| | | // 入库后一直前进,车头移出驶离线 |
| | | if (++exitAreaCfm >= 4) { // 避免信号漂移造成的误判 |
| | | AddExamFault(10103, rtkTime); |
| | | DEBUG("直接驶离测试区,不按考试员指令驾驶"); |
| | | testStatus = TEST_FAIL; |
| | | } |
| | | } else { |
| | | exitAreaCfm = 0; |
| | | } |
| | | |
| | | if (occurMoveBack) { |
| | | uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); |
| | | |
| | | if (!reportExamTimeout && tp - moveBackTimePoint >= examParam.park_edge_limit_time) { |
| | | // 超时90秒,不合格 |
| | | AddExamFault(20402, rtkTime); |
| | | reportExamTimeout = true; |
| | | DEBUG("移库90秒超时"); |
| | | } |
| | | } |
| | | |
| | | if (moveStatus != prevMoveStatus) { |
| | | if (moveStatus == 0) { |
| | | parkStatus = 0; |
| | | stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); |
| | | storeMoveStatusBeforeStop = prevMoveStatus; |
| | | 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("停车时挡位 = %d", gearAtStop); |
| | | } else if (prevMoveStatus == 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); |
| | | uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); |
| | | |
| | | DEBUG("停车时间 %ld", tp - stopTimepoint); |
| | | DEBUG("再次移动时挡位 = %d", currGear == GEAR_R ? 1 : 0); |
| | | |
| | | if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_edge_pause_criteria) |
| | | && occurMoveBack |
| | | && gearAtStop == (currGear == GEAR_R ? 1 : 0)) { |
| | | // 停车超2秒,每次扣5分 |
| | | AddExamFault(20406, rtkTime); |
| | | DEBUG("停车超时"); |
| | | } |
| | | |
| | | if (moveStatus == 1 && checkPark && !checkLight) { |
| | | // 在这里检查转向灯状态 |
| | | checkLight = true; |
| | | if (ReadCarStatus(TURN_SIGNAL_LAMP) != LEFT_TURN_LIGHT) { |
| | | // 不开转向灯,扣10分 |
| | | AddExamFault(20405, rtkTime); |
| | | DEBUG("未开启转向灯"); |
| | | } |
| | | } |
| | | |
| | | /*if (moveStatus == storeMoveStatusBeforeStop) { |
| | | // 同方向再启动,继续判断是否停车超时 |
| | | if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_edge_pause_criteria) && occurMoveBack) { |
| | | // 停车超2秒,每次扣5分 |
| | | AddExamFault(20406, rtkTime); |
| | | DEBUG("停车超时"); |
| | | } |
| | | } else { |
| | | // 倒车切换到前进 |
| | | if (moveStatus == 1 && tp - stopTimepoint >= CHECK_PARK_DELAY) { |
| | | if (parkStatus == 0) { |
| | | if (EnterParking(map, car)) { |
| | | parkStatus = 1; |
| | | parkSuccess = true; |
| | | } |
| | | } |
| | | |
| | | if (!parkSuccess && !reportParkFail) { |
| | | // 停止后,车身出线,不合格 |
| | | AddExamFault(20401, rtkTime); |
| | | reportParkFail = true; |
| | | DEBUG("移库不入"); |
| | | } |
| | | |
| | | // 在这里检查转向灯状态 |
| | | if (ReadCarStatus(TURN_SIGNAL_LAMP) != LEFT_TURN_LIGHT) { |
| | | // 不开转向灯,扣10分 |
| | | AddExamFault(20405, rtkTime); |
| | | DEBUG("未开启转向灯"); |
| | | } |
| | | } |
| | | }*/ |
| | | |
| | | /* if (moveStatus == -1 && !occurMoveBack) { |
| | | DEBUG("开始倒车"); |
| | | occurMoveBack = true; |
| | | moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); |
| | | MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 1); |
| | | }*/ |
| | | } |
| | | |
| | | prevMoveStatus = moveStatus; |
| | | } /*else if (moveStatus == 0 && parkStatus == 0) { |
| | | uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); |
| | | |
| | | if (tp - stopTimepoint >= CHECK_PARK_DELAY) { |
| | | if (EnterParking(map, car)) { |
| | | parkStatus = 1; |
| | | parkSuccess = true; |
| | | } else { |
| | | parkStatus = -1; |
| | | } |
| | | } |
| | | }*/ |
| | | |
| | | TEST_END: |
| | | if (testStatus == TEST_FINISH) { |
| | | DEBUG("侧方停车结束"); |
| | | MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 0); |
| | | return 0; |
| | | } |
| | | return 1; |
| | | AppTimer_delete(ParkTimeout); |
| | | } |
| | | |
| | | int EnterParkEdge(prime_t &prime) { |
| | | if (prime.prev_modeling_index == -1 || prime.curr_modeling_index == -1) { |
| | | return -1; |
| | | } |
| | | |
| | | PointF &lp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]]; |
| | | PointF &lp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]]; |
| | | PointF &rp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]]; |
| | | PointF &rp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]]; |
| | | |
| | | Line left_trace, right_trace; |
| | | |
| | | MAKE_LINE(left_trace, lp1, lp2); |
| | | MAKE_LINE(right_trace, rp1, rp2); |
| | | |
| | | // 不同地区左右倒库次序可能不同,所以两个方向都可以进入 |
| | | for (int i = 0; i < prime.pMap->park_button_map.size(); ++i) { |
| | | Line left_entrance_line; |
| | | |
| | | MAKE_LINE(left_entrance_line, prime.pMap->park_button_map[i].map[1], prime.pMap->park_button_map[i].map[0]); |
| | | |
| | | if (IntersectionOf(left_trace, left_entrance_line) == GM_Intersection |
| | | && IntersectionOfLine(lp1, left_entrance_line) == RELATION_RIGHT) { |
| | | return i; |
| | | } |
| | | } |
| | | return -1; |
| | | } |
| | | |
| | | // 车轮是否压道路边线 |
| | | static bool CrashRedLine1(const Polygon *map, const car_model_t *car) |
| | | static void ParkTimeout(apptimer_var_t val) |
| | | { |
| | | bool ret = false; |
| | | // 超时90秒,不合格 |
| | | DEBUG("移库90秒超时"); |
| | | AddExamFault(20402); |
| | | } |
| | | |
| | | Line red_line; |
| | | const int red_lines[][2] = {{0, 7}, {1, 2}, {5, 6}}; |
| | | void TestParkEdge(prime_t &prime) |
| | | { |
| | | if (prime.examing_area.type != MAP_TYPE_PARK_EDGE) |
| | | return; |
| | | // 检测后轮是否驶过库位前段 |
| | | if (!overpass_parkingspace) { |
| | | overpass_parkingspace = OverpassParkspace(prime); |
| | | } |
| | | |
| | | if (prime.pMotion->move == BACKWARD && prime.sensor.gear == GEAR_R) { |
| | | if (!firstMoveBack) { |
| | | // 开始挂倒挡倒车 |
| | | firstMoveBack = true; |
| | | if (!overpass_parkingspace) { |
| | | DEBUG("后轮没有开过库位线就开始倒车"); |
| | | AddExamFault(10103); |
| | | } |
| | | prime.examing_area.stage = PE_PARKING; |
| | | AppTimer_delete(ParkTimeout); |
| | | AppTimer_add(ParkTimeout, prime.examParam.park_edge_limit_time); |
| | | } |
| | | } |
| | | |
| | | //从倒车状态,看是否从倒挡退出(如果只是看停车状态,无法和中途停车有效区分) |
| | | if (firstMoveBack && prime.sensor.gear != GEAR_R) { |
| | | if (!check_parking_space) { |
| | | check_parking_space = true; |
| | | if (!CheckParkspace(prime)) { |
| | | // 停止后,车身出线,不合格 |
| | | DEBUG("车身出线"); |
| | | AddExamFault(20401); |
| | | } |
| | | prime.examing_area.stage = PE_EXIT; |
| | | } |
| | | } |
| | | |
| | | // 检测左转向灯 |
| | | if (check_parking_space && prime.pMotion->move == FORWARD && !checkLight) { |
| | | checkLight = true; |
| | | if (prime.sensor.turn_signal_lamp != LEFT_TURN_LIGHT) { |
| | | // 不开转向灯,扣10分 |
| | | DEBUG("未开启转向灯"); |
| | | AddExamFault(20405); |
| | | } |
| | | } |
| | | |
| | | if (CrashRedLine(prime)) { |
| | | if (!occurCrashRedLine) { |
| | | // 车身压库位线,每次扣10分 |
| | | DEBUG("车身压库位线"); |
| | | AddExamFault(20404); |
| | | occurCrashRedLine = true; |
| | | } |
| | | } else { |
| | | occurCrashRedLine = false; |
| | | } |
| | | } |
| | | |
| | | // 车轮或车身是否压道路边线 |
| | | static bool CrashRedLine(prime_t &prime) |
| | | { |
| | | const int red_lines[][2] = {{0, 7}, {1, 2}, {2, 3}, {3, 4}, {4, 5}, {5, 6}}; |
| | | |
| | | Line frontAxle, rearAxle; |
| | | |
| | | 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]]); |
| | | MAKE_LINE(frontAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]]); |
| | | MAKE_LINE(rearAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]], prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]); |
| | | |
| | | 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, frontAxle) == GM_Intersection || |
| | | IntersectionOf(red_line, rearAxle) == GM_Intersection) { |
| | | ret = true; |
| | | break; |
| | | } |
| | | } |
| | | |
| | | return ret; |
| | | } |
| | | |
| | | // 车身是否压库位线 |
| | | static bool CrashRedLine2(const Polygon *map, const car_model_t *car) |
| | | { |
| | | bool ret = false; |
| | | |
| | | Line red_line; |
| | | const int red_lines[][2] = {{0, 7}, {2, 3}, {3, 4}, {4, 5}}; |
| | | |
| | | 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]]; |
| | | MakePolygon body(prime.pModel->body.size()); |
| | | for (int i = 0; i < prime.pModel->body.size(); ++i) { |
| | | body.AddPoint(prime.pModeling->points[prime.pModel->body[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) { |
| | | ret = true; |
| | | break; |
| | | } |
| | | } |
| | | |
| | | free(car_body.point); |
| | | return ret; |
| | | } |
| | | |
| | | static bool CrashRedLine3(const Polygon *map, const car_model_t *car) { |
| | | bool ret = false; |
| | | |
| | | if (!occurMoveBack) { |
| | | // 倒车前,车身不得压库位虚线 |
| | | Polygon car_body; |
| | | Line red_line; |
| | | |
| | | 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]]; |
| | | MAKE_LINE(red_line, std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[red_lines[i][0]], std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[red_lines[i][1]]); |
| | | if (IntersectionOf(red_line, frontAxle) == GM_Intersection |
| | | || IntersectionOf(red_line, rearAxle) == GM_Intersection |
| | | || IntersectionOf(red_line, body.GetPolygon()) != GM_None) { |
| | | return true; |
| | | } |
| | | |
| | | MakeLine(&red_line, &map->point[2], &map->point[5]); |
| | | if (IntersectionOf(red_line, &car_body) != GM_None) { |
| | | ret = true; |
| | | } |
| | | |
| | | free(car_body.point); |
| | | } |
| | | |
| | | return ret; |
| | | return false; |
| | | } |
| | | |
| | | static bool CheckParkspace(prime_t &prime) { |
| | | DEBUG("检查停车到位..."); |
| | | |
| | | MakePolygon area({std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[8], |
| | | std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[3], |
| | | std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[4], |
| | | std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[9]}); |
| | | MakePolygon car_body(prime.pModel->body.size()); |
| | | for (int i = 0; i < prime.pModel->body.size(); ++i) { |
| | | car_body.AddPoint(prime.pModeling->points[prime.pModel->body[i]]); |
| | | } |
| | | |
| | | return (IntersectionOf(car_body.GetPolygon(), area.GetPolygon()) == GM_Containment)? true : false; |
| | | } |
| | | |
| | | static bool OverpassParkspace(prime_t &prime) |
| | | { |
| | | Line parkspace_top; |
| | | |
| | | MAKE_LINE(parkspace_top, std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[4], std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[5]); |
| | | |
| | | if (IntersectionOfLine(prime.pModeling->points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]], parkspace_top) == REL_POS_RIGHT |
| | | && IntersectionOfLine(prime.pModeling->points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]], parkspace_top) == REL_POS_RIGHT) { |
| | | return true; |
| | | } |
| | | return false; |
| | | } |