From 4ff195404b21f74ca11e26a69cbf0418eaa4595f Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期二, 17 三月 2020 17:57:03 +0800
Subject: [PATCH] 科目三草案实现
---
lib/src/main/cpp/Geometry.h | 1
lib/src/main/cpp/test_items2/road_exam.h | 8
lib/src/main/cpp/driver_test.cpp | 189 +++++++++----
lib/src/main/cpp/master/comm_if.cpp | 249 ++++++------------
lib/src/main/cpp/Geometry.cpp | 48 +++
lib/src/main/cpp/test_items2/road_exam.cpp | 211 ++++++++++++++++
lib/src/main/cpp/CMakeLists.txt | 1
lib/src/main/cpp/driver_test.h | 30 ++
8 files changed, 512 insertions(+), 225 deletions(-)
diff --git a/lib/src/main/cpp/CMakeLists.txt b/lib/src/main/cpp/CMakeLists.txt
index 35a34fe..f1afd23 100644
--- a/lib/src/main/cpp/CMakeLists.txt
+++ b/lib/src/main/cpp/CMakeLists.txt
@@ -38,6 +38,7 @@
test_items2/common_check.cpp
test_items2/dummy_light.cpp
+ test_items2/road_exam.cpp
rtk_module/rtk.cpp
rtk_module/virtual_rtk.cpp
diff --git a/lib/src/main/cpp/Geometry.cpp b/lib/src/main/cpp/Geometry.cpp
index 39531c1..2693543 100644
--- a/lib/src/main/cpp/Geometry.cpp
+++ b/lib/src/main/cpp/Geometry.cpp
@@ -301,6 +301,52 @@
return fabs(point.X*line.Y1 + point.Y*line.X2 + line.X1*line.Y2 - line.X2*line.Y1 - line.X1*point.Y - point.X*line.Y2) / c;
}
+/*********************************************************
+ * p2----------->p1 绾跨鍜孻杞寸殑澶硅
+ * @param p1
+ * @param p2
+ * @return yaw
+ */
+double YawOf(PointF p1, PointF p2)
+{
+ double deg = 0.0;
+
+ if (fabs(p1.Y - p2.Y) <= GLB_EPSILON) {
+ if (p1.X > p2.X) {
+ deg = 90;
+ } else {
+ deg = 270;
+ }
+ } else if (fabs(p1.X - p2.X) <= GLB_EPSILON) {
+ if (p1.Y > p2.Y) {
+ deg = 0;
+ } else {
+ deg = 180;
+ }
+ } else {
+ deg = atan(fabs(p1.X - p2.X) /
+ fabs(p1.Y - p2.Y));
+
+ deg = toDegree(deg);
+
+ if (p1.X > p2.X &&
+ p1.Y > p2.Y) {
+
+ } else if (p1.X < p2.X &&
+ p1.Y > p2.Y) {
+ deg = 360 - deg;
+ } else if (p1.X < p2.X &&
+ p1.Y < p2.Y) {
+ deg = 180 + deg;
+ } else if (p1.X > p2.X &&
+ p1.Y < p2.Y) {
+ deg = 180 - deg;
+ }
+ }
+
+ return deg;
+}
+
/**********************************************************
* base 鍜� dest鐨勭浜岀偣閲嶅悎鏃跺舰鎴愮殑澶硅
* @param base
@@ -364,7 +410,7 @@
}
/***************************************************************
- * @brief p3浣嶄簬鐢眕1->p2鏋勬垚鐨勫皠绾匡紝宸︿晶杩樻槸鍙充晶锛屽悓鍚戯紝鍙嶅悜
+ * @brief p3浣嶄簬鐢眕1--------->p2鏋勬垚鐨勫皠绾匡紝宸︿晶杩樻槸鍙充晶锛屽悓鍚戯紝鍙嶅悜
* @param p1
* @param p2
* @param p3
diff --git a/lib/src/main/cpp/Geometry.h b/lib/src/main/cpp/Geometry.h
index 12db160..d050c05 100644
--- a/lib/src/main/cpp/Geometry.h
+++ b/lib/src/main/cpp/Geometry.h
@@ -48,6 +48,7 @@
Relation IntersectionOf(Line line1, Line line2);
double DistanceOf(PointF point1, PointF point2);
double DistanceOf(PointF point, Line line);
+double YawOf(PointF p1, PointF p2);
double CalculateAngle(Line base, Line dest);
PointF rotatePoint(PointF oldPoint, PointF centre, double degree);
bool InsidePolygon(const Polygon *t1, const Polygon *t2);
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 0898aa2..99eac50 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -14,6 +14,7 @@
#include <list>
#include <numeric>
#include <algorithm>
+#include <string>
#include "driver_test.h"
#include "defs.h"
@@ -88,42 +89,9 @@
Polygon map2;
} MapList[MAP_LIST_SIZE];
-struct trigger_line {
- int mapId;
- Line tirgger;
-};
+static Polygon RoadMapPoints;
-struct road_exam_parent_map {
- int id;
- int type;
- Polygon map;
- int redLineNum;
- Polygon *redLine;
- int greenLineNum;
- Polygon *greenLine;
- int redAreaNum;
- Polygon *redArea;
- int triggerLineNum;
- struct trigger_line *triggerLine;
-};
-
-static struct road_exam_parent_map * RoadParentMap;
-
-struct road_exam_son_map {
- int id;
- int type;
- char tts[512];
- Line triggerLine;
- Line controlLine;
- Line targetLine;
- int stop_flag;
-
- int redLineNum;
- Polygon *redLine;
-};
-
-typedef list<struct road_exam_son_map *> LIST_ROAD_SON_MAP;
-static LIST_ROAD_SON_MAP RoadSonMapList;
+static LIST_ROAD_MAP RoadMapList;
static int MapNum = 0;
static int CurrExamMapIndex = -1;
@@ -136,7 +104,7 @@
typedef list<car_model *> LIST_CAR_MODEL;
-static LIST_CAR_MODEL CarModelList;
+static LIST_CAR_MODEL CarModelList; // 涓�娈垫椂闂寸殑杞﹁締杞ㄨ抗闆嗗悎
static struct dummy_light_exam *DummyLightContent;
static int DummyLightContentSize;
@@ -174,8 +142,10 @@
CarModel = NULL;
CarModelList.clear();
- RoadParentMap = NULL;
- RoadSonMapList.clear();
+ RoadMapPoints.num = 0;
+ RoadMapPoints.point = NULL;
+
+ RoadMapList.clear();
CarSensorInit();
@@ -252,42 +222,133 @@
DEBUG("AddMap num %d", MapNum);
}
-void AddRoadMapParent(int id, int type, const double (*points)[2], int pointNum, const int **redLine,
- int redLineNum, const int **greenLine, int greenLineNum)
+void CleanRoadMap(void)
{
- int id;
- int type;
- Polygon map;
- int redLineNum;
- Polygon *redLine;
- int greenLineNum;
- Polygon *greenLine;
- int redAreaNum;
- Polygon *redArea;
- int triggerLineNum;
- struct trigger_line *triggerLine;
+ if (ExamStart) return;
- RoadParentMap = (struct road_exam_parent_map *)malloc(sizeof(struct road_exam_parent_map));
- RoadParentMap->id = id;
- RoadParentMap->type = type;
+ if (RoadMapPoints.point != NULL) {
+ free(RoadMapPoints.point);
+ }
+ RoadMapPoints.num = 0;
- RoadParentMap->map.num = pointNum;
- if (pointNum > 0) {
- RoadParentMap->map.point = (PointF *)malloc(sizeof(PointF) * pointNum);
- for (int i = 0; i < pointNum; ++i) {
- RoadParentMap->map.point[i].X = points[i][0];
- RoadParentMap->map.point[i].Y = points[i][1];
+ for (int i = 0; i < RoadMapList.size(); ++i) {
+ struct road_exam_map map = RoadMapList[i];
+
+ if (map.redLine != NULL) {
+ for (int j = 0; j < map.redLineNum; ++j) {
+ if (map.redLine[j].point != NULL)
+ free(map.redLine[j].point);
+ }
+ free(map.redLine);
+ }
+
+ if (map.greenLine != NULL) {
+ for (int j = 0; j < map.greenLineNum; ++j) {
+ if (map.greenLine[j].point != NULL)
+ free(map.greenLine[j].point);
+ }
+ free(map.greenLine);
+ }
+
+ if (map.redArea != NULL) {
+ for (int j = 0; j < map.redAreaNum; ++j) {
+ if (map.redArea[j].point != NULL)
+ free(map.redArea[j].point);
+ }
+ free(map.redArea);
+ }
+
+ if (map.triggerLine != NULL) {
+ for (int j = 0; j < map.triggerLineNum; ++j) {
+ if (map.triggerLine[j].point != NULL)
+ free(map.triggerLine[j].point);
+ }
+ free(map.triggerLine);
}
}
- RoadParentMap->redLineNum = redLineNum;
- RoadParentMap->redLine = (Polygon *) malloc(sizeof(Polygon *));
+ RoadMapList.clear();
+}
- for (int i = 0; i < redLineNum; ++i) {
- RoadParentMap->redLine[i].num =
+void SetRoadMapPoints(vector<double> &mapPoints)
+{
+ DEBUG("鍔犲叆璺�冨湴鍥剧偣闆嗗悎 num = %d", mapPoints.size()/2);
+
+ RoadMapPoints.num = mapPoints.size()/2;
+
+ if (RoadMapPoints.num > 0) {
+ RoadMapPoints.point = (PointF *)malloc(sizeof(PointF) * RoadMapPoints.num);
+ for (int i = 0; i < RoadMapPoints.num; ++i) {
+ RoadMapPoints.point[i].X = mapPoints[i*2];
+ RoadMapPoints.point[i].Y = mapPoints[i*2+1];
+ }
+ }
+}
+
+void AddRoadMapParent(int id, int type, string tts, vector<vector<int>> &redLines,
+ vector<vector<int>> &redAreas, vector<vector<int>> &greenLines, vector<vector<int>> &triggerLines)
+{
+ struct road_exam_map newMap;
+
+ newMap.id = id;
+ newMap.type = type;
+ newMap.tts = tts;
+
+ DEBUG("鍔犲叆璺�冨湴鍥句俊鎭� id = %d type = %d", id, type);
+
+ if ((newMap.redLineNum = redLines.size()) > 0) {
+ newMap.redLine = (Polygon *) malloc(sizeof(Polygon) * newMap.redLineNum);
+
+ for (int i = 0; i < newMap.redLineNum; ++i) {
+ newMap.redLine[i].num = redLines[i].size();
+ newMap.redLine[i].point = (PointF *) malloc(sizeof(PointF) * newMap.redLine[i].num);
+
+ for (int j = 0; j < newMap.redLine[i].num; ++j) {
+ newMap.redLine[i].point[j] = RoadMapPoints.point[redLines[i][j]];
+ }
+ }
}
+ if ((newMap.redAreaNum = redAreas.size()) > 0) {
+ newMap.redArea = (Polygon *) malloc(sizeof(Polygon) * newMap.redAreaNum);
+ for (int i = 0; i < newMap.redAreaNum; ++i) {
+ newMap.redArea[i].num = redAreas[i].size();
+ newMap.redArea[i].point = (PointF *) malloc(sizeof(PointF) * newMap.redLine[i].num);
+
+ for (int j = 0; j < newMap.redArea[i].num; ++j) {
+ newMap.redArea[i].point[j] = RoadMapPoints.point[redAreas[i][j]];
+ }
+ }
+ }
+
+ if ((newMap.greenLineNum = greenLines.size()) > 0) {
+ newMap.greenLine = (Polygon *) malloc(sizeof(Polygon) * newMap.greenLineNum);
+
+ for (int i = 0; i < newMap.greenLineNum; ++i) {
+ newMap.greenLine[i].num = greenLines[i].size();
+ newMap.greenLine[i].point = (PointF *) malloc(sizeof(PointF) * newMap.greenLine[i].num);
+
+ for (int j = 0; j < newMap.greenLine[i].num; ++j) {
+ newMap.greenLine[i].point[j] = RoadMapPoints.point[greenLines[i][j]];
+ }
+ }
+ }
+
+ if ((newMap.triggerLineNum = triggerLines.size()) > 0) {
+ newMap.triggerLine = (Polygon *) malloc(sizeof(Polygon) * newMap.triggerLineNum);
+
+ for (int i = 0; i < newMap.triggerLineNum; ++i) {
+ newMap.triggerLine[i].num = triggerLines[i].size();
+ newMap.triggerLine[i].point = (PointF *) malloc(sizeof(PointF) * newMap.triggerLine[i].num);
+
+ for (int j = 0; j < newMap.triggerLine[i].num; ++j) {
+ newMap.triggerLine[i].point[j] = RoadMapPoints.point[triggerLines[i][j]];
+ }
+ }
+ }
+
+ RoadMapList.push_back(newMap);
}
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index 24c2444..8a0ff4a 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -10,6 +10,7 @@
#include "test_items2/dummy_light.h"
#include <vector>
+#include <string>
#define EXAM_AREA_NONE 0
#define EXAM_AREA_START 1
@@ -81,12 +82,41 @@
int wrong_id;
};
+struct road_exam_map {
+ int id;
+ int type;
+
+ string tts;
+
+ int redLineNum;
+ Polygon *redLine;
+
+ int greenLineNum;
+ Polygon *greenLine;
+
+ int redAreaNum;
+ Polygon *redArea;
+
+ int triggerLineNum;
+ Polygon *triggerLine;
+
+ Line startLine;
+ Line endLine;
+};
+
+typedef vector<struct road_exam_map> LIST_ROAD_MAP;
+
//vector<ExamFault> ExamFaultList;
void DriverTestInit(void);
void ReadDriverExamPrimer(void);
void ClearMap(void);
void AddMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2);
+void CleanRoadMap(void);
+void SetRoadMapPoints(vector<double> &mapPoints);
+void AddRoadMapParent(int id, int type, string tts, vector<vector<int>> &redLines,
+ vector<vector<int>> &redAreas, vector<vector<int>> &greenLines, vector<vector<int>> &triggerLines);
+
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
int *body, int bodyNum, double (*point)[2], int pointNum, double antPitch);
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index 6101577..9f52d64 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -490,30 +490,100 @@
doc.Parse(value);
if (!doc.HasParseError()) {
- const Value &a = doc.GetArray();
- if (a.IsArray()) {
+ CleanRoadMap();
+
+ vector<double> mapPoints;
+ mapPoints.clear();
+
+ if (doc.HasMember("points")) {
+ const Value &s = doc["points"];
+ // X-Y鍧愭爣闆嗗悎
+ for (Value::ConstValueIterator itr2 = s.Begin();
+ itr2 != s.End(); ++itr2) {
+ mapPoints.push_back((*itr2).GetDouble());
+ }
+
+ SetRoadMapPoints(mapPoints);
+ }
+
+ if (doc.HasMember("maps")) {
+ const Value &a = doc["maps"];
+
for (Value::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) {
if (itr->IsObject()) {
// a Map
- int id, type, pointNum = 0;
+ int id, type;
int stop_flag = 0;
- char *tts = NULL;
- double (*map)[2] = NULL;
+ string tts;
- int redLineNum = 0, greenLineNum = 0, triggerLineNum = 0, redAreaNum = 0;
- int **redLine = NULL, **greenLine = NULL, **triggerLine = NULL, **redArea = NULL;
+ vector<vector<int>> redLines;
+ vector<vector<int>> greenLines;
+ vector<vector<int>> triggerLines;
+ vector<vector<int>> redAreas;
- struct line_t {
- int point_num;
- int *points;
- };
+ tts.clear();
+ redLines.clear();
+ greenLines.clear();
+ triggerLines.clear();
+ redAreas.clear();
- struct trigger_area {
- int map_id;
- int lineNum;
- int *line;
- } *triggerArea = NULL; // 瑙﹀彂鍚勫瓙椤圭殑绾�
- int triggerAreaNum;
+ vector<int> points;
+
+ if (itr->HasMember("red_line")) {
+ const Value &s = (*itr)["red_line"];
+
+ for (Value::ConstValueIterator itrLine = s.Begin();
+ itrLine != s.End(); ++itrLine) {
+ points.clear();
+ for (Value::ConstValueIterator itrPoint = (*itrLine).Begin();
+ itrPoint != (*itrLine).End(); ++itrPoint) {
+ points.push_back((*itrPoint).GetInt());
+ }
+ redLines.push_back(points);
+ }
+ }
+
+ if (itr->HasMember("green_line")) {
+ const Value &s = (*itr)["green_line"];
+
+ for (Value::ConstValueIterator itrLine = s.Begin();
+ itrLine != s.End(); ++itrLine) {
+ points.clear();
+ for (Value::ConstValueIterator itrPoint = (*itrLine).Begin();
+ itrPoint != (*itrLine).End(); ++itrPoint) {
+ points.push_back((*itrPoint).GetInt());
+ }
+ greenLines.push_back(points);
+ }
+ }
+
+ if (itr->HasMember("trigger_line")) {
+ const Value &s = (*itr)["trigger_line"];
+
+ for (Value::ConstValueIterator itrLine = s.Begin();
+ itrLine != s.End(); ++itrLine) {
+ points.clear();
+ for (Value::ConstValueIterator itrPoint = (*itrLine).Begin();
+ itrPoint != (*itrLine).End(); ++itrPoint) {
+ points.push_back((*itrPoint).GetInt());
+ }
+ triggerLines.push_back(points);
+ }
+ }
+
+ if (itr->HasMember("red_area")) {
+ const Value &s = (*itr)["red_area"];
+
+ for (Value::ConstValueIterator itrLine = s.Begin();
+ itrLine != s.End(); ++itrLine) {
+ points.clear();
+ for (Value::ConstValueIterator itrPoint = (*itrLine).Begin();
+ itrPoint != (*itrLine).End(); ++itrPoint) {
+ points.push_back((*itrPoint).GetInt());
+ }
+ redAreas.push_back(points);
+ }
+ }
if (itr->HasMember("id")) {
const Value &s = (*itr)["id"];
@@ -532,151 +602,10 @@
if (itr->HasMember("tts")) {
const Value &s = (*itr)["tts"];
- tts = new char[s.GetStringLength() + 1];
- strcpy(tts, s.GetString());
+ tts = s.GetString();
}
- // X-Y鍧愭爣闆嗗悎
- if (itr->HasMember("map")) {
- const Value &s = (*itr)["map"];
- int i = 0, j = 0;
-
- pointNum = s.Size() / 2;
- map = (double (*)[2]) new double[pointNum][2];
-
- for (Value::ConstValueIterator itr2 = s.Begin();
- itr2 != s.End(); ++itr2) {
- map[i][j] = (*itr2).GetDouble();
- if (++j == 2) {
- j = 0;
- i++;
- }
- }
- }
-
- //AddMap(id, type, map, pointNum, redLineNum, redLine, greenLine, redArea, triggerLine);
- // 绾㈢嚎闆嗗悎
- if (itr->HasMember("red_line")) {
- const Value &s = (*itr)["red_line"];
-
- redLineNum = s.Size();
- redLine = new int *[redLineNum];
- int x = 0;
- for (Value::ConstValueIterator itr2 = s.Begin();
- itr2 != s.End(); ++itr2) {
- // 鍙互杩炴帴涓轰竴鏉＄嚎鐨勭偣
- const Value &s2 = (*itr2)["line"];
-
- redLine[x] = new int[s2.Size()];
- int y = 0;
- for (Value::ConstValueIterator itr3 = s2.Begin();
- itr3 != s2.End(); ++itr3) {
- redLine[x][y++] = (*itr3).GetInt();
- }
- x++;
- }
- }
- // 缁跨嚎闆嗗悎
- if (itr->HasMember("green_line")) {
- const Value &s = (*itr)["green_line"];
-
- greenLineNum = s.Size();
- greenLine = new int *[greenLineNum];
- int x = 0;
- for (Value::ConstValueIterator itr2 = s.Begin();
- itr2 != s.End(); ++itr2) {
- // 鍙互杩炴帴涓轰竴鏉＄嚎鐨勭偣
- const Value &s2 = (*itr2)["line"];
-
- greenLine[x] = new int[s2.Size()];
- int y = 0;
- for (Value::ConstValueIterator itr3 = s2.Begin();
- itr3 != s2.End(); ++itr3) {
- greenLine[x][y++] = (*itr3).GetInt();
- }
- x++;
- }
- }
- // 绾㈠尯闆嗗悎
- if (itr->HasMember("red_area")) {
- const Value &s = (*itr)["red_area"];
-
- redAreaNum = s.Size();
- redArea = new int *[redAreaNum];
- int x = 0;
- for (Value::ConstValueIterator itr2 = s.Begin();
- itr2 != s.End(); ++itr2) {
- // 鍙互杩炴帴涓轰竴鏉＄嚎鐨勭偣
- const Value &s2 = (*itr2)["area"];
-
- redArea[x] = new int[s2.Size()];
- int y = 0;
- for (Value::ConstValueIterator itr3 = s2.Begin();
- itr3 != s2.End(); ++itr3) {
- redArea[x][y++] = (*itr3).GetInt();
- }
- x++;
- }
- }
- // 瑙﹀彂绾块泦鍚�
- if (itr->HasMember("trigger_line")) {
- const Value &s = (*itr)["trigger_line"];
-
- triggerAreaNum = s.Size();
- triggerArea = new struct trigger_area[triggerAreaNum];
-
- int x = 0;
- for (Value::ConstValueIterator itr2 = s.Begin();
- itr2 != s.End(); ++itr2) {
- // 鍙互杩炴帴涓轰竴鏉＄嚎鐨勭偣
- const Value &s2 = (*itr2)["line"];
- const Value &s3 = (*itr2)["map_id"];
-
- triggerArea[x].line = new int[s2.Size()];
- triggerArea[x].lineNum = s2.Size();
- triggerArea[x].map_id = s3.GetInt();
-
- int y = 0;
- for (Value::ConstValueIterator itr3 = s2.Begin();
- itr3 != s2.End(); ++itr3) {
- triggerArea[x].line[y++] = (*itr3).GetInt();
- }
- x++;
- }
- }
-
- if (type == 100) {
-
- } else {
-
- }
-
- if (map != NULL)
- delete []map;
- if (tts != NULL)
- delete []tts;
- if (redLineNum > 0) {
- for(int i=0; i < redLineNum; i++)
- delete[] redLine[i];
- delete[] redLine;
- }
- if (greenLineNum > 0) {
- for(int i=0; i < greenLineNum; i++)
- delete[] greenLine[i];
- delete[] greenLine;
- }
- if (redAreaNum > 0) {
- for(int i=0; i < redAreaNum; i++)
- delete[] redArea[i];
- delete[] redArea;
- }
- if (triggerAreaNum > 0) {
- for (int i = 0; i < triggerAreaNum; ++i) {
- delete [](triggerArea[i].line);
- }
- delete []triggerArea;
- }
- ///////////////////////
+ AddRoadMapParent(id, type, tts, redLines, redAreas, greenLines, triggerLines);
}
}
}
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
new file mode 100644
index 0000000..6826a10
--- /dev/null
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -0,0 +1,211 @@
+//
+// Created by YY on 2020/3/17.
+//
+
+#include "road_exam.h"
+#include "../driver_test.h"
+#include "../utils/xconvert.h"
+#include "../common/apptimer.h"
+#include "../jni_log.h"
+
+#define DEBUG(fmt, args...) LOGD("<road_exam> <%s>: " fmt, __func__, ##args)
+
+static bool occurCrashRedLine;
+static bool occurCrashGreenLine;
+static bool occurOverSpeed;
+static int checkCrashGreenTimeout;
+static char carIntersectionOfGreenLine;
+
+struct {
+ int hour;
+ int min;
+ int sec;
+ int msec;
+} crashGreenRunTime, crashGreenCmpTime;
+
+static const uint32_t CHANGE_ROAD_MIN_INTERVAL = D_SEC(10);
+static const uint32_t CRASH_DOTTED_LINE_TIMEOUT = D_SEC(10);
+static const double MAX_SPEED = 40.0 * 1000.0 / 3600.0;
+
+static bool CrashRedLine(LIST_ROAD_MAP &RoadMapList, const car_model *car);
+static bool CrashGreenLine(LIST_ROAD_MAP &RoadMapList, const car_model *car, PointF &p1, PointF &p2);
+
+void Init(void)
+{
+ crashGreenCmpTime.hour = -1;
+ occurCrashRedLine = false;
+ occurCrashGreenLine = false;
+ occurOverSpeed = false;
+ checkCrashGreenTimeout = 0;
+ carIntersectionOfGreenLine = 0;
+}
+
+void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, double speed, int moveStatus, const struct RtkTime *rtkTime)
+{
+ if (speed > MAX_SPEED) {
+ if (!occurOverSpeed) {
+ occurOverSpeed = true;
+ // 瓒呴�燂紝涓嶅悎鏍�
+ AddExamFault(10, rtkTime);
+ }
+ } else {
+ occurOverSpeed = false;
+ }
+
+ if (CrashRedLine(RoadMapList, car)) {
+ if (!occurCrashRedLine) {
+ // 杞﹁締琛岄┒涓獞杞ц溅閬撲腑蹇冨疄绾挎垨鑰呰溅閬撹竟缂樺疄绾匡紝涓嶅悎鏍�
+ AddExamFault(11, rtkTime);
+ occurCrashRedLine = true;
+ }
+ } else {
+ occurCrashRedLine = false;
+ }
+
+ static PointF p1, p2;
+
+ if (CrashGreenLine(RoadMapList, car, p1, p2)) {
+ if (moveStatus != 0) {
+ if (checkCrashGreenTimeout == 0) {
+ checkCrashGreenTimeout = 1;
+ crashGreenRunTime.hour = rtkTime->hh;
+ crashGreenRunTime.min = rtkTime->mm;
+ crashGreenRunTime.sec = rtkTime->ss;
+ crashGreenRunTime.msec = rtkTime->mss;
+ } else if (checkCrashGreenTimeout == 1) {
+ uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
+ crashGreenRunTime.hour, crashGreenRunTime.min, crashGreenRunTime.sec, crashGreenRunTime.msec*10);
+
+ if (diff >= CRASH_DOTTED_LINE_TIMEOUT) {
+ checkCrashGreenTimeout = 2;
+ // 闀挎椂闂撮獞杞ц溅閬撳垎鐣岀嚎琛岄┒锛屼笉鍚堟牸
+ AddExamFault(12, rtkTime);
+ }
+ }
+ } else {
+ // 鍋滆溅灏变笉璁℃椂浜�
+ checkCrashGreenTimeout = 0;
+ }
+
+ // 妫�娴嬪綋鍓嶈溅杈嗕簬铏氱嚎鐨勪綅缃紝鍋氬彉閬撴娴�;
+ // 妫�娴嬫槸鍚�3绉掑墠鏈夊紑鍚搴斾箣杞悜鐏�
+ if (!occurCrashGreenLine) {
+ occurCrashGreenLine = true;
+
+ }
+
+ // p1 ---------------> p2
+ double angle = car->yaw - YawOf(p2, p1);
+ if (angle < 0 || angle > 180) {
+ // 鍙充晶
+ carIntersectionOfGreenLine = 'R';
+ } else {
+ // 宸︿晶
+ carIntersectionOfGreenLine = 'L';
+ }
+ } else {
+ if (occurCrashGreenLine) {
+ int inter = IntersectionOfLine(p1, p2, car->basePoint);
+
+ // 鏈夎繃绾垮姩浣�
+ if ((inter == 1 && carIntersectionOfGreenLine == 'R') ||
+ (inter == -1 && carIntersectionOfGreenLine == 'L')) {
+ DEBUG("璺ㄨ秺铏氱嚎");
+
+ if (crashGreenCmpTime.hour >= 0) {
+ uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
+ rtkTime->mss * 10,
+ crashGreenCmpTime.hour, crashGreenCmpTime.min,
+ crashGreenCmpTime.sec, crashGreenCmpTime.msec * 10);
+
+ if (diff < CHANGE_ROAD_MIN_INTERVAL) {
+ // 杩炵画鍙橀亾锛屼笉鍚堟牸
+ AddExamFault(15, rtkTime);
+ }
+ }
+
+ crashGreenCmpTime.hour = rtkTime->hh;
+ crashGreenCmpTime.min = rtkTime->mm;
+ crashGreenCmpTime.sec = rtkTime->ss;
+ crashGreenCmpTime.msec = rtkTime->mss;
+ }
+ }
+ occurCrashGreenLine = false;
+ checkCrashGreenTimeout = 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 == 100) {
+
+ // 姣忔潯绾块兘妫�娴�
+ 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
+ */
+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 == 100) {
+
+ // 姣忔潯绾块兘妫�娴�
+ 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;
+}
diff --git a/lib/src/main/cpp/test_items2/road_exam.h b/lib/src/main/cpp/test_items2/road_exam.h
new file mode 100644
index 0000000..697d0c5
--- /dev/null
+++ b/lib/src/main/cpp/test_items2/road_exam.h
@@ -0,0 +1,8 @@
+//
+// Created by YY on 2020/3/17.
+//
+
+#ifndef MYAPPLICATION2_ROAD_EXAM_H
+#define MYAPPLICATION2_ROAD_EXAM_H
+
+#endif //MYAPPLICATION2_ROAD_EXAM_H
--
Gitblit v1.8.0