From a073dc3c983b4c56c5da92642c9ad11995bdb844 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期一, 11 五月 2020 18:05:31 +0800
Subject: [PATCH] 新的科目三
---
lib/src/main/cpp/test_items2/road_exam.cpp | 674 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 files changed, 663 insertions(+), 11 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index de97518..f712b10 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -18,6 +18,7 @@
#include <vector>
#include <list>
+#include <map>
#include <string>
#include <cstdlib>
@@ -39,6 +40,9 @@
static const int TURN_CHECK_INTERVAL = 500;
const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
+
+static const int FIND_POSITION = -2;
+static const int INVALID_POSITION = -1;
static bool occurCrashRedLine;
static bool occurCrashGreenLine;
@@ -76,9 +80,16 @@
static bool handBreakActive = false;
static bool reportRPMOver = false;
+static int currCarOnRoadIndex;
+
static const uint32_t TURN_ERROR_COLD_TIME = D_SEC(10);
static bool turnError13Cold, turnError14Cold;
+static struct car_on_lane {
+ int road;
+ int separate;
+ int lane;
+} CarOnLane;
static const int MAX_ENGINE_RPM = 2500;
static const double START_CAR_MOVE_DISTANCE = 10.0;
@@ -90,10 +101,16 @@
static const uint32_t CRASH_DOTTED_LINE_TIMEOUT = D_SEC(10);
static const uint32_t TURN_SIGNAL_LAMP_ADVANCE = D_SEC(3);
+static const int CRL_NONE = 0;
+static const int CRL_SEP_DOTTED = 1;
+static const int CRL_SEP_SOLID = 2;
+static const int CRL_EDGE_DOTTED = 3;
+static const int CRL_EDGE_SOLID = 4;
+
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 TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime);
+static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime);
static char isTurn(int currYaw, int prevYaw, int &ang);
static char CheckCarTurn(LIST_CAR_MODEL &CarModelList);
@@ -106,8 +123,12 @@
static void TurnSignalError13ColdTimer(union sigval sig);
static void TurnSignalError14ColdTimer(union sigval sig);
static void ReportTurnSignalError(int err, const struct RtkTime *rtkTime);
+static bool UpdateLane(struct car_on_lane &out, road_t &road, const car_model *car);
+static int CrashRoadLine(road_t &road, const car_model *car);
+static bool LaneIsSame(struct car_on_lane lane1, struct car_on_lane lane2);
+static bool LaneIsValid(struct car_on_lane lane);
-void InitRoadExam(void)
+void InitRoadExam(road_exam_map &RoadMap)
{
DEBUG("Start road_exam");
@@ -132,7 +153,7 @@
prevGearNSlide = false;
gearNSlideTime = 0;
- currExamMapIndex = -1;
+ currExamMapIndex = FIND_POSITION;
startCar = START_CAR_NOT_DO;
@@ -143,6 +164,12 @@
checkStartCarSignal = startCarLeftTurnSignal = false;
turnError13Cold = turnError14Cold = true;
+
+ currCarOnRoadIndex = FIND_POSITION;
+
+ CarOnLane.road = CarOnLane.separate = CarOnLane.lane = -1;
+
+ InitThroughSomething(RoadMap);
}
void TerminateRoadExam(void)
@@ -188,7 +215,7 @@
}
}
-static void TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
+static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
double moveDistance;
@@ -256,6 +283,558 @@
} else {
}
+
+ return startCar;
+}
+
+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;
+ // 瓒呴�燂紝涓嶅悎鏍�
+ AddExamFault(10, rtkTime);
+ }
+ } else {
+ occurOverSpeed = false;
+ }
+
+ // 鍓埞杞︽娴�
+ if (ReadCarStatus(SECOND_BREAK) == BREAK_ACTIVE) {
+ // 鍓埞杞﹁俯涓嬶紝涓嶅悎鏍�
+ if (!occurSecondBreak) {
+ DEBUG("鍓埞杞﹀姩浣滀簡");
+ occurSecondBreak = true;
+ AddExamFault(17, rtkTime);
+ }
+ } else {
+ occurSecondBreak = 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;
+ }
+
+ // 妫�娴嬮�氳繃璺彛銆佷汉琛岄亾绛夊尯鍩熸椂锛岄噴鏀惧埞杞︽垨鍑忛��
+ CheckBreakActive(RoadMap, car, CarModelList);
+
+ // 妫�娴嬬寮�姝よ矾娈碉紝鍏ㄨ溅闇�涓嶅湪鑼冨洿鍐�
+ if (currExamMapIndex >= 0) {
+ Polygon area;
+ int n = 0;
+
+ area.num = 0;
+
+ for (int j = 0; j < RoadMap.roads[currExamMapIndex].leftEdge.size(); ++j) {
+ if (j > 0) {
+ area.num += RoadMap.roads[currExamMapIndex].leftEdge[j].points.size() - 1;
+ } else {
+ area.num += RoadMap.roads[currExamMapIndex].leftEdge[j].points.size();
+ }
+ }
+ for (int j = 0; j < RoadMap.roads[currExamMapIndex].rightEdge.size(); ++j) {
+ if (j > 0) {
+ area.num += RoadMap.roads[currExamMapIndex].rightEdge[j].points.size() - 1;
+ } else {
+ area.num += RoadMap.roads[currExamMapIndex].rightEdge[j].points.size();
+ }
+ }
+
+ area.point = (PointF *) malloc(area.num * sizeof(PointF));
+
+ for (int j = 0; j < RoadMap.roads[currExamMapIndex].leftEdge.size(); ++j) {
+ for (int k = (j>0?1:0); k < RoadMap.roads[currExamMapIndex].leftEdge[j].points.size(); ++k) {
+ area.point[n++] = RoadMap.roads[currExamMapIndex].leftEdge[j].points[k];
+ }
+ }
+
+ for (int j = RoadMap.roads[currExamMapIndex].rightEdge.size() - 1; j >= 0; --j) {
+ if (j == RoadMap.roads[currExamMapIndex].rightEdge.size() - 1) {
+ for (int k = RoadMap.roads[currExamMapIndex].rightEdge[j].points.size() - 1; k >= 0; --k) {
+ area.point[n++] = RoadMap.roads[currExamMapIndex].rightEdge[j].points[k];
+ }
+ } else {
+ for (int k = RoadMap.roads[currExamMapIndex].rightEdge[j].points.size() - 2; k >= 0; --k) {
+ area.point[n++] = RoadMap.roads[currExamMapIndex].rightEdge[j].points[k];
+ }
+ }
+ }
+
+ // 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
+ 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, &area) == GM_None) {
+ DEBUG("绂诲紑璺 id = %d", RoadMap.roads[currExamMapIndex].id);
+ currExamMapIndex = FIND_POSITION;
+ }
+
+ free(carBody.point);
+ free(area.point);
+ }
+ if (currExamMapIndex == FIND_POSITION) {
+ for (int i = 0; i < RoadMap.roads.size(); ++i) {
+ Polygon area;
+ int n = 0;
+
+ area.num = 0;
+
+ for (int j = 0; j < RoadMap.roads[i].leftEdge.size(); ++j) {
+ if (j > 0) {
+ area.num += RoadMap.roads[i].leftEdge[j].points.size() - 1;
+ } else {
+ area.num += RoadMap.roads[i].leftEdge[j].points.size();
+ }
+ }
+ for (int j = 0; j < RoadMap.roads[i].rightEdge.size(); ++j) {
+ if (j > 0) {
+ area.num += RoadMap.roads[i].rightEdge[j].points.size() - 1;
+ } else {
+ area.num += RoadMap.roads[i].rightEdge[j].points.size();
+ }
+ }
+
+ area.point = (PointF *) malloc(area.num * sizeof(PointF));
+
+ for (int j = 0; j < RoadMap.roads[i].leftEdge.size(); ++j) {
+ for (int k = (j>0?1:0); k < RoadMap.roads[i].leftEdge[j].points.size(); ++k) {
+ area.point[n++] = RoadMap.roads[i].leftEdge[j].points[k];
+ }
+ }
+
+ for (int j = RoadMap.roads[i].rightEdge.size() - 1; j >= 0; --j) {
+ if (j == RoadMap.roads[i].rightEdge.size() - 1) {
+ for (int k = RoadMap.roads[i].rightEdge[j].points.size() - 1; k >= 0; --k) {
+ area.point[n++] = RoadMap.roads[i].rightEdge[j].points[k];
+ }
+ } else {
+ for (int k = RoadMap.roads[i].rightEdge[j].points.size() - 2; k >= 0; --k) {
+ area.point[n++] = RoadMap.roads[i].rightEdge[j].points[k];
+ }
+ }
+ }
+
+ if (IntersectionOf(car->carXY[car->axial[AXIAL_FRONT]], &area) == GM_Containment) {
+ currExamMapIndex = i;
+ DEBUG("杩涘叆閬撹矾 id = %d", RoadMap.roads[i].id);
+ break;
+ }
+
+ free(area.point);
+ }
+ if (currExamMapIndex < 0) {
+ currExamMapIndex = INVALID_POSITION;
+ DEBUG("鎼滃鏈灉");
+ }
+ } else if (currExamMapIndex == INVALID_POSITION) {
+ for (int i = 0; i < RoadMap.roads.size(); ++i) {
+ if (CrashTheLine(RoadMap.roads[i].startLine, car, CarModelList)) {
+ currExamMapIndex = i;
+ DEBUG("杩涘叆閬撹矾 id = %d", RoadMap.roads[i].id);
+ break;
+ }
+ }
+ }
+
+ if (currExamMapIndex >= 0) {
+ int crl = CrashRoadLine(RoadMap.roads[currExamMapIndex], car);
+
+ if (crl == CRL_NONE) {
+ DEBUG("浠�涔堥兘娌″帇");
+ } else if (crl == CRL_SEP_DOTTED) {
+ DEBUG("鍘嬪垎閬撹櫄绾�");
+ } else if (crl == CRL_SEP_SOLID) {
+ DEBUG("鍘嬪垎閬撳疄绾�");
+ } else if (crl == CRL_EDGE_DOTTED) {
+ DEBUG("鍘嬭竟娌胯櫄绾�");
+ } else if (crl == CRL_EDGE_SOLID) {
+ DEBUG("鍘嬭竟娌垮疄绾�");
+ }
+
+ if (crl != CRL_SEP_DOTTED || crl != CRL_SEP_SOLID) {
+ struct car_on_lane lane;
+ UpdateLane(lane, RoadMap.roads[currExamMapIndex], car);
+ if (!LaneIsSame(lane, CarOnLane)) {
+ if (LaneIsValid(CarOnLane)) {
+ // 杞﹂亾鍙樻崲
+ DEBUG("鍙樻洿杞﹂亾");
+ }
+ CarOnLane = lane;
+ }
+ }
+ }
+}
+
+/*************************************************************
+ * 妫�娴嬪綋鍓嶈溅閬擄紝浠ヨ溅澶翠腑鐐逛负鍩哄噯
+ * @param road
+ * @param car
+ */
+static bool UpdateLane(struct car_on_lane &out, road_t &road, const car_model *car)
+{
+ bool leftExt = false, rightExt = false;
+ Line leftExtLine, rightExtLine;
+ struct car_on_lane lane;
+
+ for (int i = 0; i < road.leftEdge.size(); ++i) {
+ PointF p1, p2;
+ Line edge;
+
+ p1 = road.leftEdge[i].points[0];
+ for (int j = 1; j < road.leftEdge[i].points.size(); ++j) {
+ p2 = road.leftEdge[i].points[j];
+ MakeLine(&edge, &p1, &p2);
+
+ PointF vp;
+ if (VerticalPointOnLine(car->carXY[car->axial[AXIAL_FRONT]], edge, vp)) {
+ leftExt = true;
+ MakeLine(&leftExtLine, &car->carXY[car->axial[AXIAL_FRONT]], &vp);
+ goto LEFT_EXT_CMP;
+ }
+ p1 = p2;
+ }
+ }
+LEFT_EXT_CMP:
+ for (int i = 0; i < road.rightEdge.size(); ++i) {
+ PointF p1, p2;
+ Line edge;
+
+ p1 = road.rightEdge[i].points[0];
+ for (int j = 1; j < road.rightEdge[i].points.size(); ++j) {
+ p2 = road.rightEdge[i].points[j];
+ MakeLine(&edge, &p1, &p2);
+
+ PointF vp;
+ if (VerticalPointOnLine(car->carXY[car->axial[AXIAL_FRONT]], edge, vp)) {
+ rightExt = true;
+ MakeLine(&rightExtLine, &car->carXY[car->axial[AXIAL_FRONT]], &vp);
+ goto RIGHT_EXT_CMP;
+ }
+ p1 = p2;
+ }
+ }
+RIGHT_EXT_CMP:
+
+ if (!leftExt || !rightExt) {
+ return false;
+ }
+
+ bool orthogonalInSegment = false;
+
+ for (int i = 0; i < road.separate.size(); ++i) { // 娈�
+ PointF p1, p2;
+ Line sep;
+
+ map<int, int> orthogonal;
+
+ // 涓�娈靛垎閬撶粍鍐咃紝鏈変竴鏉℃浜わ紝灏卞繀椤讳繚璇佸悓缁勭殑鍏ㄩ兘姝ｄ氦锛屽惁鍒欑洿鎺ラ��鍑�
+ for (int j = 0; j < road.separate[i].lines.size(); ++j) { // 绾跨粍
+ bool intersection = false;
+
+ for (int k = 0; !intersection && k < road.separate[i].lines[j].size(); ++k) { // 鑺�
+
+ p1 = road.separate[i].lines[j][k].points[0];
+ for (int m = 1; m < road.separate[i].lines[j][k].points.size(); ++m) {
+ p2 = road.separate[i].lines[j][k].points[m];
+ MakeLine(&sep, &p1, &p2);
+
+ if (IntersectionOf(leftExtLine, sep) == GM_Intersection) {
+ orthogonal.insert(pair<int, int>(j, 1));
+ orthogonalInSegment = true;
+ intersection = true;
+ DEBUG("鍒嗛亾绾� %d 宸︽浜�", j);
+ break;
+ } else if (IntersectionOf(rightExtLine, sep) == GM_Intersection) {
+ orthogonal.insert(pair<int, int>(j, 2));
+ orthogonalInSegment = true;
+ intersection = true;
+ DEBUG("鍒嗛亾绾� %d 鍙虫浜�", j);
+ break;
+ }
+ p1 = p2;
+ }
+ }
+ }
+ DEBUG("鐩爣 %d 褰撳墠 %d", road.separate[i].lines.size(), orthogonal.size());
+
+ if (orthogonal.size() > 0) {
+ if (orthogonal.size() == road.separate[i].lines.size()) {
+ // 寰楀埌褰撳墠鍦ㄧ鍑犱釜杞﹂亾
+ int x = 0;
+ for (x = 0; x < orthogonal.size(); ++x) {
+ auto itx = orthogonal.find(x);
+ if (itx != orthogonal.end()) {
+ if (itx->second != 1) {
+ lane.road = road.id;
+ lane.separate = i;
+ lane.lane = itx->first;
+
+ DEBUG("璺� %d 娈� %d 杞﹂亾 %d", lane.road, lane.separate, lane.lane);
+ break;
+ }
+ }
+ }
+ if (x >= orthogonal.size()) {
+ lane.road = road.id;
+ lane.separate = i;
+ lane.lane = orthogonal.size();
+
+ DEBUG("璺� %d 娈� %d 杞﹂亾 %d", lane.road, lane.separate, lane.lane);
+ }
+ out = lane;
+ return true;
+ } else {
+ // 涓嶅畬鍏ㄦ浜わ紝鐩存帴閫�鍑�
+ }
+ return false;
+ }
+ }
+ return false;
+}
+
+/*************************************************************************
+ * 纰拌Е杞﹂亾绾垮瀷
+ * @param road
+ * @param car
+ */
+static int CrashRoadLine(road_t &road, 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 < road.leftEdge.size(); ++i) {
+ PointF p1, p2;
+ Line edge;
+
+ p1 = road.leftEdge[i].points[0];
+ for (int j = 1; j < road.leftEdge[i].points.size(); ++j) {
+ p2 = road.leftEdge[i].points[j];
+ MakeLine(&edge, &p1, &p2);
+
+ if (IntersectionOf(edge, frontAxle) == GM_Intersection ||
+ IntersectionOf(edge, rearAxle) == GM_Intersection) {
+ // 鍘嬮亾璺乏杈圭嚎
+ if (road.leftEdge[i].character != LINE_SOLID) {
+ return CRL_EDGE_DOTTED;
+ }
+ return CRL_EDGE_SOLID;
+ }
+ p1 = p2;
+ }
+ }
+
+ for (int i = 0; i < road.rightEdge.size(); ++i) {
+ PointF p1, p2;
+ Line edge;
+
+ p1 = road.rightEdge[i].points[0];
+ for (int j = 1; j < road.rightEdge[i].points.size(); ++j) {
+ p2 = road.rightEdge[i].points[j];
+ MakeLine(&edge, &p1, &p2);
+
+ if (IntersectionOf(edge, frontAxle) == GM_Intersection ||
+ IntersectionOf(edge, rearAxle) == GM_Intersection) {
+ // 鍘嬮亾璺彸杈圭嚎
+ if (road.rightEdge[i].character != LINE_SOLID) {
+ return CRL_EDGE_DOTTED;
+ }
+ return CRL_EDGE_SOLID;
+ }
+ p1 = p2;
+ }
+ }
+
+ // 妫�鏌ュ垎閬撶嚎
+ for (int i = 0; i < road.separate.size(); ++i) {
+ // 娈�
+ PointF p1, p2;
+ Line sep;
+
+ for (int j = 0; j < road.separate[i].lines.size(); ++j) {
+ // 绾�
+ for (int k = 0; k < road.separate[i].lines[j].size(); ++k) {
+ // 鑺�
+ p1 = road.separate[i].lines[j][k].points[0];
+
+ for (int m = 1; m < road.separate[i].lines[j][k].points.size(); ++m) {
+ p2 = road.separate[i].lines[j][k].points[m];
+ MakeLine(&sep, &p1, &p2);
+
+ if (IntersectionOf(sep, frontAxle) == GM_Intersection ||
+ IntersectionOf(sep, rearAxle) == GM_Intersection) {
+ // 妫�鏌ラ亾璺垎闅旂嚎绫诲瀷
+ if (road.separate[i].lines[j][k].character == LINE_DOTTED) {
+ // 鍘嬭櫄绾�
+ return CRL_SEP_DOTTED;
+ } else if (road.separate[i].lines[j][k].character == LINE_SOLID) {
+ // 鍘嬪疄绾�
+ return CRL_SEP_SOLID;
+ } else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_LEFT) {
+ if (LaneIsValid(CarOnLane) && CarOnLane.lane <= j) {
+ return CRL_SEP_SOLID;
+ }
+ return CRL_SEP_DOTTED;
+ } else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_RIGHT) {
+ if (LaneIsValid(CarOnLane) && CarOnLane.lane > j) {
+ return CRL_SEP_SOLID;
+ }
+ return CRL_SEP_DOTTED;
+ }
+ }
+
+ p1 = p2;
+ }
+ }
+ }
+ }
+
+ return CRL_NONE;
+}
+
+static bool LaneIsSame(struct car_on_lane lane1, struct car_on_lane lane2)
+{
+ if (lane1.road == lane2.road && lane1.separate == lane2.separate && lane1.lane == lane2.lane) {
+ return true;
+ }
+ return false;
+}
+
+static bool LaneIsValid(struct car_on_lane lane)
+{
+ if (lane.road >= 0 && lane.separate >= 0 && lane.lane >= 0) {
+ return true;
+ }
+ return false;
}
void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
@@ -974,6 +1553,28 @@
return map_idx;
}
+bool CrashTheLine(Line line, const car_model *car, LIST_CAR_MODEL &CarModelList)
+{
+ if (CarModelList.size() < 2)
+ return false;
+
+ list<car_model *>::iterator iter = CarModelList.begin();
+
+ Line trace;
+ PointF p1, p2;
+
+ p1 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]];
+ ++iter;
+ p2 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]];
+ MakeLine(&trace, &p1, &p2);
+
+ if (IntersectionOf(trace, line) == GM_Intersection &&
+ IntersectionOfLine(p1, line) == -1) {
+ return true;
+ }
+ return false;
+}
+
static int FindMapIndexById(int id, LIST_ROAD_MAP &RoadMapList)
{
for (int i = 0; i < RoadMapList.size(); ++i) {
@@ -984,12 +1585,63 @@
return -1;
}
-#if 1
+/*********************************************************************
+ * 璁＄畻鏌愮偣鍒伴亾璺乏杈圭嚎鐨勬渶杩戝瀭鐐�
+ * @param edge
+ * @param road
+ * @param point
+ * @return
+ */
+PointF GetSELine(vector<edge_t> &edge, PointF point)
+{
+ PointF p1, p2;
+ PointF px;
+
+ vector<PointF> vps;
+
+ Line line;
+
+ for (int i = 0; i < edge.size(); ++i) {
+ p1 = edge[i].points[0];
+ for (int j = 1; j < edge[i].points.size(); ++j) {
+ p2 = edge[i].points[j];
+ MakeLine(&line, &p1, &p2);
+
+ PointF vp;
+ if (VerticalPointOnLine(point, line, vp)) {
+ vps.push_back(vp);
+ }
+
+ p1 = p2;
+ }
+ }
+
+ if (vps.size() == 0) {
+ if (DistanceOf(point, edge[0].points[0]) < DistanceOf(point, edge[edge.size() - 1].points[edge[edge.size() - 1].points.size() - 1])) {
+ px = GetVerticalPoint(edge[0].points[0], edge[0].points[1], point);
+ } else {
+ px = GetVerticalPoint(edge[edge.size() - 1].points[edge[edge.size() - 1].points.size() - 2], edge[edge.size() - 1].points[edge[edge.size() - 1].points.size() - 1], point);
+ }
+ } else if (vps.size() == 1) {
+ px = vps[0];
+ } else {
+ px = vps[0];
+ for (int i = 1; i < vps.size(); ++i) {
+ if (DistanceOf(point, vps[i]) < DistanceOf(point, px)) {
+ px = vps[i];
+ }
+ }
+ }
+
+ return px;
+}
+
+#if 0
typedef struct {
int road;
int segment;
- int track;
+ int lane;
} CarOnTrackInfo_t;
static CarOnTrackInfo_t CarOnTrackInfo;
@@ -1001,7 +1653,7 @@
if (newCarOnTrackInfo.road == CarOnTrackInfo.road &&
newCarOnTrackInfo.segment == CarOnTrackInfo.segment &&
- newCarOnTrackInfo.track != CarOnTrackInfo.track) {
+ newCarOnTrackInfo.lane != CarOnTrackInfo.lane) {
}
@@ -1145,7 +1797,7 @@
static void DetectSeparate(int currIndex, struct road_exam2_map &map, const car_model *car)
{
int segment;
- int track = -1;
+ int lane = -1;
CarOnTrackInfo_t newInfo;
@@ -1190,7 +1842,7 @@
vrecord.push_back(v);
- track = separate_line_num;//
+ lane = separate_line_num;//
}
@@ -1215,14 +1867,14 @@
if (rel != -1) {
newInfo.road = currIndex;
newInfo.segment = i;
- newInfo.track = x;
+ newInfo.lane = x;
break;
}
}
newInfo.road = currIndex;
newInfo.segment = i;
- newInfo.track = vrecord.size();
+ newInfo.lane = vrecord.size();
break;
}
--
Gitblit v1.8.0