From 576d819c4d9aabd1a8d102ae9c3bc13c9165ee1f Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期五, 14 八月 2020 19:15:45 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/road_exam.cpp | 596 +++++------------------------------------------------------
1 files changed, 51 insertions(+), 545 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index d75355d..5196886 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -83,13 +83,8 @@
const int OVERTAKE_HOLD_TIME = D_SEC(3); // 鍦ㄨ秴杞﹂亾琛岄┒鐨勪竴娈垫椂闂�
const double EXAM_RANGE = 3000.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;
static bool occurCrashRedLine;
static bool occurCrashGreenLine;
@@ -125,7 +120,6 @@
static int startCar, stopCar;
static int currExamMapIndex;
static trigger_line_t *currRoadItem;
-static int nextRoadId;
static PointF roadItemStartPoint;
static struct drive_timer roadItemStartTime;
static bool overtake = false;
@@ -168,12 +162,9 @@
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 bool AllCmp(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);
-static PointF CalcProjectionWithRoadEdge(vector<edge_t> &edge, PointF point);
+static bool AllCmp(road_exam_map &map);
+
static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car);
void InitRoadExam(road_exam_map &RoadMap)
@@ -215,15 +206,14 @@
checkStartCarSignal = startCarLeftTurnSignal = false;
Lane.road = Lane.sep = Lane.no = -1;
+ Lane.guide = 0;
- nextRoadId = -1;
checkTurn = false;
- ClearAll(RoadMap);
CrashLineType = -1;
ResetOdo();
-
+ ResetTarget(RoadMap);
// 鍒濆鍖栬�冮」
}
@@ -242,7 +232,7 @@
* @param point
* @return
*/
-static PointF CalcProjectionWithRoadEdge(vector<edge_t> &edge, PointF point)
+PointF CalcProjectionWithRoadEdge(vector<edge_t> &edge, PointF point)
{
PointF p1, p2;
PointF proj;
@@ -346,7 +336,7 @@
return theProj;
}
-static double CalcDistanceReference(PointF point, PointF refPoint, vector<edge_t> &edge)
+double CalcDistanceReference(PointF point, PointF refPoint, vector<edge_t> &edge)
{
double distance = 0;
@@ -657,7 +647,7 @@
for (int i = 0; i < RoadMap.roads[roadIndex].separate[sepIndex].lane_direct.size(); ++i) {
double d1 = CalcDistanceReference(point, RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].start, RoadMap.roads[roadIndex].rightEdge);
double d2 = CalcDistanceReference(point, RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].end, RoadMap.roads[roadIndex].rightEdge);
- if (fabs(d1) < 1e-6 && d1 < 0.0 && d2 > 1e-6 && laneNo < RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].direct.size()) {
+ if (d1 < -1e-3 && d2 > 1e-3 && laneNo < RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].direct.size()) {
return RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].direct[laneNo];
}
}
@@ -1063,144 +1053,51 @@
}
}
-static map <int, int> TargetReduceRec;
-
-enum {
- NOT_REDUCE_SPEED,
- REDUCE_SPEED,
- STOP_CAR,
- OVER_SPEED
-};
-
-static void ChangeTargetReduceRec(int roadIndex, int index, int status)
+static int NearbyCrossingGuide(int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
- int key = roadIndex * 100 + index;
+ int guide = 0;
+ double distance;
- auto it = CrossingHint.find(key);
-
- if (it != CrossingHint.end()) {
- CrossingHint.erase(it);
- }
- CrossingHint.insert(pair<int, int>(key, status));
-}
-
-static int GetTargetReduceRec(int roadIndex, int index)
-{
- int key = roadIndex * 100 + index;
-
- auto it = CrossingHint.find(key);
-
- if (it != CrossingHint.end()) {
- return it->second;
- }
- return -1;
-}
-
-void ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, double speed, int moveDirect)
-{
- car_sensor_value_t brk = ReadCarSensorValue(BREAK);
-
- if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
- return;
-
- for (int n = 0; n < RoadMap.roads[roadIndex].stopLine.size(); ++n) {
- PointF point;
+ struct nearby_crossing {
+ int guide;
double distance;
+ };
+
+ vector<struct nearby_crossing> set;
+
+ for (int i = 0; i < road.stopLine.size(); ++i) {
+ PointF point;
point.X = road.stopLine[i].line.X1;
point.Y = road.stopLine[i].line.Y1;
- distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, RoadMap.roads[roadIndex].rightEdge);
+ distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point,
+ road.rightEdge);
- if (distance > 1e-3 && distance < LASTEST_BREAK_POINT) {
- // 璁板綍鍒硅溅锛屽仠杞�
- if (brk.value == BREAK_ACTIVE) {
+ if (distance > 1e-3) {
+ struct nearby_crossing temp;
- }
- } else if (distance > LASTEST_BREAK_POINT + 5) {
+ temp.guide = road.stopLine[i].active;
+ temp.distance = distance;
+ set.push_back(temp);
}
}
- for (int i = 0; i < RoadMap.specialAreas.size(); i++) {
- if (RoadMap.specialAreas[i].type == GRID_AREA || RoadMap.specialAreas[i].road != RoadMap.roads[roadIndex].id)
- continue;
+ if (set.size() > 0) {
+ distance = set[0].distance;
+ guide = set[0].guide;
- 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;
+ for (int i = 1; i < set.size(); ++i) {
+ if (distance > set[i].distance) {
+ distance = set[i].distance;
+ guide = set[i].guide;
}
-
- PointF vPoint = CalcProjectionWithRoadEdge(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 = CalcProjectionWithRoadEdge(map.roads[road].leftEdge, map.specialAreas[i].area[1]);
-// 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) {
-// DEBUG("鏂戦┈绾�");
- 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;
-// DEBUG("瀛︽牎");
- 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);
}
}
+
+ return guide;
}
-
-
void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
@@ -1384,20 +1281,19 @@
}
} else {
// 鍓嶈繘
-
}
-
-
- // 妫�娴嬮�氳繃璺彛銆佷汉琛岄亾绛夊尯鍩熸椂锛岄噴鏀惧埞杞︽垨鍑忛��
- CheckBreakActive(RoadMap, car, CarModelList,
- speed, moveDirect, rtkTime);
// 妫�娴嬬寮�姝よ矾娈碉紝鍏ㄨ溅闇�涓嶅湪鑼冨洿鍐�
int oldid = currExamMapIndex;
currExamMapIndex = CalcRoadIndex(currExamMapIndex, RoadMap, car);
if (currExamMapIndex >= 0) {
+ car_sensor_value_t brk = ReadCarSensorValue(BREAK);
HintCrossing(currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList);
+
+ // 妫�娴嬮�氳繃璺彛銆佷汉琛岄亾绛夊尯鍩熸椂锛岄噴鏀惧埞杞︽垨鍑忛��
+ ApproachTarget(RoadMap, car, currExamMapIndex, (brk.value == BREAK_ACTIVE), speed, moveDirect, rtkTime);
}
+ ExitTarget(RoadMap, car, CarModelList, rtkTime);
if (oldid != currExamMapIndex) {
DEBUG("閬撹矾ID鍒囨崲 %d", currExamMapIndex);
@@ -1411,7 +1307,19 @@
DEBUG("鍘嬬嚎绫诲瀷鍒囨崲 %d", CrashLineType);
}
+
+ oldid = Lane.guide;
DetectLane(RoadMap, car, currExamMapIndex, rtkTime);
+ if (Lane.guide > 0 && currExamMapIndex >= 0) {
+ if (!(NearbyCrossingGuide(currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList) & Lane.guide)) {
+ DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒");
+ AddExamFault(9, rtkTime);
+ }
+ }
+
+ if (Lane.guide != oldid) {
+ DEBUG("瀵煎悜绫诲瀷鍒囨崲 %d", Lane.guide);
+ }
// 妫�娴嬪帇绾跨姸鎬�
TestRoadStartCar(car, speed, moveDirect, rtkTime);
@@ -1702,405 +1610,3 @@
}
return true;
}
-
-/************************************************************************
- * 寮�濮嬫柊鐨勮�冭瘯鍚庯紝娓呴櫎鍦板浘鎵�鏈夌殑鍒硅溅銆佸仠杞﹁褰�
- * @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;
-// }
-// for (int i = 0; i < map.triggerLines.size(); ++i) {
-// map.triggerLines[i].cmp = 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);
- }
- }
- }
- }*/
-
- // 浜鸿閬撱�佸叕浜ょ珯鍒硅溅鐐癸紱瀛︽牎闄愰�熷尯
-// DEBUG("琛ュ叏鐗规畩鍖哄煙 size = %d", map.specialAreas.size());
- 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 = CalcProjectionWithRoadEdge(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 = CalcProjectionWithRoadEdge(map.roads[road].leftEdge, map.specialAreas[i].area[1]);
-// 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) {
-// DEBUG("鏂戦┈绾�");
- 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;
-// DEBUG("瀛︽牎");
- 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);
- }
- }
-// DEBUG("琛ュ叏鐗规畩鍖哄煙 over");
-}
-
-#if 0
-
-typedef struct {
- int road;
- int segment;
- int lane;
-} CarOnTrackInfo_t;
-
-static CarOnTrackInfo_t CarOnTrackInfo;
-
-void TestRoadGeneral(struct road_exam2_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
-{
- // 妫�娴嬭溅閬撳彉鏇�
- CarOnTrackInfo_t newCarOnTrackInfo;
-
- if (newCarOnTrackInfo.road == CarOnTrackInfo.road &&
- newCarOnTrackInfo.segment == CarOnTrackInfo.segment &&
- newCarOnTrackInfo.lane != CarOnTrackInfo.lane) {
-
- }
-
- CarOnTrackInfo = newCarOnTrackInfo;
-
-
-}
-
-/**************************************************
- * 杞﹁締褰撳墠鎵�鍦ㄨ矾娈碉紝杞﹀ご闇�瓒婅繃璧峰绾夸竴瀹氳窛绂�
- * @param currIndex
- * @param map
- * @param car
- */
-static void EnterRoad(int &currIndex, struct road_exam2_map &map, const car_model *car)
-{
- 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]];
- }
-
- Polygon roadArea;
- roadArea.num = 0;
- roadArea.point = NULL;
-
- for (int i = 0; i < map.roads.size(); ++i) {
- if (roadArea.point != NULL) {
- free(roadArea.point);
- }
-
- for (int x = 0; x < map.roads[i].leftEdge.size(); ++x) {
- roadArea.num += map.roads[i].leftEdge[x].points.size();
- }
- for (int x = 0; x < map.roads[i].rightEdge.size(); ++x) {
- roadArea.num += map.roads[i].rightEdge[x].points.size();
- }
- roadArea.point = (PointF *) malloc(roadArea.num * sizeof(PointF));
-
- int n = 0;
- for (int j = 0; j < map.roads[i].leftEdge.size(); j++) {
- for (int x = 0; x < map.roads[i].leftEdge[j].points.size(); ++x) {
- if (n > 0 && x == 0 && isEqual2(roadArea.point[n-1].X, map.roads[i].leftEdge[j].points[x].X) &&
- isEqual2(roadArea.point[n-1].Y, map.roads[i].leftEdge[j].points[x].Y)) {
- // 绗竴涓偣宸茬粡瀛樺湪浜庝笂涓�鏉＄嚎
- } else {
- roadArea.point[n++] = map.roads[i].leftEdge[j].points[x];
- }
- }
- }
- for (int j = map.roads[i].rightEdge.size(); j > 0; j--) {
- for (int x = map.roads[i].rightEdge[j].points.size(); x > 0; --x) {
- if (n > 0 && x == map.roads[i].rightEdge[j].points.size() &&
- isEqual2(roadArea.point[n-1].X, map.roads[i].rightEdge[j - 1].points[x-1].X) &&
- isEqual2(roadArea.point[n-1].Y, map.roads[i].rightEdge[j - 1].points[x-1].Y)) {
- // 绗竴涓偣宸茬粡瀛樺湪浜庝笂涓�鏉＄嚎
- } else {
- roadArea.point[n++] = map.roads[i].rightEdge[j - 1].points[x - 1];
- }
- }
- }
- roadArea.num = n;
-
- if (IntersectionOf(car->carXY[car->axial[AXIAL_FRONT]], &roadArea) == GM_Containment) {
- currIndex = i;
- goto CHECK_CAR_ON_ROAD_END;
- }
- }
- currIndex = INVALID_ROAD;
-
- CHECK_CAR_ON_ROAD_END:
- if (roadArea.point != NULL) {
- free(roadArea.point);
- }
- free(carBody.point);
-}
-
-/******************************************************
- * 鍏ㄨ溅閮介渶绂诲紑杩欎釜鍖哄煙
- * @param currIndex
- * @param map
- * @param car
- * @return
- */
-static bool ExitRoad(int currIndex, struct road_exam2_map &map, const car_model *car)
-{
- // 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
- bool ret = false;
-
- if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
- return ret;
-
- 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, &map.roads[currIndex].area) == GM_None) {
- ret = true;
- }
-
- free(carBody.point);
- return ret;
-}
-
-static bool CrashSeparateLine(int currIndex, struct road_exam2_map &map, const car_model *car)
-{
- Line frontAxle, rearAxle;
-
- if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
- return false;
-
- 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 < map.roads[currIndex].separate.size(); i++) {
- // 鍒嗘涓殑姣忔潯绾�
- for (int j = 0; j < map.roads[currIndex].separate[i].line.size(); j++) {
- Line theLine;
- int p1 = 0;
- for (int p2 = 1; p2 < map.roads[currIndex].separate[i].line[j].num; ++p2) {
- MakeLine(&theLine, &map.roads[currIndex].separate[i].line[j].point[p1], &map.roads[currIndex].separate[i].line[j].point[p2]);
- if (IntersectionOf(theLine, frontAxle) == GM_Intersection ||
- IntersectionOf(theLine, rearAxle) == GM_Intersection) {
- return true;
- }
- p1 = p2;
- }
- }
- }
- return false;
-}
-
-
-static void DetectSeparate(int currIndex, struct road_exam2_map &map, const car_model *car)
-{
- int segment;
- int lane = -1;
-
- CarOnTrackInfo_t newInfo;
-
- if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
- return;
-
- // 閬嶅巻姣忎竴鍒嗘
- for (int i = 0; i < map.roads[currIndex].separate.size(); i++) {
- int separate_line_num = map.roads[currIndex].separate[i].lines.size();
-
- struct vrecord_ {
- int valid;
- int p1;
- int p2;
- };
-
- vector<struct vrecord_> vrecord;
- vrecord.clear();
-
- bool match1 = false;
- // 閬嶅巻褰撳墠鍒嗘鐨勬瘡涓�鏉＄嚎
- for (int j = 0; j < separate_line_num; ++j) {
- Line theLine;
- int p1 = 0;
-
- struct vrecord_ v;
- v.valid = 0;
-
- // 鍗曠嫭鐨勪竴鏉¤櫄绾�
- for (int p2 = 1; p2 < map.roads[currIndex].separate[i].lines[j].num; ++p2) {
- MakeLine(&theLine, &map.roads[currIndex].separate[i].lines[j].point[p1],
- &map.roads[currIndex].separate[i].lines[j].point[p2]);
- if (VerticalPointOnLine(car->basePoint, theLine)) {
- v.valid = 1;
- v.p1 = p1;
- v.p2 = p2;
- match1 = true;
- break;
- }
- p1 = p2;
- }
-
- vrecord.push_back(v);
-
- lane = separate_line_num;//
- }
-
-
-
- if (match1) {
- for (int x = 0; x < vrecord.size(); ++x) {
- if (vrecord[i].valid == 0) {
- // 棣栧熬涓ゆ绾跨殑寤剁敵蹇呮湁涓�涓瀭鐐�
- if (DistanceOf(car->carXY[ car->axial[AXIAL_FRONT] ], map.roads[currIndex].separate[i].lines[x].point[0]) <
- DistanceOf(car->carXY[ car->axial[AXIAL_FRONT] ], map.roads[currIndex].separate[i].lines[x].point[1])) {
- vrecord[x].p1 = 0;
- vrecord[x].p2 = 1;
- } else {
- vrecord[x].p1 = map.roads[currIndex].separate[i].lines[x].num - 2;
- vrecord[x].p2 = map.roads[currIndex].separate[i].lines[x].num - 1;
- }
- }
-
- int rel = IntersectionOfLine(map.roads[currIndex].separate[i].lines[x].point[vrecord[x].p1],
- map.roads[currIndex].separate[i].lines[x].point[vrecord[x].p2],
- car->basePoint);
- if (rel != -1) {
- newInfo.road = currIndex;
- newInfo.segment = i;
- newInfo.lane = x;
- break;
- }
- }
-
- newInfo.road = currIndex;
- newInfo.segment = i;
- newInfo.lane = vrecord.size();
-
- break;
- }
- }
-}
-
-
-
-#endif
--
Gitblit v1.8.0