From e43f00fbe051dc8f9dfa5a19143c38613b64ecad Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期五, 07 八月 2020 19:09:36 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/driver_test.cpp | 2
lib/src/main/cpp/native-lib.cpp | 2
lib/src/main/cpp/test_items2/road_exam.cpp | 615 +++++++++++++++++++++++++++++++++++++++++++++++++++----
lib/src/main/cpp/test_common/Geometry.h | 2
lib/src/main/cpp/rtk_module/rtk.cpp | 8
lib/src/main/cpp/driver_test.h | 7
lib/src/main/cpp/test_common/Geometry.cpp | 5
7 files changed, 581 insertions(+), 60 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index d7ef572..b43d9a2 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -372,7 +372,7 @@
reportSeatbeltEject = false;
if (type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
- exam_dummy_light = 0;
+ exam_dummy_light = 1; //0
}
if (type == TEST_TYPE_AREA) {
InitAreaExam();
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index 14c6407..cbad97b 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -133,8 +133,15 @@
std::vector<PointF> points;
} segment_t;
+typedef struct {
+ PointF start;
+ PointF end;
+ std::vector<int> direct;
+} lane_direct_t;
+
// 涓�缁勫钩琛岀殑鍒嗛亾绾�
typedef struct {
+ std::vector<lane_direct_t> lane_direct;
std::vector<std::vector<segment_t>> lines; // 姣忔鍩熶笅鐨勫钩琛岀殑涓�缁勭嚎
} separate_t;
diff --git a/lib/src/main/cpp/native-lib.cpp b/lib/src/main/cpp/native-lib.cpp
index c54b0c2..70aa448 100644
--- a/lib/src/main/cpp/native-lib.cpp
+++ b/lib/src/main/cpp/native-lib.cpp
@@ -26,7 +26,7 @@
const int RTK_PLATFORM_PORT = 12125;
const uint8_t phone[] = {0x20,0x19,0x10,0x15,0x00,0x00,0x00,0x01};
-const char *VIRTUAL_RTK_IP = "192.168.1.16";
+const char *VIRTUAL_RTK_IP = "192.168.16.100";
const int VIRTUAL_RTK_PORT = 9001;
static pthread_mutex_t tts_mutex = PTHREAD_MUTEX_INITIALIZER;
diff --git a/lib/src/main/cpp/rtk_module/rtk.cpp b/lib/src/main/cpp/rtk_module/rtk.cpp
index f064637..732292e 100644
--- a/lib/src/main/cpp/rtk_module/rtk.cpp
+++ b/lib/src/main/cpp/rtk_module/rtk.cpp
@@ -214,7 +214,7 @@
}*/
if (RxBufLen > 0) {
-#if 1
+#if 0
const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
if(ptr != RxBuf) {
memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));
@@ -475,7 +475,7 @@
DEBUG("RTK Module failure!!");
- PlayTTS("RTK妯″潡鏃犳硶閫氳");
-
- CheckPjkParam();
+// PlayTTS("RTK妯″潡鏃犳硶閫氳");
+//
+// CheckPjkParam();
}
diff --git a/lib/src/main/cpp/test_common/Geometry.cpp b/lib/src/main/cpp/test_common/Geometry.cpp
index 03b5434..7a8884b 100644
--- a/lib/src/main/cpp/test_common/Geometry.cpp
+++ b/lib/src/main/cpp/test_common/Geometry.cpp
@@ -628,3 +628,8 @@
return ext;
}
+
+bool IsSamePoint(PointF p1, PointF p2)
+{
+ return isEqual(p1.X, p2.X) && isEqual(p1.Y, p2.Y);
+}
diff --git a/lib/src/main/cpp/test_common/Geometry.h b/lib/src/main/cpp/test_common/Geometry.h
index b07dd52..ca673d8 100644
--- a/lib/src/main/cpp/test_common/Geometry.h
+++ b/lib/src/main/cpp/test_common/Geometry.h
@@ -64,4 +64,6 @@
bool VerticalPointOnLine(PointF point, Line line, PointF &vp);
PointF Calc3Point(PointF p1, PointF p2, double L, char dir);
PointF PointExtend(PointF ori, double length, double yaw);
+bool IsSamePoint(PointF p1, PointF p2);
+
#endif //GUI_GEOMETRY_H
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index e28d9af..3a1a5b0 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -119,6 +119,18 @@
static double odoPrevSpeed;
static int odoCnt;
+typedef struct {
+ int road;
+ int sep;
+ int lane;
+} lane_t;
+
+typedef struct {
+ int edgeIndex;
+ int pointIndex;
+ PointF point;
+} projection_t;
+
static const int MAX_ENGINE_RPM = 2500;
static const double START_CAR_MOVE_DISTANCE = 10.0;
static const double START_CAR_CHECK_DOOR_DISTANCE = 1.0;
@@ -128,7 +140,6 @@
static const uint32_t CHANGE_LANE_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 int CRL_NONE = 0;
static const int CRL_SEP_DOTTED = 1;
@@ -157,6 +168,9 @@
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 int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car);
void InitRoadExam(road_exam_map &RoadMap)
{
@@ -222,6 +236,523 @@
AppTimer_delete(TurnSignalError13ColdTimer);
AppTimer_delete(TurnSignalError14ColdTimer);
+}
+
+/*********************************************************************
+ * 璁＄畻鏌愮偣鍒伴亾璺竟绾跨殑鎶曞奖鐐�
+ * @param edge
+ * @param road
+ * @param point
+ * @return
+ */
+static PointF CalcProjectionWithRoadEdge(vector<edge_t> &edge, PointF point)
+{
+ PointF p1, p2;
+ PointF proj;
+
+ vector<PointF> projArray;
+
+ 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)) {
+ projArray.push_back(vp);
+ }
+
+ p1 = p2;
+ }
+ }
+
+ if (projArray.size() == 0) {
+ // 娌℃湁浠讳綍鍨傜偣锛屾壘鍒版渶杩戠殑鐐�
+ proj = edge[0].points[0];
+ for (int i = 0; i < edge.size(); ++i) {
+ for (int j = 1; j < edge[i].points.size(); ++j) {
+ if (DistanceOf(point, edge[i].points[j]) < DistanceOf(point, proj)) {
+ proj = edge[i].points[j];
+ }
+ }
+ }
+ } else {
+ // 鏈�杩戠殑鍨傜偣
+ proj = projArray[0];
+ for (int i = 1; i < projArray.size(); ++i) {
+ if (DistanceOf(point, projArray[i]) < DistanceOf(point, proj)) {
+ proj = projArray[i];
+ }
+ }
+ }
+
+ return proj;
+}
+
+static projection_t CalcProjectionWithRoadEdgeEx(vector<edge_t> &edge, PointF point)
+{
+ PointF p1, p2;
+ Line line;
+ projection_t theProj;
+
+ vector<projection_t> projSet;
+
+ // 璁＄畻鍨傜偣
+ 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)) {
+ projection_t proj;
+ proj.edgeIndex = i;
+ proj.pointIndex = j - 1;
+ proj.point = vp;
+ projSet.push_back(proj);
+ }
+
+ p1 = p2;
+ }
+ }
+
+ if (projSet.size() == 0) {
+ // 娌℃湁浠讳綍鍨傜偣锛屾壘鍒版渶杩戠殑鐐�
+ theProj.edgeIndex = 0;
+ theProj.pointIndex = 0;
+ theProj.point = edge[0].points[0];
+
+ for (int i = 0; i < edge.size(); ++i) {
+ for (int j = 1; j < edge[i].points.size(); ++j) {
+ if (DistanceOf(point, edge[i].points[j]) < DistanceOf(point, theProj.point)) {
+ theProj.edgeIndex = i;
+ theProj.pointIndex = j - 1;
+ theProj.point = edge[i].points[j];
+ }
+ }
+ }
+ } else {
+ // 鏈�杩戠殑鍨傜偣
+ theProj = projSet[0];
+ for (int i = 1; i < projSet.size(); ++i) {
+ if (DistanceOf(point, projSet[i].point) < DistanceOf(point, theProj.point)) {
+ theProj = projSet[i];
+ }
+ }
+ }
+
+ return theProj;
+}
+
+static double CalcDistanceReference(PointF point, PointF refPoint, vector<edge_t> &edge)
+{
+ double distance = 0;
+
+ projection_t projection, refProjection;
+
+ projection = CalcProjectionWithRoadEdgeEx(edge, point);
+ refProjection = CalcProjectionWithRoadEdgeEx(edge, refPoint);
+
+ if (projection.edgeIndex != refProjection.edgeIndex || projection.pointIndex != refProjection.pointIndex) {
+ bool ahead = false;
+ if (projection.edgeIndex != refProjection.edgeIndex) {
+ if (projection.edgeIndex > refProjection.edgeIndex) {
+ projection_t temp;
+ temp = projection;
+ projection = refProjection;
+ refProjection = temp;
+ ahead = true;
+ }
+ int a = projection.edgeIndex;
+ int b = projection.pointIndex + 1;
+
+ if (b < edge[a].points.size()) {
+ distance += DistanceOf(projection.point, edge[projection.edgeIndex].points[b]);
+ }
+
+ for (; a < refProjection.edgeIndex; ++a) {
+ for (; b < edge[a].points.size() - 1; ++b) {
+ distance += DistanceOf(edge[a].points[b], edge[a].points[b+1]);
+ }
+ b = 0;
+ }
+
+ for (int i = 0; i < refProjection.pointIndex; i++) {
+ distance += DistanceOf(edge[a].points[i], edge[a].points[i+1]);
+ }
+
+ distance += DistanceOf(refProjection.point, edge[refProjection.edgeIndex].points[refProjection.pointIndex]);
+ } else {
+ if (projection.pointIndex > refProjection.pointIndex) {
+ projection_t temp;
+ temp = projection;
+ projection = refProjection;
+ refProjection = temp;
+ ahead = true;
+ }
+ int a = projection.edgeIndex;
+ int b = projection.pointIndex + 1;
+
+ if (b < edge[a].points.size()) {
+ distance += DistanceOf(projection.point, edge[projection.edgeIndex].points[b]);
+ }
+
+ for (; b < refProjection.pointIndex; b++) {
+ distance += DistanceOf(edge[a].points[b], edge[a].points[b+1]);
+ }
+
+ distance += DistanceOf(refProjection.point, edge[refProjection.edgeIndex].points[refProjection.pointIndex]);
+ }
+ if (ahead) {
+ distance *= -1.0;
+ }
+ } else {
+ distance = DistanceOf(projection.point, refProjection.point);
+
+ if (DistanceOf(projection.point, edge[projection.edgeIndex].points[projection.pointIndex]) >
+ DistanceOf(refProjection.point, edge[refProjection.edgeIndex].points[refProjection.pointIndex])) {
+ distance *= -1.0;
+ }
+ }
+
+ return distance;
+}
+
+/***************************************************************
+ * 杞﹁締鎵�鍦ㄩ亾璺紝鏍规嵁杞﹁締鐨勪腑杞村墠鐐�
+ * @param currRoadIndex
+ * @param RoadMap
+ * @param car
+ * @return
+ */
+static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car)
+{
+ int n = 0;
+ int index = currRoadIndex;
+
+ if (index < 0) {
+ index = 0;
+ }
+
+ while (n++ < RoadMap.roads.size()) {
+ bool changeSegment = false;
+ vector<PointF> roadOutLine;
+ Polygon area;
+
+ for (int i = 0; i < RoadMap.roads[index].leftEdge.size(); ++i) {
+ for (int j = 0; j < RoadMap.roads[index].leftEdge[i].points.size(); ++j) {
+ if (changeSegment && roadOutLine.size() > 0 && IsSamePoint(roadOutLine.back(), RoadMap.roads[index].leftEdge[i].points[j])) {
+ continue;
+ }
+ changeSegment = false;
+ roadOutLine.push_back(RoadMap.roads[index].leftEdge[i].points[j]);
+ }
+ changeSegment = true;
+ }
+
+ for (int i = 0; i < RoadMap.roads[index].rightEdge.size(); ++i) {
+ for (int j = RoadMap.roads[index].rightEdge[i].points.size() - 1; j >= 0; --j) {
+ if (changeSegment && roadOutLine.size() > 0 && IsSamePoint(roadOutLine.back(), RoadMap.roads[index].leftEdge[i].points[j])) {
+ continue;
+ }
+ changeSegment = false;
+ roadOutLine.push_back(RoadMap.roads[index].rightEdge[i].points[j]);
+ }
+ changeSegment = true;
+ }
+
+ area.num = roadOutLine.size();
+ area.point = (PointF *) malloc(area.num * sizeof(PointF));
+
+ for (int i = 0; i < area.num; ++i) {
+ area.point[i] = roadOutLine[i];
+ }
+
+ if (IntersectionOf(car->carXY[car->axial[AXIAL_FRONT]], &area) == GM_Containment) {
+ free(area.point);
+ break;
+ }
+
+ free(area.point);
+
+ index = (index + 1) % RoadMap.roads.size();
+ }
+
+ if (n >= RoadMap.roads.size()) {
+ index = -1;
+ }
+
+ return index;
+}
+
+/************************************************
+ * 杞﹁疆鍘嬪疄绾匡紝鍓嶅悗杞翠氦鍙夛紝鍓嶅悗杞抗浜ゅ弶
+ * @param mode
+ * @param RoadMap
+ * @param car
+ * @param CarModelList
+ * @return
+ */
+static bool CrashRedLine(int mode, int roadIndex, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
+{
+ Line frontTireAxle, rearTireAxle;
+ Line frontLeftTireTrack, frontRightTireTrack;
+ Line rearLeftTireTrack, rearRightTireTrack;
+ bool track = false;
+
+ 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]]);
+
+ if (CarModelList.size() >= 2) {
+ list<car_model *>::iterator iter = CarModelList.begin();
+
+ PointF p11, p12, p21, p22, p31, p32, p41, p42;
+
+ p11 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]];
+ p21 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_front_tire[TIRE_OUTSIDE]];
+ p31 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_rear_tire[TIRE_OUTSIDE]];
+ p41 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_rear_tire[TIRE_OUTSIDE]];
+
+ struct RtkTime time1 = ((car_model *)(*iter))->tm;
+
+ ++iter;
+
+ p12 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]];
+ p22 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_front_tire[TIRE_OUTSIDE]];
+ p32 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_rear_tire[TIRE_OUTSIDE]];
+ p42 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_rear_tire[TIRE_OUTSIDE]];
+
+ struct RtkTime time2 = ((car_model *)(*iter))->tm;
+
+ if (TimeGetDiff(time1.hh, time1.mm, time1.ss, time1.mss, time2.hh, time2.mm, time2.ss, time2.mss) <= D_SEC(1)) {
+ MakeLine(&frontLeftTireTrack, &p11, &p12);
+ MakeLine(&frontRightTireTrack, &p21, &p22);
+ MakeLine(&rearLeftTireTrack, &p31, &p32);
+ MakeLine(&rearRightTireTrack, &p41, &p42);
+ track = true;
+ }
+ }
+
+ 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) {
+ p1 = RoadMap.roads[n].leftEdge[m].points[0];
+ for (int l = 1; l < RoadMap.roads[n].leftEdge[m].points.size(); ++l) {
+ p2 = RoadMap.roads[n].leftEdge[m].points[l];
+
+ 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;
+ }
+ }
+ }
+
+ for (int m = 0; m < RoadMap.roads[n].rightEdge.size(); ++m) {
+ if (RoadMap.roads[n].rightEdge[m].character == LINE_SOLID && RoadMap.roads[n].rightEdge[m].points.size() >= 2) {
+ p1 = RoadMap.roads[n].rightEdge[m].points[0];
+ for (int l = 1; l < RoadMap.roads[n].rightEdge[m].points.size(); ++l) {
+ p2 = RoadMap.roads[n].rightEdge[m].points[l];
+
+ 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;
+ }
+ }
+ }
+
+ for (int m = 0; m < RoadMap.roads[n].separate.size(); ++m) {
+ // 涓�缁勮溅閬�
+ for (int l = 0; l < RoadMap.roads[n].separate[m].lines.size(); ++l) {
+ // 澶氭牴鍒嗛亾绾�
+ for (int a = 0; a < RoadMap.roads[n].separate[m].lines[l].size(); ++a) {
+ // 涓�鏍瑰垎閬撶嚎涓嚎鍨嬬浉鍚岀殑
+ 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];
+
+ 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;
+ }
+ }
+ // 姣旇緝鏈�杩戠殑杩炵画鐨刵涓乏渚х偣鍜屼笂涓�涓彸渚х偣
+ if (character == LINE_HALF_SOLID_LEFT) {
+ // 涓嶈兘浠庡乏绉诲姩鍒板彸
+
+ }
+ }
+ }
+ }
+ }
+
+ return false;
+}
+
+/********************************************
+ * 璁＄畻鏌愮偣鍦ㄥ摢涓溅閬�
+ * @param point
+ * @param RoadMap
+ * @param roadIndex
+ * @return
+ */
+static bool GetLane(lane_t &theLane, PointF point, road_exam_map &RoadMap, int roadIndex)
+{
+ Line leftProjLine, rightProjLine;
+ Line sep;
+ PointF 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);
+
+ MakeLine(&leftProjLine, &point, &p1);
+ MakeLine(&rightProjLine, &point, &p2);
+
+ for (int i = 0; i < RoadMap.roads[roadIndex].separate.size(); ++i) {
+ struct car_lane_pos {
+ int lane_no;
+ char pos; // 鐐瑰湪杞﹂亾鐨勫乏鍙充綅缃�
+
+ bool operator == (const int &x) {
+ return(this->lane_no == x);
+ }
+ };
+
+ vector<struct car_lane_pos> CLP;
+ bool oneMoreIntersection = false;
+
+ for (int j = 0; j < RoadMap.roads[roadIndex].separate[i].lines.size(); ++j) { // 姣忎竴鏍圭嚎鍒嗙粍
+ bool intersection = false;
+ for (int k = 0; !intersection && k < RoadMap.roads[roadIndex].separate[i].lines[j].size(); ++k) { // 鍚屼竴绾垮瀷鐨勭嚎鍒嗙粍
+
+ p1 = RoadMap.roads[roadIndex].separate[i].lines[j][k].points[0];
+ for (int m = 1; m < RoadMap.roads[roadIndex].separate[i].lines[j][k].points.size(); ++m) {
+ p2 = RoadMap.roads[roadIndex].separate[i].lines[j][k].points[m];
+ MakeLine(&sep, &p1, &p2);
+
+ if (IntersectionOf(sep, leftProjLine) == GM_Intersection) {
+ intersection = true;
+ struct car_lane_pos temp;
+
+ temp.lane_no = j;
+ temp.pos = 'R';
+ CLP.push_back(temp);
+ break;
+ }
+ if (IntersectionOf(sep, rightProjLine) == GM_Intersection) {
+ intersection = true;
+ struct car_lane_pos temp;
+
+ temp.lane_no = j;
+ temp.pos = 'L';
+ CLP.push_back(temp);
+ break;
+ }
+ p1 = p2;
+ }
+ }
+ if (!intersection) {
+ struct car_lane_pos temp;
+ temp.lane_no = j;
+ temp.pos = 0;
+ CLP.push_back(temp);
+ } else {
+ oneMoreIntersection = true;
+ }
+ }
+
+ if (oneMoreIntersection) {
+ // 鑰冭檻鍒伴噰闆嗗亸宸彲鑳芥瘡涓�鏍圭嚎鏃犳硶瀵归綈锛屽彧瑕佸叾涓竴鏍圭浉浜わ紝鍏跺畠涓嶇浉浜ょ殑涔熷弬涓�
+ for (vector<struct car_lane_pos>::iterator it = CLP.begin(); it != CLP.end(); ++it) {
+ if ((*it).pos == 0) {
+ int m = RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no].size();
+ int n = RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points.size();
+
+ if (DistanceOf(RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][0].points[0], point) >
+ DistanceOf(RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points[n-1], point)) {
+ // 杞﹁締鎺ヨ繎杞﹂亾灏鹃儴
+ MakeLine(&sep, &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points[n-2], &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points[n-1]);
+ } else {
+ // 杞﹁締鎺ヨ繎杞﹂亾澶撮儴
+ MakeLine(&sep, &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][0].points[0], &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][0].points[1]);
+ }
+
+ struct car_lane_pos temp;
+
+ temp.lane_no = (*it).lane_no;
+
+ if (IntersectionOfLine(point, sep) == -1) {
+ temp.pos = 'R';
+ } else {
+ temp.pos = 'L';
+ }
+ (*it) = temp;
+ }
+ }
+
+ theLane.road = roadIndex;
+ theLane.sep = i;
+ theLane.lane = CLP.size();
+
+ for (int x = 0; x < CLP.size(); ++x) {
+ if (CLP[x].pos == 'L') {
+ theLane.lane = x;
+ break;
+ }
+ }
+
+ return true;
+ }
+ }
+
+ return false;
}
static void TurnSignalError13ColdTimer(union sigval sig)
@@ -331,6 +862,32 @@
void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
+ uint32_t cts = AppTimer_GetTickCount();
+ int ri = CalcRoadIndex(-1, RoadMap, car);
+ bool crash = CrashRedLine(0, RoadMap, car, CarModelList);
+ lane_t laneInfo;
+ double redist = -1;
+
+ laneInfo.road = -1;
+ laneInfo.sep = -1;
+ laneInfo.lane = -1;
+
+ if (ri >= 0) {
+ GetLane(laneInfo, car->carXY[car->axial[AXIAL_FRONT]], RoadMap, ri);
+
+ int m = RoadMap.roads[ri].rightEdge.size();
+ int n = RoadMap.roads[ri].rightEdge[m-1].points.size();
+
+ PointF base;
+
+ base.X = 428922.2985; base.Y = 3292119.5457;
+
+ redist = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], base,
+ RoadMap.roads[ri].rightEdge);
+ }
+
+ DEBUG("褰撳墠閬撹矾绱㈠紩 %d, 瑙﹀彂绾㈢嚎 %d lane %d 璺濈 %f %ld", ri, crash, laneInfo.lane, redist, AppTimer_GetTickCount() - cts);
+
// 琛岄┒璺濈锛屼笉鍖呭惈鍊掕溅
if (odoCnt == 0 && moveDirect == 1) {
odoPrevSpeed = speed;
@@ -1424,56 +1981,6 @@
return false;
}
-/*********************************************************************
- * 璁＄畻鏌愮偣鍒伴亾璺乏杈圭嚎鐨勬渶杩戝瀭鐐�
- * @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;
-}
/***************************************************
* 鎺ヨ繎璺彛鏃讹紝鎻愮ず涓嬩竴姝ユ�庝箞璧�
@@ -1515,7 +2022,7 @@
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]));
+ RoadMap.triggerLines[i].leftPoints.push_back(CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[j]));
}
for (int j = 0; j < RoadMap.triggerLines[i].points.size(); ++j) {
@@ -1630,12 +2137,12 @@
break;
}
- PointF vPoint = GetSELine(map.roads[road].leftEdge, map.specialAreas[i].area[0]);
+ 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 = GetSELine(map.roads[road].leftEdge, map.specialAreas[i].area[1]);
+ 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);
}
--
Gitblit v1.8.0