From 03ca970f73af542456326b2fa277090ce0ea532f Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期二, 11 八月 2020 18:33:51 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/road_exam.cpp | 269 ++++++++++++-----------------------------------------
1 files changed, 63 insertions(+), 206 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index 3d6a4e1..674f248 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -15,6 +15,7 @@
#include "drive_straight.h"
#include "stop_car.h"
#include "operate_gear.h"
+#include "../test_common/odo_graph.h"
#include <cmath>
#include <vector>
@@ -115,15 +116,13 @@
static bool laneChanging;
static int changeLaneDirect;
-static double odoGraph;
-static struct drive_timer odoTimer;
-static double odoPrevSpeed;
-static int odoCnt;
+
typedef struct {
int road;
int sep;
int lane;
+ int guide;
} lane_t;
typedef struct {
@@ -161,8 +160,6 @@
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);
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);
@@ -199,7 +196,7 @@
prevGearNSlide = false;
gearNSlideTime = 0;
- currExamMapIndex = FIND_POSITION;
+ currExamMapIndex = -1;
startCar = START_CAR_NOT_DO;
stopCar = STOP_CAR_NOT_DO;
@@ -222,8 +219,7 @@
checkTurn = false;
ClearAll(RoadMap);
- odoGraph = 0.0;
- odoCnt = 0;
+ ResetOdo();
// 鍒濆鍖栬�冮」
}
@@ -426,7 +422,7 @@
/***************************************************************
* 杞﹁締鎵�鍦ㄩ亾璺紝鏍规嵁杞﹁締鐨勪腑杞村墠鐐�
- * @param currRoadIndex
+ * @param currRoadIndex, 浼樺厛妫�娴嬪綋鍓嶉亾璺�
* @param RoadMap
* @param car
* @return
@@ -493,7 +489,7 @@
/************************************************
* 杞﹁疆鍘嬪疄绾匡紝鍓嶅悗杞翠氦鍙夛紝鍓嶅悗杞抗浜ゅ弶
- * @param mode
+ * @param mode 1 - 閬撹矾瀹炵嚎/鍒嗛亾瀹炵嚎 2 - 鍒嗛亾铏氱嚎/鍗婂疄鍗婅櫄绾�
* @param RoadMap
* @param car
* @param CarModelList
@@ -505,6 +501,8 @@
Line frontLeftTireTrack, frontRightTireTrack;
Line rearLeftTireTrack, rearRightTireTrack;
bool track = false;
+ int lineType = -1;
+
MakeLine(&frontTireAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
MakeLine(&rearTireAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
@@ -542,6 +540,7 @@
PointF p1, p2;
Line redLine;
+ // 宸﹀彸璺竟瀹炵嚎
for (int n = 0; n < RoadMap.roads.size(); ++n) {
for (int m = 0; m < RoadMap.roads[n].leftEdge.size(); ++m) {
if (RoadMap.roads[n].leftEdge[m].character == LINE_SOLID && RoadMap.roads[n].leftEdge[m].points.size() >= 2) {
@@ -553,7 +552,8 @@
if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
- return true;
+ lineType = LINE_SOLID;
+ goto CRASH_LINE_CONFIRM;
}
if (track &&
@@ -561,7 +561,8 @@
IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
- return true;
+ lineType = LINE_SOLID;
+ goto CRASH_LINE_CONFIRM;
}
p1 = p2;
}
@@ -578,7 +579,8 @@
if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
- return true;
+ lineType = LINE_SOLID;
+ goto CRASH_LINE_CONFIRM;
}
if (track &&
@@ -586,13 +588,15 @@
IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
- return true;
+ lineType = LINE_SOLID;
+ goto CRASH_LINE_CONFIRM;
}
p1 = p2;
}
}
}
+ // 鍒嗛亾瀹炵嚎
for (int m = 0; m < RoadMap.roads[n].separate.size(); ++m) {
// 涓�缁勮溅閬�
for (int l = 0; l < RoadMap.roads[n].separate[m].lines.size(); ++l) {
@@ -601,38 +605,54 @@
// 涓�鏍瑰垎閬撶嚎涓嚎鍨嬬浉鍚岀殑
int character = RoadMap.roads[n].separate[m].lines[l][a].character;
- if (character == LINE_SOLID && RoadMap.roads[n].separate[m].lines[l][a].points.size() >= 2) {
- p1 = RoadMap.roads[n].separate[m].lines[l][a].points[0];
+ if (RoadMap.roads[n].separate[m].lines[l][a].points.size() < 2)
+ continue;
- for (int b = 1; b < RoadMap.roads[n].separate[m].lines[l][a].points.size(); ++b) {
- p2 = RoadMap.roads[n].separate[m].lines[l][a].points[b];
- MakeLine(&redLine, &p1, &p2);
- if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
- IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
- return true;
- }
- if (track &&
- (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
- IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
- IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
- IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
- return true;
- }
- p1 = p2;
+ p1 = RoadMap.roads[n].separate[m].lines[l][a].points[0];
+
+ for (int b = 1; b < RoadMap.roads[n].separate[m].lines[l][a].points.size(); ++b) {
+ p2 = RoadMap.roads[n].separate[m].lines[l][a].points[b];
+ MakeLine(&redLine, &p1, &p2);
+ if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
+ IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
+ lineType = character;
+ goto CRASH_LINE_CONFIRM;
}
+ if (track &&
+ (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
+ IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
+ IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
+ IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
+ lineType = character;
+ goto CRASH_LINE_CONFIRM;
+ }
+ p1 = p2;
}
- // 姣旇緝鏈�杩戠殑杩炵画鐨刵涓乏渚х偣鍜屼笂涓�涓彸渚х偣
- if (character == LINE_HALF_SOLID_LEFT) {
- // 涓嶈兘浠庡乏绉诲姩鍒板彸
- }
}
+ }
+ }
+ }
+
+CRASH_LINE_CONFIRM:
+ if (lineType == LINE_HALF_SOLID_LEFT) {
+ if (currCrashLineType != LINE_HALF_SOLID_LEFT && track) {
+ if (IntersectionOfLine(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], redLine) == -1) {
+ // 闈炴硶璺ㄧ嚎
+ }
+ }
+ } else if (lineType == LINE_HALF_SOLID_RIGHT) {
+ if (currCrashLineType != LINE_HALF_SOLID_RIGHT && track) {
+ if (IntersectionOfLine(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], redLine) == 1) {
+ // 闈炴硶璺ㄧ嚎
}
}
}
return false;
}
+
+
static int GetGuideDirect(road_exam_map &RoadMap, PointF point, int roadIndex, int sepIndex, int laneNo)
{
@@ -657,13 +677,13 @@
{
Line leftProjLine, rightProjLine;
Line sep;
- PointF p1, p2;
+ PointF p0, p1, p2;
if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
return false;
p1 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].leftEdge, point);
- p2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, point);
+ p0 = p2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, point);
MakeLine(&leftProjLine, &point, &p1);
MakeLine(&rightProjLine, &point, &p2);
@@ -760,7 +780,7 @@
break;
}
}
-
+ theLane.guide = GetGuideDirect(RoadMap, p0, theLane.road, theLane.sep, theLane.lane);
return true;
}
}
@@ -877,7 +897,7 @@
{
uint32_t cts = AppTimer_GetTickCount();
int ri = CalcRoadIndex(-1, RoadMap, car);
- bool crash = CrashRedLine(0, RoadMap, car, CarModelList);
+ bool crash = CrashRedLine(0, 0, RoadMap, car, CarModelList);
lane_t laneInfo;
double redist = -1;
@@ -901,24 +921,7 @@
DEBUG("褰撳墠閬撹矾绱㈠紩 %d, 瑙﹀彂绾㈢嚎 %d lane %d 璺濈 %f %ld", ri, crash, laneInfo.lane, redist, AppTimer_GetTickCount() - cts);
- // 琛岄┒璺濈锛屼笉鍖呭惈鍊掕溅
- if (odoCnt == 0 && moveDirect == 1) {
- odoPrevSpeed = speed;
- odoCnt = 1;
- Rtk2DriveTimer(odoTimer, rtkTime);
- } else if (odoCnt == 1) {
- uint32_t tm = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
- odoTimer.hour, odoTimer.min, odoTimer.sec, odoTimer.msec*10);
- if (tm >= D_SEC(1)) {
- odoGraph += ((double)tm)*(odoPrevSpeed + speed)/2.0/1000.0;
- if (moveDirect == 1) {
- Rtk2DriveTimer(odoTimer, rtkTime);
- odoPrevSpeed = speed;
- } else {
- odoCnt = 0;
- }
- }
- }
+ UpdataOdo(speed, moveDirect, rtkTime);
// 瓒呴�熸娴�
if (moveDirect != 0 && speed > MAX_SPEED) {
@@ -1097,137 +1100,7 @@
speed, moveDirect, rtkTime);
// 妫�娴嬬寮�姝よ矾娈碉紝鍏ㄨ溅闇�涓嶅湪鑼冨洿鍐�
- 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);
-
- RoadMap.roads[currExamMapIndex].arrivedTail = false;
-
- currExamMapIndex = FIND_POSITION;
- }
-
- free(carBody.point);
- free(area.point);
- }
- if (currExamMapIndex == FIND_POSITION) {
- DEBUG("鎼滅储閬撹矾");
- 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);
- if (nextRoadId >= 0 && RoadMap.roads[i].id != nextRoadId) {
- DEBUG("涓嶆寜瑙勭煩琛岄┒锛岃繘鍏ラ敊璇矾娈�");
- AddExamFault(3, rtkTime);
- }
- nextRoadId = -1;
- checkTurn = false;
- break;
- }
-
- free(area.point);
- }
- if (currExamMapIndex < 0) {
- currExamMapIndex = FIND_POSITION;//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;
- }
- }
- }
+ currExamMapIndex = CalcRoadIndex(currExamMapIndex, RoadMap, car);
// 妫�娴嬪帇绾跨姸鎬�
bool crashRedLineNow = false;
@@ -1497,7 +1370,7 @@
if (startCar != START_CAR_DONE)
return;
- if (odoGraph > EXAM_RANGE && currRoadItem == NULL && AllCmp(RoadMap) && stopCar == STOP_CAR_NOT_DO) {
+ if (ReadOdo() > EXAM_RANGE && currRoadItem == NULL && AllCmp(RoadMap) && stopCar == STOP_CAR_NOT_DO) {
// 鍦ㄥ悎閫傛潯浠朵笅鍋滆溅缁撴潫鑰冭瘯
StartStopCarExam("璇烽潬杈瑰仠杞�");
stopCar = STOP_CAR_DOING;
@@ -1827,22 +1700,6 @@
}
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 Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
--
Gitblit v1.8.0