From 00c07fb94c6927f3bb8ebfec383a33936f04447b Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期五, 22 五月 2020 19:01:18 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/road_exam.cpp | 512 ++++++++++++++++++++++++++++++++++++--------------------
1 files changed, 324 insertions(+), 188 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index a2205f9..d132808 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -40,6 +40,14 @@
static const int TURN_CHECK_INTERVAL = 500;
const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
+const double CHANGE_LANE_RANGE = 100.0;
+const double OVERTAKE_RANGE = 150.0;
+const double OVERTAKE_HOLD_RANGE = 30.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;
@@ -75,13 +83,15 @@
static int startCar;
static int currExamMapIndex;
+static trigger_line_t *currRoadItem;
static int nextRoadId;
+static PointF roadItemStartPoint;
+static struct drive_timer roadItemStartTime;
+static bool overtake = false;
static bool checkDoor = false;
static bool handBreakActive = false;
static bool reportRPMOver = false;
-
-static int currCarOnRoadIndex;
static const uint32_t TURN_ERROR_COLD_TIME = D_SEC(10);
@@ -90,6 +100,8 @@
int road;
int separate;
int lane;
+ int direct; // 杞﹂亾鏂瑰悜闄愬埗
+ int type; // 瀹炵嚎锛岃櫄绾�
} CurrentLane;
static bool laneChanging;
@@ -103,7 +115,6 @@
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 NEXT_ROAD_TIP = 100.0; // 鍒拌揪璺彛鍓嶆彁绀轰笅涓�涓�庝箞璧�
static const int CRL_NONE = 0;
static const int CRL_SEP_DOTTED = 1;
@@ -118,11 +129,6 @@
static char isTurn(int currYaw, int prevYaw, int &ang);
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);
static void TurnSignalError13ColdTimer(union sigval sig);
static void TurnSignalError14ColdTimer(union sigval sig);
@@ -132,6 +138,10 @@
static bool LaneIsSame(struct car_on_lane lane1, struct car_on_lane lane2);
static bool LaneIsValid(struct car_on_lane lane);
static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList);
+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 void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList,
+ double speed, int moveDirect, const struct RtkTime *rtkTime);
void InitRoadExam(road_exam_map &RoadMap)
{
@@ -141,6 +151,7 @@
occurCrashRedLine = false;
occurCrashGreenLine = false;
occurOverSpeed = false;
+
occurSecondBreak = false;
checkCrashGreenTimeout = 0;
carIntersectionOfGreenLine = 0;
@@ -161,6 +172,7 @@
currExamMapIndex = FIND_POSITION;
startCar = START_CAR_NOT_DO;
+ currRoadItem = NULL;
checkDoor = false;
handBreakActive = false;
@@ -170,12 +182,11 @@
turnError13Cold = turnError14Cold = true;
- currCarOnRoadIndex = FIND_POSITION;
-
CurrentLane.road = CurrentLane.separate = CurrentLane.lane = -1;
laneChanging = false;
- InitThroughSomething(RoadMap);
+ nextRoadId = -1;
+ ClearAll(RoadMap);
}
void TerminateRoadExam(void)
@@ -295,14 +306,12 @@
void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
- // 璧锋妫�娴�
- TestRoadStartCar(car, speed, moveDirect, rtkTime);
-
// 瓒呴�熸娴�
if (moveDirect != 0 && speed > MAX_SPEED) {
if (!occurOverSpeed) {
occurOverSpeed = true;
// 瓒呴�燂紝涓嶅悎鏍�
+ DEBUG("瓒呴�� %f", speed);
AddExamFault(10, rtkTime);
}
} else {
@@ -470,7 +479,8 @@
}
// 妫�娴嬮�氳繃璺彛銆佷汉琛岄亾绛夊尯鍩熸椂锛岄噴鏀惧埞杞︽垨鍑忛��
- CheckBreakActive(RoadMap, car, CarModelList);
+ CheckBreakActive(RoadMap, car, CarModelList,
+ speed, moveDirect, rtkTime);
// 妫�娴嬬寮�姝よ矾娈碉紝鍏ㄨ溅闇�涓嶅湪鑼冨洿鍐�
if (currExamMapIndex >= 0) {
@@ -579,6 +589,11 @@
if (IntersectionOf(car->carXY[car->axial[AXIAL_FRONT]], &area) == GM_Containment) {
currExamMapIndex = i;
DEBUG("杩涘叆閬撹矾 id = %d", RoadMap.roads[i].id);
+ if (nextRoadId >= 0 && RoadMap.roads[i].id != nextRoadId) {
+ DEBUG("涓嶆寜瑙勭煩琛岄┒锛岃繘鍏ラ敊璇矾娈�");
+ AddExamFault(3, rtkTime);
+ }
+ nextRoadId = -1;
break;
}
@@ -608,16 +623,16 @@
if (crl == CRL_NONE) {
// DEBUG("浠�涔堥兘娌″帇");
} else if (crl == CRL_SEP_DOTTED) {
-// DEBUG("鍘嬪垎閬撹櫄绾�");
+ DEBUG("鍘嬪垎閬撹櫄绾�");
crashGreenLineNow = true;
} else if (crl == CRL_SEP_SOLID) {
-// DEBUG("鍘嬪垎閬撳疄绾�");
+ DEBUG("鍘嬪垎閬撳疄绾�");
crashRedLineNow = true;
} else if (crl == CRL_EDGE_DOTTED) {
-// DEBUG("鍘嬭竟娌胯櫄绾�");
+ DEBUG("鍘嬭竟娌胯櫄绾�");
crashGreenLineNow = true;
} else if (crl == CRL_EDGE_SOLID) {
-// DEBUG("鍘嬭竟娌垮疄绾�");
+ DEBUG("鍘嬭竟娌垮疄绾�");
crashRedLineNow = true;
}
@@ -700,9 +715,23 @@
} else {
// 鍙橀亾瀹屾垚
DEBUG("鍙橀亾瀹屾垚");
+
+ if (currRoadItem != NULL && currRoadItem->active == ROAD_ITEM_CHANGE_LANE) {
+ DEBUG("鍙樻洿杞﹂亾椤圭洰瀹屾垚");
+ currRoadItem = NULL;
+ } else if (currRoadItem != NULL && currRoadItem->active == ROAD_ITEM_OVERTAKE) {
+ if (CurrentLane.lane > lane.lane) {
+ DEBUG("瓒呰溅鍙橀亾瀹屾垚");
+
+ } else {
+ DEBUG("鍙抽亾瓒呰溅锛岄敊璇�");
+ AddExamFault(3, rtkTime);
+ currRoadItem = NULL;
+ }
+ }
+
CurrentLane = lane;
laneChanging = false;
-
// 璁板綍鏈鍙橀亾鏃堕棿鐐�
Rtk2DriveTimer(crashGreenCmpTime, rtkTime);
}
@@ -710,11 +739,20 @@
if (lane.road != CurrentLane.road || lane.separate != CurrentLane.separate) {
// 璺垨娈靛彉鏇达紝鎾ら攢鍙橀亾璺熻釜
- DEBUG("============== 璺垨娈靛彉鏇� CURR %d, %d NEW %d, %d", CurrentLane.road,
- CurrentLane.separate,
- lane.road, lane.separate);
+ DEBUG("============== 璺垨娈靛彉鏇� CURR %d, %d dir %d NEW %d, %d, dir %d", CurrentLane.road,
+ CurrentLane.separate, CurrentLane.direct,
+ lane.road, lane.separate, lane.direct);
+
CurrentLane = lane;
laneChanging = false;
+ }
+
+ if (CurrentLane.direct != 0 && !(CurrentLane.direct & RoadMap.roads[currExamMapIndex].active)) {
+ if (!RoadMap.roads[currExamMapIndex].errorLane) {
+ RoadMap.roads[currExamMapIndex].errorLane = true;
+ DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囩嚎鏂瑰悜琛岄┒ %d", CurrentLane.direct);
+ AddExamFault(9, rtkTime);
+ }
}
}
@@ -773,6 +811,50 @@
// 涓嶅啀鍘嬭櫄绾�
occurCrashGreenLine = false;
checkCrashGreenTimeout = 0;
+ }
+
+ TestRoadStartCar(car, speed, moveDirect, rtkTime);
+
+ if (startCar != START_CAR_DONE)
+ return;
+
+ // 妫�娴嬬敱瑙﹀彂绾挎帶鍒剁殑椤圭洰
+ if (currRoadItem != NULL) {
+ if (currRoadItem->active == ROAD_ITEM_CHANGE_LANE) {
+ if (DistanceOf(car->basePoint, roadItemStartPoint) > CHANGE_LANE_RANGE) {
+ DEBUG("鍙橀亾璺濈瓒呮爣");
+ AddExamFault(3, rtkTime);
+ currRoadItem = NULL;
+ }
+ } else if (currRoadItem->active == ROAD_ITEM_OVERTAKE) {
+ if (!overtake && DistanceOf(car->basePoint, roadItemStartPoint) > OVERTAKE_RANGE) {
+ DEBUG("瓒呰溅璺濈瓒呮爣");
+ AddExamFault(3, rtkTime);
+ currRoadItem = NULL;
+ } else if (overtake && DistanceOf(car->basePoint, roadItemStartPoint) > OVERTAKE_HOLD_RANGE) {
+ DEBUG("鍥炲師杞﹂亾");
+ PlayTTS("璇疯繑鍥炲師杞﹂亾");
+ currRoadItem = NULL;
+ }
+ } else if (currRoadItem->active == ROAD_ITEM_OPERATE_GEAR) {
+ ExecuteOperateGearExam(rtkTime);
+ }
+ } else if (currExamMapIndex >= 0) {
+ currRoadItem = EntryItem(currExamMapIndex, RoadMap, car, CarModelList);
+ if (currRoadItem != NULL) {
+ if (!currRoadItem->tts.empty())
+ PlayTTS(currRoadItem->tts.c_str());
+
+ // 鍒濆鏃堕棿鍜岃窛绂婚檺鍒�
+ roadItemStartPoint = car->basePoint;
+ Rtk2DriveTimer(roadItemStartTime, rtkTime);
+
+ if (currRoadItem->active == ROAD_ITEM_OVERTAKE) {
+ overtake = false;
+ } else if (currRoadItem->active == ROAD_ITEM_OPERATE_GEAR) {
+ StartOperateGearExam(rtkTime);
+ }
+ }
}
}
@@ -839,7 +921,7 @@
PointF p1, p2;
Line sep;
- map<int, int> orthogonal;
+ map<int, vector<int>> orthogonal;
// 涓�娈靛垎閬撶粍鍐咃紝鏈変竴鏉℃浜わ紝灏卞繀椤讳繚璇佸悓缁勭殑鍏ㄩ兘姝ｄ氦锛屽惁鍒欑洿鎺ラ��鍑�
for (int j = 0; j < road.separate[i].lines.size(); ++j) { // 绾跨粍
@@ -853,15 +935,31 @@
MakeLine(&sep, &p1, &p2);
if (IntersectionOf(leftExtLine, sep) == GM_Intersection) {
- orthogonal.insert(pair<int, int>(j, 1));
+ vector<int> stor(4);
+
+ stor[0] = 1;
+ stor[1] = road.separate[i].lines[j][k].character;
+ stor[2] = road.separate[i].lines[j][k].left_lane_direct;
+ stor[3] = road.separate[i].lines[j][k].right_lane_direct;
+
+ orthogonal.insert(pair<int, vector<int>>(j, stor));
orthogonalInSegment = true;
intersection = true;
+
// DEBUG("鍒嗛亾绾� %d 宸︽浜�", j);
break;
} else if (IntersectionOf(rightExtLine, sep) == GM_Intersection) {
- orthogonal.insert(pair<int, int>(j, 2));
+ vector<int> stor(4);
+
+ stor[0] = 2;
+ stor[1] = road.separate[i].lines[j][k].character;
+ stor[2] = road.separate[i].lines[j][k].left_lane_direct;
+ stor[3] = road.separate[i].lines[j][k].right_lane_direct;
+
+ orthogonal.insert(pair<int, vector<int>>(j, stor));
orthogonalInSegment = true;
intersection = true;
+
// DEBUG("鍒嗛亾绾� %d 鍙虫浜�", j);
break;
}
@@ -875,16 +973,26 @@
if (orthogonal.size() == road.separate[i].lines.size()) {
// 寰楀埌褰撳墠鍦ㄧ鍑犱釜杞﹂亾
int x = 0;
+ int left_char = LINE_SOLID; // 閬撹矾宸﹁竟绾�
+ int right_direct = 0;
+
for (x = 0; x < orthogonal.size(); ++x) {
auto itx = orthogonal.find(x);
if (itx != orthogonal.end()) {
- if (itx->second != 1) {
+ if (itx->second[0] != 1) { // 鍦ㄨ溅杈嗗彸渚�
lane.road = road.id;
lane.separate = i;
lane.lane = itx->first;
-
+ if ((left_char == LINE_SOLID || left_char == LINE_HALF_SOLID_RIGHT) &&
+ (itx->second[1] == LINE_SOLID || itx->second[1] == LINE_HALF_SOLID_LEFT)) // 杞﹂亾宸﹀彸鍧囨槸瀹炵嚎
+ lane.direct = itx->second[2];
+ else
+ lane.direct = 0;
// DEBUG("璺� %d 娈� %d 杞﹂亾 %d", lane.road, lane.separate, lane.lane);
break;
+ } else {
+ right_direct = itx->second[3];
+ left_char = itx->second[2];
}
}
}
@@ -892,7 +1000,11 @@
lane.road = road.id;
lane.separate = i;
lane.lane = orthogonal.size();
-
+ // 鏈�鍚庤溅閬撶殑鍙充晶闄愬畾
+ if (left_char == LINE_SOLID || left_char == LINE_HALF_SOLID_RIGHT)
+ lane.direct = right_direct;
+ else
+ lane.direct = 0;
// DEBUG("璺� %d 娈� %d 杞﹂亾 %d", lane.road, lane.separate, lane.lane);
}
out = lane;
@@ -1026,6 +1138,7 @@
return false;
}
+#if 0
void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
// 璧锋妫�娴�
@@ -1383,6 +1496,7 @@
}
}
+
bool ExitSonArea(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car)
{
bool ret = false;
@@ -1464,6 +1578,7 @@
return ret;
}
+#endif
void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
{
@@ -1556,48 +1671,13 @@
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)
+/*static bool CrashRedArea(LIST_ROAD_MAP &RoadMapList, const car_model *car)
{
bool ret = false;
@@ -1625,119 +1705,7 @@
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<car_model *>::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;
-}
+}*/
bool CrashTheLine(Line line, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
@@ -1759,16 +1727,6 @@
return true;
}
return false;
-}
-
-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;
}
/*********************************************************************
@@ -1822,6 +1780,12 @@
return px;
}
+/***************************************************
+ * 鎺ヨ繎璺彛鏃讹紝鎻愮ず涓嬩竴姝ユ�庝箞璧�
+ * @param road
+ * @param car
+ * @param CarModelList
+ */
static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
// 璁＄畻杞﹀墠杩涜建杩瑰欢闀跨嚎
@@ -1836,6 +1800,7 @@
IntersectionOfLine(extPoint, road.stopLine) == -1) {
if (DistanceOf(extPoint, road.stopLine) > 1.0 && !road.arrivedTail) {
// 鎺ヨ繎璺彛鍚庯紝瑕佹鏌ヨ溅杈嗘槸鍚﹁繘鍏ラ敊璇溅閬�
+ DEBUG("鎺ヨ繎璺彛");
road.arrivedTail = true;
if (!road.tts.empty())
PlayTTS(road.tts.c_str());
@@ -1845,6 +1810,177 @@
}
}
+static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
+{
+ for (int i = 0; i < RoadMap.triggerLines.size(); ++i) {
+ if (RoadMap.triggerLines[i].road == RoadMap.roads[index].id) {
+ Line triggerLine;
+
+ if (RoadMap.triggerLines[i].leftPoints.size() != RoadMap.triggerLines[i].points.size()) {
+ RoadMap.triggerLines[i].leftPoints.clear();
+
+ for (int j = 0; j < RoadMap.triggerLines[i].points.size(); ++j) {
+ RoadMap.triggerLines[i].leftPoints.push_back(GetSELine(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[j]));
+ }
+ }
+
+ MakeLine(&triggerLine, &RoadMap.triggerLines[i].points[0], &RoadMap.triggerLines[i].leftPoints[0]);
+
+ if (CrashTheLine(triggerLine, car, CarModelList)) {
+ return &RoadMap.triggerLines[i];
+ }
+ }
+ }
+
+ return NULL;
+}
+
+/************************************************************************
+ * 寮�濮嬫柊鐨勮�冭瘯鍚庯紝娓呴櫎鍦板浘鎵�鏈夌殑鍒硅溅銆佸仠杞﹁褰�
+ * @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;
+ }
+}
+
+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);
+ }
+ }
+ }
+ }
+ // 浜鸿閬撱�佸叕浜ょ珯鍒硅溅鐐癸紱瀛︽牎闄愰�熷尯
+ 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 = GetSELine(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 = GetSELine(map.roads[road].leftEdge, map.specialAreas[i].area[0]);
+ 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) {
+ 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;
+
+ 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);
+ }
+ }
+}
+
#if 0
typedef struct {
--
Gitblit v1.8.0