// // Created by YY on 2020/3/17. // #include "road_exam.h" #include "../driver_test.h" #include "../utils/xconvert.h" #include "../common/apptimer.h" #include "../jni_log.h" #include "../defs.h" #include "../test_common/car_sensor.h" #include "../native-lib.h" #include "through_something.h" #include "../master/comm_if.h" #include #include #include #include #define DEBUG(fmt, args...) LOGD(" <%s>: " fmt, __func__, ##args) using namespace std; #define TURN_CHECK_CNT 4 static const int TURN_THRESHOLD = 5; static const int TURN_CHECK_INTERVAL = 500; const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3; const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1; static bool occurCrashRedLine; static bool occurCrashGreenLine; static bool occurOverSpeed; static int checkCrashGreenTimeout; static char carIntersectionOfGreenLine; static int currTurnSignalStatus; static int turnSignalStatusWhenCrashGreenLine; static bool reportTurnSignalError; static int prevMoveDirect; static uint32_t stopTimepoint = 0; static bool reportStopCarOnRedArea; static PointF stopPoint; static bool prevGearError = false; static bool prevGearNSlide = false; static bool slideLongDistance; static bool slideNormalDistance; static bool occurSlide; static struct drive_timer { int hour; int min; int sec; int msec; } crashGreenRunTime, crashGreenCmpTime, crashGreenStartTime, turnSignalChangeTime; static struct drive_timer gearErrorTimePoint; static struct drive_timer gearNSlideTimePoint; static int gearErrorTime; static int gearNSlideTime; static int currExamMapIndex; static const uint32_t GEAR_N_SLIDE_TIMEOUT = D_SEC(5); static const uint32_t GEAR_ERROR_TIMEOUT = D_SEC(15); static const uint32_t STOP_CAR_TIME = D_SEC(2); static const uint32_t CHANGE_ROAD_MIN_INTERVAL = D_SEC(10); static const uint32_t CRASH_DOTTED_LINE_TIMEOUT = D_SEC(10); static const uint32_t TURN_SIGNAL_LAMP_ADVANCE = D_SEC(3); static const double MAX_SPEED = 40.0 * 1000.0 / 3600.0; static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}}; static void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime); static char isTurn(int currYaw, int prevYaw); static char CheckCarTurn(LIST_CAR_MODEL &CarModelList); static bool CrashRedLine(LIST_ROAD_MAP &RoadMapList, const car_model *car); static bool CrashRedArea(LIST_ROAD_MAP &RoadMapList, const car_model *car); static bool CrashGreenLine(LIST_ROAD_MAP &RoadMapList, const car_model *car, PointF &p1, PointF &p2); static int CrashTriggerLine(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList); static int FindMapIndexById(int id, LIST_ROAD_MAP &RoadMapList); void InitRoadExam(void) { DEBUG("Start road_exam"); crashGreenCmpTime.hour = -1; occurCrashRedLine = false; occurCrashGreenLine = false; occurOverSpeed = false; checkCrashGreenTimeout = 0; carIntersectionOfGreenLine = 0; reportTurnSignalError = false; currTurnSignalStatus = OFF_LIGHT; prevMoveDirect = 0; reportStopCarOnRedArea = false; occurSlide = false; slideLongDistance = false; slideNormalDistance = false; prevGearError = false; gearErrorTime = 0; prevGearNSlide = false; gearNSlideTime = 0; currExamMapIndex = -1; } void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) { // 超速检测 if (moveDirect != 0 && speed > MAX_SPEED) { if (!occurOverSpeed) { occurOverSpeed = true; // 超速,不合格 AddExamFault(10, rtkTime); } } else { occurOverSpeed = false; } // 挡位匹配检测 bool currGearError = false; bool currGearNSlide = false; switch (ReadCarStatus(GEAR)) { case GEAR_N: if (moveDirect != 0) { // 空档滑行 currGearNSlide = true; } break; case GEAR_1: if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[0][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[0][1]) { currGearError = true; } break; case GEAR_2: if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[1][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[1][1]) { currGearError = true; } break; case GEAR_3: if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[2][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[2][1]) { currGearError = true; } break; case GEAR_4: if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[3][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[3][1]) { currGearError = true; } break; case GEAR_5: if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[4][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[4][1]) { currGearError = true; } break; default:break; } // 空档滑行超时 if (currGearNSlide && prevGearNSlide) { gearNSlideTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10, gearNSlideTimePoint.hour, gearNSlideTimePoint.min, gearNSlideTimePoint.sec, gearNSlideTimePoint.msec*10); } if (gearNSlideTime > GEAR_N_SLIDE_TIMEOUT) { // 空档滑行超5秒,不合格 DEBUG("挡位滑行,超过5秒"); AddExamFault(8, rtkTime); gearNSlideTime = 0; } prevGearNSlide = currGearNSlide; if (prevGearNSlide) { Rtk2DriveTimer(gearNSlideTimePoint, rtkTime); } else { gearNSlideTime = 0; } // 挡位不匹配超时 if (currGearError && prevGearError) { gearErrorTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10, gearErrorTimePoint.hour, gearErrorTimePoint.min, gearErrorTimePoint.sec, gearErrorTimePoint.msec*10); } if (gearErrorTime > GEAR_ERROR_TIMEOUT) { // 累计15秒,挡位-车速不匹配,不合格 DEBUG("挡位错误超过15秒"); AddExamFault(6, rtkTime); gearErrorTime = 0; } prevGearError = currGearError; if (prevGearError) { Rtk2DriveTimer(gearErrorTimePoint, rtkTime); } // 起步后滑 if (moveDirect != prevMoveDirect) { if (moveDirect == 0) { stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); reportStopCarOnRedArea = false; DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss); } else if (moveDirect == -1 && prevMoveDirect == 0) { DEBUG("开始后滑"); stopPoint = car->basePoint; occurSlide = true; } prevMoveDirect = moveDirect; } else if (moveDirect == 0) { uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); if (tp - stopTimepoint >= STOP_CAR_TIME && !reportStopCarOnRedArea && CrashRedArea(RoadMapList, car)) { // 停车超2秒,停在红区,不合格 AddExamFault(16, rtkTime); DEBUG("禁停区停车"); reportStopCarOnRedArea = true; } } else if (moveDirect == -1) { if (occurSlide) { double slideDistance = DistanceOf(stopPoint, car->basePoint); if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) { slideNormalDistance = true; } if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED && !slideLongDistance) { // 后滑超过30厘米, 不合格 AddExamFault(5, rtkTime); DEBUG("后滑超过30厘米"); slideLongDistance = true; } } } else { if (slideNormalDistance) { // 后滑,扣10分 AddExamFault(18, rtkTime); DEBUG("后滑超过10厘米, 但不超过30厘米"); } slideNormalDistance = false; slideLongDistance = false; occurSlide = false; } switch (ReadCarStatus(TURN_SIGNAL_LAMP)) { case LEFT_TURN_LIGHT: if (currTurnSignalStatus != LEFT_TURN_LIGHT) { currTurnSignalStatus = LEFT_TURN_LIGHT; Rtk2DriveTimer(turnSignalChangeTime, rtkTime); } break; case RIGHT_TURN_LIGHT: if (currTurnSignalStatus != RIGHT_TURN_LIGHT) { currTurnSignalStatus = RIGHT_TURN_LIGHT; Rtk2DriveTimer(turnSignalChangeTime, rtkTime); } break; default: currTurnSignalStatus = ReadCarStatus(TURN_SIGNAL_LAMP); break; } // 检查是否持续转向 char turnDirect = CheckCarTurn(CarModelList); if (turnDirect == 'L') { PlayTTS("左1", 5692); if (currTurnSignalStatus != LEFT_TURN_LIGHT) { if (!reportTurnSignalError) { DEBUG("没打左转灯"); // 没打左转灯,不合格 AddExamFault(13, rtkTime); reportTurnSignalError = true; } } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10, turnSignalChangeTime.hour, turnSignalChangeTime.min, turnSignalChangeTime.sec, turnSignalChangeTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) { if (!reportTurnSignalError) { DEBUG("转向灯时间不足"); // 不足3秒,不合格 AddExamFault(14, rtkTime); reportTurnSignalError = true; } } } else if (turnDirect == 'R') { PlayTTS("右1", 5692); if (currTurnSignalStatus != RIGHT_TURN_LIGHT) { if (!reportTurnSignalError) { DEBUG("没打右转灯"); // 没打右转灯,不合格 AddExamFault(13, rtkTime); reportTurnSignalError = true; } } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10, turnSignalChangeTime.hour, turnSignalChangeTime.min, turnSignalChangeTime.sec, turnSignalChangeTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) { if (!reportTurnSignalError) { DEBUG("转向灯时间不足"); // 不足3秒,不合格 AddExamFault(14, rtkTime); reportTurnSignalError = true; } } } else { reportTurnSignalError = false; } // 撞红线 if (CrashRedLine(RoadMapList, car)) { if (!occurCrashRedLine) { // 车辆行驶中骑轧车道中心实线或者车道边缘实线,不合格 DEBUG("撞道路边缘线"); AddExamFault(11, rtkTime); occurCrashRedLine = true; } } else { occurCrashRedLine = false; } // 撞绿线 static PointF p1, p2; if (CrashGreenLine(RoadMapList, car, p1, p2)) { // 压虚线 if (moveDirect != 0) { if (checkCrashGreenTimeout == 0) { checkCrashGreenTimeout = 1; Rtk2DriveTimer(crashGreenRunTime, rtkTime); // 运动中压虚线的开始时间点 } else if (checkCrashGreenTimeout == 1) { uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10, crashGreenRunTime.hour, crashGreenRunTime.min, crashGreenRunTime.sec, crashGreenRunTime.msec*10); if (diff >= CRASH_DOTTED_LINE_TIMEOUT) { DEBUG("长时间压虚线"); checkCrashGreenTimeout = 2; // 长时间骑轧车道分界线行驶,不合格 AddExamFault(12, rtkTime); } } } else { // 停车就不计时了 checkCrashGreenTimeout = 0; } // 检测当前车辆于虚线的位置,做变道检测; // 检测是否3秒前有开启对应之转向灯 if (!occurCrashGreenLine) { occurCrashGreenLine = true; // 记录开始压线的时间,不确定是否有变道意图,待确认变道后再处理之 Rtk2DriveTimer(crashGreenStartTime, rtkTime); turnSignalStatusWhenCrashGreenLine = currTurnSignalStatus; } // p1 ---------------> p2 double angle = car->yaw - YawOf(p2, p1); if (angle < 0 || angle > 180) { // 右侧 carIntersectionOfGreenLine = 'R'; } else { // 左侧 carIntersectionOfGreenLine = 'L'; } } else { // 不再压虚线 if (occurCrashGreenLine) { int inter = IntersectionOfLine(p1, p2, car->basePoint); // 完成跨线动作 if ((inter == 1 && carIntersectionOfGreenLine == 'R') || (inter == -1 && carIntersectionOfGreenLine == 'L')) { // 比较上次跨线时间 if (crashGreenCmpTime.hour >= 0) { uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss * 10, crashGreenCmpTime.hour, crashGreenCmpTime.min, crashGreenCmpTime.sec, crashGreenCmpTime.msec * 10); if (diff < CHANGE_ROAD_MIN_INTERVAL) { DEBUG("===================== 连续变道 ============!!"); // 连续变道,不合格 AddExamFault(15, rtkTime); } } // 记录本次变道时间点 Rtk2DriveTimer(crashGreenCmpTime, rtkTime); // 检查变道前,是否提前转向灯 if (inter == 1) { PlayTTS("左2", 5698); // 向左侧变道 DEBUG("向左侧变道"); if (turnSignalStatusWhenCrashGreenLine != LEFT_TURN_LIGHT) { DEBUG("变调未打灯!!"); // 没打灯,不合格 AddExamFault(13, rtkTime); } } else { PlayTTS("右2", 5698); // 向右侧变道 DEBUG("向右侧变道"); if (turnSignalStatusWhenCrashGreenLine != RIGHT_TURN_LIGHT) { DEBUG("变调未打灯!!"); // 没打灯,不合格 AddExamFault(13, rtkTime); } } } } occurCrashGreenLine = false; checkCrashGreenTimeout = 0; } // 触发线检测 if (currExamMapIndex == -1) { currExamMapIndex = CrashTriggerLine(RoadMapList, car, CarModelList); if (currExamMapIndex != -1) { DEBUG("碰撞触发线"); MA_EnterMap(RoadMapList[currExamMapIndex].id, RoadMapList[currExamMapIndex].type, 1); StartThroughExam(currExamMapIndex, RoadMapList); } } else { int prevIdx = currExamMapIndex; currExamMapIndex = ExecuteThroughExam(currExamMapIndex, RoadMapList, car, CarModelList, speed, moveDirect, rtkTime); if (currExamMapIndex == -1) { MA_EnterMap(RoadMapList[prevIdx].id, RoadMapList[prevIdx].type, 1); } } } bool ExitSonArea(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car) { bool ret = false; if (index < 0 || index > RoadMapList.size()) return true; if (RoadMapList[index].area.point != NULL) { // 需要车身全部离开 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, &RoadMapList[index].area) == GM_None) { ret = true; } free(carBody.point); } return ret; } bool CrashSonRedLine(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList) { bool ret = false; if (CarModelList.size() < 5 || index < 0 || index >= RoadMapList.size()) return ret; Polygon trace; trace.num = 5; // 最近5个轮轨迹 trace.point = (PointF *) malloc(sizeof(PointF) * trace.num); list::iterator iter = CarModelList.begin(); int pn = 0; while (iter != CarModelList.end() && pn < trace.num) { trace.point[pn++] = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]]; ++iter; } // 每条线都检测 for (int j = 0; j < RoadMapList[index].redLineNum; ++j) { Line red_line; int kp = 0; // 触发线一般应该只有首尾2点(id, p1, p2) for (int k = 1; k < RoadMapList[index].redLine[j].num; ++k) { MakeLine(&red_line, &RoadMapList[index].redLine[j].point[kp], &RoadMapList[index].redLine[j].point[k]); int pp = 0; for (int p = 1; p < pn; ++p) { Line trace_line; MakeLine(&trace_line, &trace.point[pp], &trace.point[p]); if (IntersectionOf(trace_line, red_line) == GM_Intersection) { // 碰到触发线 ret = true; goto SEARCH_SON_RED_LINE_END; } pp = p; } kp = k; } } SEARCH_SON_RED_LINE_END: free(trace.point); return ret; } static void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime) { tm.hour = rtkTime->hh; tm.min = rtkTime->mm; tm.sec = rtkTime->ss; tm.msec = rtkTime->mss; } static char isTurn(int currYaw, int prevYaw) { // DEBUG("currYaw %d prevYaw %d", currYaw, prevYaw); int deltaAng = 0; if (ABS(currYaw - prevYaw) > 180) { deltaAng = 360 - ABS(currYaw-prevYaw); } else { deltaAng = ABS(currYaw - prevYaw); } // DEBUG("角度差值 %d", deltaAng); if (deltaAng >= TURN_THRESHOLD) { if((( currYaw + 360 - prevYaw) % 360) < 180) { // DEBUG("右转"); return 'R'; } else { // DEBUG("左转"); return 'L'; } } return 0; } static char CheckCarTurn(LIST_CAR_MODEL &CarModelList) { // 最近3秒内,每秒的角度差大于10度,且方向相同,连续3秒,认为转向 if (CarModelList.size() < 1) return false; list::iterator iter = CarModelList.begin(); car_model *c1 = *iter, *c2; ++iter; char turn[TURN_CHECK_CNT] = {0}; int checkCnt = 0; // DEBUG("CheckCarTurn........."); while (iter != CarModelList.end() && checkCnt < TURN_CHECK_CNT) { c2 = *iter; uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10); if (tdiff >= TURN_CHECK_INTERVAL) { turn[checkCnt] = isTurn((int)c1->yaw, (int)c2->yaw); // DEBUG("%c 角度比较 %02d:%02d:%02d.%03d %02d:%02d:%02d.%03d", turn[checkCnt], c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10); if (turn[checkCnt] == 0) { break; } c1 = c2; checkCnt++; } ++iter; } int i = 0; for (; checkCnt == TURN_CHECK_CNT && i < TURN_CHECK_CNT-1; ++i) { if (turn[i] != turn[i+1]) break; } if (i == TURN_CHECK_CNT-1) { DEBUG("左右转确认 %c", turn[0]); return turn[0]; } return 0; } /***************************************************** * CrashRedLine 整个考试区域的道路边缘线,实线等。 * 按车轮碰触计算,车身不计 */ static bool CrashRedLine(LIST_ROAD_MAP &RoadMapList, const car_model *car) { 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]]); for (int i = 0; i < RoadMapList.size(); ++i) { if (RoadMapList[i].type == GENERAL_MAP) { // 每条线都检测 for (int j = 0; j < RoadMapList[i].redLineNum; ++j) { Line red_line; int kp = 0; for (int k = 1; k < RoadMapList[i].redLine[j].num; ++k) { MakeLine(&red_line, &RoadMapList[i].redLine[j].point[kp], &RoadMapList[i].redLine[j].point[k]); if (IntersectionOf(red_line, frontAxle) == GM_Intersection || IntersectionOf(red_line, rearAxle) == GM_Intersection) { return true; } kp = k; } } break; } } return false; } /********************************************************** * 按整个车身是否覆盖计算 * @param RoadMapList * @param car * @return */ static bool CrashRedArea(LIST_ROAD_MAP &RoadMapList, 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]]; } for (int i = 0; i < RoadMapList.size(); ++i) { if (RoadMapList[i].type == GENERAL_MAP) { // 每条红区都检测 for (int j = 0; j < RoadMapList[i].redAreaNum; ++j) { if (IntersectionOf(&carBody, &RoadMapList[i].redArea[j]) != GM_None) { ret = true; } } break; } } free(carBody.point); return ret; } /************************************************** * 车轮触碰道路虚线。检测行驶时间超长;变道情况; * @param RoadMapList * @param car */ static bool CrashGreenLine(LIST_ROAD_MAP &RoadMapList, const car_model *car, PointF &p1, PointF &p2) { 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]]); for (int i = 0; i < RoadMapList.size(); ++i) { if (RoadMapList[i].type == GENERAL_MAP) { // 每条线都检测 for (int j = 0; j < RoadMapList[i].greenLineNum; ++j) { Line green_line; int kp = 0; for (int k = 1; k < RoadMapList[i].greenLine[j].num; ++k) { MakeLine(&green_line, &RoadMapList[i].greenLine[j].point[kp], &RoadMapList[i].greenLine[j].point[k]); if (IntersectionOf(green_line, frontAxle) == GM_Intersection || IntersectionOf(green_line, rearAxle) == GM_Intersection) { p1 = RoadMapList[i].greenLine[j].point[kp]; p2 = RoadMapList[i].greenLine[j].point[k]; return true; } kp = k; } } break; } } return false; } /************************************************************ * 检测车辆是否触发子考项地图 * @param RoadMapList * @param car * @param CarModelList * @return */ static int CrashTriggerLine(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList) { int map_idx = -1; if (CarModelList.size() < 5) return map_idx; Polygon trace; trace.num = 5; trace.point = (PointF *) malloc(sizeof(PointF) * trace.num); list::iterator iter = CarModelList.begin(); int pn = 0; while (iter != CarModelList.end() && pn < trace.num) { trace.point[pn++] = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]]; ++iter; } for (int i = 0; i < RoadMapList.size(); ++i) { if (RoadMapList[i].type == GENERAL_MAP) { // 每条线都检测 for (int j = 0; j < RoadMapList[i].triggerLineNum; ++j) { Line trigger_line; int kp = 0; // 触发线一般应该只有首尾2点(id, p1, p2) for (int k = 1; k < RoadMapList[i].triggerLine[j].line.num; ++k) { MakeLine(&trigger_line, &RoadMapList[i].triggerLine[j].line.point[kp], &RoadMapList[i].triggerLine[j].line.point[k]); int pp = 0; for (int p = 1; p < pn; ++p) { Line trace_line; MakeLine(&trace_line, &trace.point[pp], &trace.point[p]); if (IntersectionOf(trace_line, trigger_line) == GM_Intersection && IntersectionOfLine(RoadMapList[i].triggerLine[j].line.point[kp], RoadMapList[i].triggerLine[j].line.point[k], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 && DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], trigger_line) > 0.1) { // 碰到触发线 DEBUG("碰撞触发线 引发地图 id = %d", RoadMapList[i].triggerLine[j].triggerMapId); map_idx = FindMapIndexById(RoadMapList[i].triggerLine[j].triggerMapId, RoadMapList); goto SEARCH_TRIGGER_LINE_END; } pp = p; } kp = k; } } break; } } SEARCH_TRIGGER_LINE_END: free(trace.point); return map_idx; } static int FindMapIndexById(int id, LIST_ROAD_MAP &RoadMapList) { for (int i = 0; i < RoadMapList.size(); ++i) { if (RoadMapList[i].id == id) { return i; } } return -1; }