From 59abff0d03403344619420aa0bcd9c2b28ff3522 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期三, 08 四月 2020 11:06:58 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/driver_test.cpp | 451 ++-----------------------
lib/src/main/cpp/native-lib.h | 2
lib/src/main/cpp/test_items/park_edge.h | 2
lib/src/main/cpp/test_items/park_bottom.h | 2
app/src/main/assets/map.json | 73 ++--
lib/src/main/cpp/test_items/area_exam.cpp | 336 +++++++++++++++++++
lib/src/main/cpp/CMakeLists.txt | 2
lib/src/main/cpp/driver_test.h | 21
lib/src/main/cpp/test_common/Geometry.cpp | 15
lib/src/main/cpp/test_items/park_bottom.cpp | 2
lib/src/main/cpp/test_items/turn_a90.cpp | 2
lib/src/main/cpp/test_items/park_edge.cpp | 2
lib/src/main/cpp/master/comm_if.cpp | 101 +++--
lib/src/main/cpp/native-lib.cpp | 2
lib/src/main/cpp/test_items/area_exam.h | 5
lib/src/main/cpp/test_items2/through_something.cpp | 2
lib/src/main/cpp/test_common/Geometry.h | 0
17 files changed, 506 insertions(+), 514 deletions(-)
diff --git a/app/src/main/assets/map.json b/app/src/main/assets/map.json
index 0c6078c..e9be599 100644
--- a/app/src/main/assets/map.json
+++ b/app/src/main/assets/map.json
@@ -1,38 +1,37 @@
-[{
- "id": 876,
- "item": 3,
- "point": [{
- "x-y": [16589.2108, -7592.2752, 16587.738, -7588.9626, 16581.1917, -7592.3313, 16580.1238, -7590.1814, 16573.4773, -7593.6051, 16574.5939, -7595.7506, 16568.6578, -7598.8277, 16570.2548, -7602.0429]
- }]
-}, {
- "id": 879,
- "item": 2,
- "point": [{
- "x-y": [16631.9724, -7572.0083, 16640.7097, -7567.8933, 16638.8423, -7563.8859, 16639.3253, -7563.5842, 16641.2092, -7567.6242, 16641.4985, -7567.4958, 16639.6009, -7563.438, 16640.095, -7563.1417, 16642.0225, -7567.2532]
- }]
-}, {
- "id": 877,
- "item": 1,
- "point": [{
- "x-y": [16606.5718, -7592.7425, 16609.7972, -7598.9155, 16615.6195, -7595.8881, 16617.9794, -7600.5532, 16620.2055, -7599.3555, 16617.8385, -7594.7065, 16623.7326, -7591.5665, 16620.5822, -7585.3858]
- }]
-}, {
- "id": 878,
- "item": 5,
- "point": [{
- "x-y": [16601.7211, -7627.4499, 16604.7674, -7625.387, 16607.3525, -7630.2736, 16611.6697, -7628.0306, 16613.0442, -7631.3969, 16605.8191, -7635.1681]
- }]
-}, {
- "id": 875,
- "item": 4,
- "point": [{
- "x-y": [16582.5745, -7625.2715, 16583.3317, -7626.2782, 16584.3733, -7627.0041, 16585.3725, -7627.3071, 16586.6709, -7627.2873, 16587.8216, -7626.8839, 16588.8947, -7626.0781, 16589.5949, -7624.9904, 16590.0621, -7623.8121, 16590.628, -7622.7458, 16591.3904, -7621.7376, 16592.2946, -7620.9083, 16593.2589, -7620.2455, 16594.1722, -7619.8063, 16595.3586, -7619.4565, 16596.6087, -7619.2749, 16598.3161, -7619.4024, 16599.5229, -7619.7126, 16600.5292, -7620.151, 16601.5604, -7620.8035, 16602.481, -7621.6376, 16603.0784, -7622.348, 16603.3629, -7622.755]
+{
+ "items": [{
+ "id": 876,
+ "item": 3,
+ "point": [{
+ "x-y": [16589.2108, -7592.2752, 16587.738, -7588.9626, 16581.1917, -7592.3313, 16580.1238, -7590.1814, 16573.4773, -7593.6051, 16574.5939, -7595.7506, 16568.6578, -7598.8277, 16570.2548, -7602.0429]
+ }]
}, {
- "x-y": [16579.3086, -7626.9764, 16579.8354, -7627.7888, 16580.3393, -7628.4316, 16581.181, -7629.2701, 16582.0667, -7629.9052,
- 16583.1605, -7630.4413, 16584.3072, -7630.818, 16585.5416, -7630.993, 16586.918, -7630.9599, 16588.0948, -7630.7205, 16589.1489, -7630.3119, 16590.3826, -7629.6211, 16591.2856, -7628.8977, 16592.0383, -7628.0128,
-
- 16592.8408, -7626.7007, 16593.3354, -7625.4394, 16593.886, -7624.4265, 16594.9656, -7623.4808, 16596.0498, -7623.0286, 16597.3348, -7622.955, 16598.4564, -7623.2286,
- 16599.4254, -7623.7931, 16600.1166, -7624.5026
- ]
- }]
-}]
\ No newline at end of file
+ "id": 879,
+ "item": 2,
+ "point": [{
+ "x-y": [16631.9724, -7572.0083, 16640.7097, -7567.8933, 16638.8423, -7563.8859, 16639.3253, -7563.5842, 16641.2092, -7567.6242, 16641.4985, -7567.4958, 16639.6009, -7563.438, 16640.095, -7563.1417, 16642.0225, -7567.2532]
+ }]
+ }, {
+ "id": 877,
+ "item": 1,
+ "point": [{
+ "x-y": [16606.5718, -7592.7425, 16609.7972, -7598.9155, 16615.6195, -7595.8881, 16617.9794, -7600.5532, 16620.2055, -7599.3555, 16617.8385, -7594.7065, 16623.7326, -7591.5665, 16620.5822, -7585.3858]
+ }]
+ }, {
+ "id": 878,
+ "item": 5,
+ "point": [{
+ "x-y": [16601.7211, -7627.4499, 16604.7674, -7625.387, 16607.3525, -7630.2736, 16611.6697, -7628.0306, 16613.0442, -7631.3969, 16605.8191, -7635.1681]
+ }]
+ }, {
+ "id": 875,
+ "item": 4,
+ "point": [{
+ "x-y": [16582.5745, -7625.2715, 16583.3317, -7626.2782, 16584.3733, -7627.0041, 16585.3725, -7627.3071, 16586.6709, -7627.2873, 16587.8216, -7626.8839, 16588.8947, -7626.0781, 16589.5949, -7624.9904, 16590.0621, -7623.8121, 16590.628, -7622.7458, 16591.3904, -7621.7376, 16592.2946, -7620.9083, 16593.2589, -7620.2455, 16594.1722, -7619.8063, 16595.3586, -7619.4565, 16596.6087, -7619.2749, 16598.3161, -7619.4024, 16599.5229, -7619.7126, 16600.5292, -7620.151, 16601.5604, -7620.8035, 16602.481, -7621.6376, 16603.0784, -7622.348, 16603.3629, -7622.755]
+ }, {
+ "x-y": [16579.3086, -7626.9764, 16579.8354, -7627.7888, 16580.3393, -7628.4316, 16581.181, -7629.2701, 16582.0667, -7629.9052, 16583.1605, -7630.4413, 16584.3072, -7630.818, 16585.5416, -7630.993, 16586.918, -7630.9599, 16588.0948, -7630.7205, 16589.1489, -7630.3119, 16590.3826, -7629.6211, 16591.2856, -7628.8977, 16592.0383, -7628.0128, 16592.8408, -7626.7007, 16593.3354, -7625.4394, 16593.886, -7624.4265, 16594.9656, -7623.4808, 16596.0498, -7623.0286, 16597.3348, -7622.955, 16598.4564, -7623.2286, 16599.4254, -7623.7931, 16600.1166, -7624.5026]
+ }]
+ }],
+ "name": "绉戜簩鍦鸿�冨湴鍥�",
+ "type": "yard"
+}
\ No newline at end of file
diff --git a/lib/src/main/cpp/CMakeLists.txt b/lib/src/main/cpp/CMakeLists.txt
index 368252d..d6861f1 100644
--- a/lib/src/main/cpp/CMakeLists.txt
+++ b/lib/src/main/cpp/CMakeLists.txt
@@ -24,7 +24,7 @@
rtk_platform/parse_net.cpp
rtk_platform/platform.cpp
rtk_module/parse_gps.cpp
- Geometry.cpp
+ test_common/Geometry.cpp
driver_test.cpp
mcu/mcu_if.cpp
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index b502094..22b8ff2 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -18,7 +18,7 @@
#include "driver_test.h"
#include "defs.h"
-#include "Geometry.h"
+#include "test_common/Geometry.h"
#include "common/apptimer.h"
#include "jni_log.h"
#include "test_items/park_edge.h"
@@ -35,6 +35,7 @@
#include "mcu/mcu_if.h"
#include "test_common/car_sensor.h"
#include "test_items2/road_exam.h"
+#include "test_items/area_exam.h"
#define DEBUG(fmt, args...) LOGD("<driver_test> <%s>: " fmt, __func__, ##args)
@@ -59,29 +60,16 @@
static int ExamType;
static bool reportSeatbeltEject;
-static int CarInArea = 0;
-int errs = 0;
-
-vector<int> ErrorList;
vector<ExamFault> ExamFaultList;
static int examFaultIndex = 0;
-static struct map_list {
- int id;
- int type;
- Polygon map;
- Polygon map2;
-} MapList[MAP_LIST_SIZE];
+static LIST_AREA_MAP AreaMapList;
static Polygon RoadMapPoints;
static LIST_ROAD_MAP RoadMapList;
-static int MapNum = 0;
-static int CurrExamMapIndex = -1;
-static int CurrEnterMapIndex = -1;
-static int CurrExamStatus = EXAM_AREA_NONE; // 1 娴嬭瘯瀹屾垚 0 娴嬭瘯涓� -1 娴嬭瘯閿欒閫�鍑�
static int exam_dummy_light;
static car_model *CarModel = NULL;
@@ -102,7 +90,6 @@
static rtk_info *RtkBuffer = NULL;
static int RtkBufferNum = 0, RtkBufferIn = 0;
-static void DetectEnterOrExitMap(void);
static void EngineStartHold(union sigval sig);
static void ExecuteExam(const struct RtkTime* rtkTime);
static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime);
@@ -111,19 +98,15 @@
static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel);
static bool UpdateCarCoord(double &spd, int &mov, int &idx);
-static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList);
-static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, const struct map_list *mapList, int mapNum);
-static bool ExitMap(const car_model *car, int mapId, const struct map_list *mapList, int mapNum);
-static int GetMapId(int index, const struct map_list *mapList, int mapNum);
-static int GetMapType(int index, const struct map_list *mapList, int mapNum);
void DriverTestInit(void)
{
ExamStart = false;
- memset(&MapList, 0, sizeof(MapList));
- MapNum = 0;
+
CarModel = NULL;
CarModelList.clear();
+
+ AreaMapList.clear();
RoadMapPoints.num = 0;
RoadMapPoints.point = NULL;
@@ -137,8 +120,6 @@
RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info));
RtkBufferNum = RtkBufferIn = 0;
-
- CurrExamStatus = EXAM_AREA_NONE;
}
static void ReadDriverExamPrimerTimeout(union sigval sig)
@@ -155,54 +136,54 @@
MA_ReadSensor();
}
-void ClearMap(void)
+void ClearAreaMap(void)
{
- if (ExamStart) return;
-
- for (int i = 0; i < MapNum; ++i) {
- if (MapList[i].map.point != NULL)
- free(MapList[i].map.point);
- if (MapList[i].map2.point != NULL)
- free(MapList[i].map2.point);
- }
- memset(&MapList, 0, sizeof(MapList));
- MapNum = 0;
-}
-
-void AddMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2)
-{
- DEBUG("鍔犲叆鍦板浘淇℃伅 id %d type %d pointNum %d point2Num %d", id, type, pointNum, pointNum2);
-
- AppTimer_delete(ReadDriverExamPrimerTimeout);
-
- if (map == NULL || pointNum == 0 || ExamStart)
+ if (ExamStart)
return;
- MapList[MapNum].id = id;
+ for (int i = 0; i < AreaMapList.size(); ++i) {
+ if (AreaMapList[i].map.point != NULL)
+ free(AreaMapList[i].map.point);
+ if (AreaMapList[i].map2.point != NULL)
+ free(AreaMapList[i].map2.point);
+ }
- MapList[MapNum].type = type;
+ AreaMapList.clear();
+}
- MapList[MapNum].map.num = pointNum;
+void AddAreaMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2)
+{
+ if (map == NULL || pointNum == 0 || ExamStart)
+ return;
+ DEBUG("鍔犲叆鍦板浘淇℃伅 id %d type %d pointNum %d point2Num %d", id, type, pointNum, pointNum2);
+
+ struct area_exam_map newMap;
+
+ newMap.id = id;
+ newMap.type = type;
+ newMap.map.num = pointNum;
+ newMap.map2.num = 0;
+ newMap.map.point = NULL;
+ newMap.map2.point = NULL;
+
if (pointNum > 0) {
- MapList[MapNum].map.point = (PointF *)malloc(sizeof(PointF) * pointNum);
+ newMap.map.point = (PointF *) malloc(pointNum * sizeof(PointF));
for (int i = 0; i < pointNum; ++i) {
- MapList[MapNum].map.point[i].X = map[i][0];
- MapList[MapNum].map.point[i].Y = map[i][1];
+ newMap.map.point[i].X = map[i][0];
+ newMap.map.point[i].Y = map[i][1];
}
}
- MapList[MapNum].map2.num = pointNum2;
if (pointNum2 > 0 && map2 != NULL) {
- MapList[MapNum].map2.point = (PointF *)malloc(sizeof(PointF) * pointNum2);
+ newMap.map2.num = pointNum2;
+ newMap.map2.point = (PointF *) malloc(pointNum2 * sizeof(PointF));
for (int i = 0; i < pointNum2; ++i) {
- MapList[MapNum].map2.point[i].X = map2[i][0];
- MapList[MapNum].map2.point[i].Y = map2[i][1];
+ newMap.map2.point[i].X = map2[i][0];
+ newMap.map2.point[i].Y = map2[i][1];
}
}
- MapNum++;
-
- DEBUG("AddMap num %d", MapNum);
+ AreaMapList.push_back(newMap);
}
void CleanRoadMap(void)
@@ -550,14 +531,15 @@
DEBUG("缁撴潫鑰冭瘯");
TerminateRoadExam();
+ TerminateAreaExam();
- CurrExamMapIndex = -1;
+
ExamStart = false;
MA_SendExamStatus(0, 0);
return;
}
- if (MapNum == 0 && type == TEST_TYPE_AREA) {
+ if (AreaMapList.size() == 0 && type == TEST_TYPE_AREA) {
DEBUG("娌℃湁鍦鸿�冨湴鍥�");
err = true;
MA_SendExamStatus(0, -1);
@@ -590,21 +572,12 @@
if (type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
exam_dummy_light = 0;
-
-// InitRoadExam(); ////////////////////
+ }
+ if (type == TEST_TYPE_AREA) {
+ InitAreaExam();
}
}
MA_SendExamStatus(1, 0);
- }
-}
-
-void StartMapExam(int map_id, int exam)
-{
- DEBUG("娴嬭瘯璇ュ満鍦� %d: %d", map_id, exam);
-
- if (map_id >= 0 && exam == 0) {
- CurrExamMapIndex = map_id;
- CurrExamStatus = EXAM_AREA_START;
}
}
@@ -692,35 +665,8 @@
rtkTime.ss = RtkBuffer[index].ss;
rtkTime.mss = RtkBuffer[index].dss;
- DetectEnterOrExitMap();
-
if (ExamStart) {
ExecuteExam(speed, move, azimuth, &rtkTime);
- }
- }
-}
-
-static void DetectEnterOrExitMap(void)
-{
- if (ExamType == TEST_TYPE_AREA) {
-
- }
- if (CurrExamMapIndex < 0) {
- if (CurrEnterMapIndex < 0) {
- CurrEnterMapIndex = EnterMap(CarModel, CarModelList, MapList, MapNum);
- if (CurrEnterMapIndex >= 0) {
-// DEBUG("鍙戦�佽繘鍏ュ満鍦版姤鍛� %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
-// MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1);
-
- CurrExamMapIndex = CurrEnterMapIndex;
- CurrExamStatus = EXAM_AREA_START;
- }
- } else {
- if (ExitMap(CarModel, CurrEnterMapIndex, MapList, MapNum)) {
-// DEBUG("鍙戦�佺寮�鍦哄湴鎶ュ憡 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
-// MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0);
- CurrEnterMapIndex = -1;
- }
}
}
}
@@ -875,87 +821,8 @@
if (exam_dummy_light == 2) {
TestRoadGeneral(RoadMapList, CarModel, CarModelList, speed, move, rtkTime);
}
- return;
- }
-
- if (CurrExamMapIndex >= 0) {
- int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);
-
- if (CurrExamStatus == EXAM_AREA_START) {
- DEBUG("CurrExamMapIndex %d mtype %d", GetMapId(CurrExamMapIndex, MapList, MapNum), mtype);
-
- switch (mtype) {
- case MAP_TYPE_PARK_BUTTOM:
- DEBUG("杩涘叆鍊掕溅鍏ュ簱鍦哄湴");
- MA_SendDebugInfo("杩涘叆鍊掕溅鍏ュ簱鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
-
- StartParkBottom(GetMapId(CurrExamMapIndex, MapList, MapNum), move, rtkTime);
- CurrExamStatus = EXAM_AREA_RUN;
- break;
- case MAP_TYPE_STOP_START:
- DEBUG("杩涘叆涓婂潯璧锋鍦哄湴");
- MA_SendDebugInfo("杩涘叆涓婂潯璧锋鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
- StartSAS(GetMapId(CurrExamMapIndex, MapList, MapNum), move, rtkTime);
- CurrExamStatus = EXAM_AREA_RUN;
- break;
- case MAP_TYPE_PART_EDGE:
- DEBUG("杩涘叆渚ф柟浣嶅仠杞﹀満鍦�");
- MA_SendDebugInfo("杩涘叆渚ф柟浣嶅仠杞﹀満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
- StartParkEdge(GetMapId(CurrExamMapIndex, MapList, MapNum), move, rtkTime);
-
- CurrExamStatus = EXAM_AREA_RUN;
- break;
- case MAP_TYPE_CURVE:
- DEBUG("杩涘叆鏇茬嚎琛岄┒鍦哄湴");
- MA_SendDebugInfo("杩涘叆鏇茬嚎琛岄┒鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
- StartDrivingCurve(GetMapId(CurrExamMapIndex, MapList, MapNum), move, rtkTime);
-
- CurrExamStatus = EXAM_AREA_RUN;
- break;
- case MAP_TYPE_TURN_90:
- DEBUG("杩涘叆鐩磋杞集鍦哄湴");
- MA_SendDebugInfo("杩涘叆鐩磋杞集鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
- StartTurnA90(GetMapId(CurrExamMapIndex, MapList, MapNum), move, azimuth, rtkTime);
- CurrExamStatus = EXAM_AREA_RUN;
- break;
- default:break;
- }
- } else if (CurrExamStatus == EXAM_AREA_RUN) {
- int testing = 0;
- switch (mtype) {
- case MAP_TYPE_PARK_BUTTOM:
- testing = TestParkBottom(&MapList[CurrExamMapIndex].map,
- CarModel, NULL, speed, move, rtkTime);
- break;
- case MAP_TYPE_STOP_START:
- testing = TestSAS(&MapList[CurrExamMapIndex].map, CarModel, NULL, speed, move, rtkTime);
- break;
- case MAP_TYPE_PART_EDGE:
- testing = TestParkEdge(&MapList[CurrExamMapIndex].map, CarModel, NULL, speed, move, rtkTime);
- break;
- case MAP_TYPE_CURVE:
- testing = TestDrivingCurve(&MapList[CurrExamMapIndex].map, &MapList[CurrExamMapIndex].map2, CarModel, NULL, speed, move, rtkTime);
- break;
- case MAP_TYPE_TURN_90:
- testing = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, NULL, azimuth, speed, move, rtkTime);
- break;
- default:
- break;
- }
-
- if (testing > 0) {
- CurrExamStatus = EXAM_AREA_RUN;
- } else {
- CurrExamStatus = EXAM_AREA_END;
- }
- }
- if (CurrExamStatus != EXAM_AREA_RUN) {
- // 鏌愰」缁撴潫
- //DEBUG("閫�鍑哄満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
- //MA_SendDebugInfo("閫�鍑哄満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
- CurrExamStatus = EXAM_AREA_NONE;
- CurrExamMapIndex = -1;
- }
+ } else {
+ TestAreaGeneral(AreaMapList, CarModel, CarModelList, speed, move, azimuth, rtkTime);
}
}
@@ -1140,225 +1007,6 @@
return true;
}
-static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList)
-{
- bool trigger = false;
-
- if (CarModelList.size() < 5)
- return trigger;
-
- Polygon trace, trace2;
- int pn = 0;
-
- trace2.num = trace.num = 5;
- trace.point = (PointF *) malloc(sizeof(PointF) * trace.num);
- trace2.point = (PointF *) malloc(sizeof(PointF) * trace2.num);
-
- list<car_model *>::iterator iter = CarModelList.begin();
-
- car_model *c1 = *iter;
-
- trace.point[pn] = c1->carXY[c1->left_front_tire[TIRE_OUTSIDE]];
- trace2.point[pn++] = c1->carXY[c1->left_rear_tire[TIRE_OUTSIDE]];
-
- ++iter;
-
- while (iter != CarModelList.end() && pn < trace.num) {
- car_model *c2 = *iter;
-
- uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);
-
- if (tdiff >= D_SEC(1)) {
- trace.point[pn] = c2->carXY[c2->left_front_tire[TIRE_OUTSIDE]];
- trace2.point[pn++] = c2->carXY[c2->left_rear_tire[TIRE_OUTSIDE]];
- c1 = c2;
- }
- ++iter;
- }
-
- PointF p1, p2;
-
- p1.X = triggerLine.X1;
- p1.Y = triggerLine.Y1;
- p2.X = triggerLine.X2;
- p2.Y = triggerLine.Y2;
-
- int pp = 0;
- for (int p = 1; p < pn; ++p) {
- Line trace_line, trace2_line;
-
- MakeLine(&trace_line, &trace.point[pp], &trace.point[p]);
- MakeLine(&trace2_line, &trace2.point[pp], &trace2.point[p]);
-
- if ((IntersectionOf(trace_line, triggerLine) == GM_Intersection || IntersectionOf(trace2_line, triggerLine) == GM_Intersection) &&
- IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 &&
- DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], triggerLine) > 0.1) {
- // 纰板埌瑙﹀彂绾�
- DEBUG("纰版挒瑙﹀彂绾� 寮曞彂鍦板浘");
- trigger = true;
- goto SEARCH_TRIGGER_LINE_END;
- }
- }
-
-SEARCH_TRIGGER_LINE_END:
- free(trace.point);
-
- return trigger;
-}
-
-static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, const struct map_list *mapList, int mapNum)
-{
- for (int i = 0; i < mapNum && car != NULL; ++i) {
- // 杞﹀墠杞垨鍚庤疆杞ㄨ抗瓒婅繃瑙﹀彂绾�
- if (mapList[i].type == MAP_TYPE_STOP_START) {
- // 鏋勯�犺櫄鎷熺殑宸︿笂瑙掔偣
- double x9, y9, xo, yo;
-
- xo = (mapList[i].map.point[0].X + mapList[i].map.point[7].X) / 2;
- yo = (mapList[i].map.point[0].Y + mapList[i].map.point[7].Y) / 2;
-
- x9 = 2*xo - mapList[i].map.point[8].X;
- y9 = 2*yo - mapList[i].map.point[8].Y;
-
- Line triggerLine;
-
- triggerLine.X1 = mapList[i].map.point[0].X;
- triggerLine.Y1 = mapList[i].map.point[0].Y;
- triggerLine.X2 = x9;
- triggerLine.Y2 = y9;
-
- if (CrashTriggerLine(triggerLine, car, CarModelList))
- return i;
- }
- if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
- // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴
- if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
- Line enterLine1, enterLine2;
-
- MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
- MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
-
- if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
- DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
- return i;
- }
- }
- if (mapList[i].type == MAP_TYPE_PART_EDGE) {
- // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴
- if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
- Line enterLine;
-
- MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
-
- if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine) > 0.1)
- return i;
- }
- }
- if (mapList[i].type == MAP_TYPE_TURN_90) {
- // 杞﹀墠杞垨鍚庤疆杞ㄨ抗瓒婅繃瑙﹀彂绾�
- Line triggerLine;
-
- MakeLine(&triggerLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
-
- if (CrashTriggerLine(triggerLine, car, CarModelList))
- return i;
- }
- if (mapList[i].type == MAP_TYPE_CURVE) {
- Line triggerLine;
-
- MakeLine(&triggerLine, &mapList[i].map2.point[0], &mapList[i].map.point[0]);
- if (CrashTriggerLine(triggerLine, car, CarModelList))
- return i;
- }
- }
- return -1;
-}
-
-static bool ExitMap(const car_model *car, int index, const struct map_list *mapList, int mapNum)
-{
- bool ret = false;
- if (index < 0 || mapList == NULL || mapNum == 0) return true;
-
- if (mapList[index].type == MAP_TYPE_PARK_BUTTOM ||
- mapList[index].type == MAP_TYPE_PART_EDGE ||
- mapList[index].type == MAP_TYPE_TURN_90) {
- // 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
- 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, &mapList[index].map) == GM_None) {
- ret = true;
- }
-
- free(carBody.point);
- }
- if (mapList[index].type == MAP_TYPE_STOP_START) {
- // 鏋勯�犺櫄鎷熺殑宸︿笂瑙掔偣
- double x9, y9, xo, yo;
-
- bool enter = false;
-
- xo = (mapList[index].map.point[0].X + mapList[index].map.point[7].X) / 2;
- yo = (mapList[index].map.point[0].Y + mapList[index].map.point[7].Y) / 2;
-
- x9 = 2*xo - mapList[index].map.point[8].X;
- y9 = 2*yo - mapList[index].map.point[8].Y;
-
- Polygon map;
-
- map.num = 4;
- map.point = (PointF *) malloc(map.num * sizeof(PointF));
-
- map.point[0] = mapList[index].map.point[0];
- map.point[1] = mapList[index].map.point[8];
- map.point[2] = mapList[index].map.point[7];
- map.point[3].X = x9;
- map.point[3].Y = y9;
-
- // 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
- 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) == GM_None) {
- ret = true;
- }
-
- free(carBody.point);
- free(map.point);
- }
- if (mapList[index].type == MAP_TYPE_CURVE) {
- ret = ExitDrivingCurveArea(&mapList[index].map, &mapList[index].map2, car);
- }
-
- return ret;
-}
-
-static int GetMapId(int index, const struct map_list *mapList, int mapNum)
-{
- if (index < 0 || mapList == NULL || mapNum == 0)
- return -1;
-
- return mapList[index].id;
-}
-
-static int GetMapType(int index, const struct map_list *mapList, int mapNum)
-{
- if (index < 0 || mapList == NULL || mapNum == 0)
- return -1;
-
- return mapList[index].type;
-}
-
void AddExamFault(int wrong, const struct RtkTime *rtkTime)
{
struct ExamFault fault;
@@ -1383,11 +1031,6 @@
MA_SendExamWrong(ExamFaultList);
ExamFaultList.clear();
-}
-
-car_model_cache_t *GetCarModelCache(int node)
-{
- return NULL;
}
/*******************************************************************
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index 0cc5aa0..acdb6c2 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -5,7 +5,7 @@
#ifndef RTKDRIVERTEST_DRIVER_TEST_H
#define RTKDRIVERTEST_DRIVER_TEST_H
-#include "Geometry.h"
+#include "test_common/Geometry.h"
#include "rtk_module/rtk.h"
#include "test_items2/dummy_light.h"
@@ -125,14 +125,26 @@
typedef vector<struct road_exam_map> LIST_ROAD_MAP;
+struct area_exam_map {
+ int id;
+ int type;
+ Polygon map;
+ Polygon map2;
+};
+
+typedef vector<struct area_exam_map> LIST_AREA_MAP;
+
typedef list<car_model *> LIST_CAR_MODEL;
//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 ClearAreaMap(void);
+void AddAreaMap(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, int stopFlag, vector<vector<int>> &redLines,
@@ -145,11 +157,10 @@
int *body, int bodyNum, double (*point)[2], int pointNum, double antPitch, double antHeight, double groundHeight);
void StartDriverExam(int start, int type);
-void StartMapExam(int map_id, int exam);
void UpdateRTKInfo(const rtk_info *s);
void AddExamFault(int wrong, const struct RtkTime *rtkTime);
-car_model_cache_t *GetCarModelCache(int node);
+
void SystemShutdown(int event, int timeout);
void SetDummyLightExam(int n, struct dummy_light_exam *cfg);;
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index 90b4b1a..2dd5537 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -749,69 +749,73 @@
Document doc;
doc.Parse(value);
if (!doc.HasParseError()) {
- ClearMap();
+ ClearAreaMap();
- const Value &a = doc.GetArray();
+ if (doc.HasMember("items")) {
- if (a.IsArray()) {
- for (Value::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) {
- // a Map
- int id, type, pointNum = 0, point2Num = 0;
- double (*map)[2] = NULL, (*map2)[2] = NULL;
+ const Value &a = doc["items"];
- if (itr->IsObject()) {
- if (itr->HasMember("id")) {
- const Value &s = (*itr)["id"];
- id = s.GetInt();
- }
- if (itr->HasMember("item")) {
- const Value &s = (*itr)["item"];
- type = s.GetInt();
- }
- if (itr->HasMember("point")) {
- const Value &s = (*itr)["point"];
- int map_index = 0;
+ if (a.IsArray()) {
+ for (Value::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) {
+ // a Map
+ int id, type, pointNum = 0, point2Num = 0;
+ double (*map)[2] = NULL, (*map2)[2] = NULL;
- for (Value::ConstValueIterator itr2 = s.Begin();
- itr2 != s.End(); ++itr2, ++map_index) {
- // 鏇茬嚎椹鹃┒鏈�2缁�
- const Value &s2 = (*itr2)["x-y"];
+ if (itr->IsObject()) {
+ if (itr->HasMember("id")) {
+ const Value &s = (*itr)["id"];
+ id = s.GetInt();
+ }
+ if (itr->HasMember("item")) {
+ const Value &s = (*itr)["item"];
+ type = s.GetInt();
+ }
+ if (itr->HasMember("point")) {
+ const Value &s = (*itr)["point"];
+ int map_index = 0;
- if (map_index == 0) {
- int i = 0, j = 0;
- pointNum = s2.Size()/2;
- map = (double (*)[2]) new double[pointNum][2];
+ for (Value::ConstValueIterator itr2 = s.Begin();
+ itr2 != s.End(); ++itr2, ++map_index) {
+ // 鏇茬嚎椹鹃┒鏈�2缁�
+ const Value &s2 = (*itr2)["x-y"];
+
+ if (map_index == 0) {
+ int i = 0, j = 0;
+ pointNum = s2.Size() / 2;
+ map = (double (*)[2]) new double[pointNum][2];
// map = (double (*)[2]) malloc(pointNum * 2 * sizeof(double));
- for (Value::ConstValueIterator itr3 = s2.Begin();
- itr3 != s2.End(); ++itr3) {
- map[i][j] = (*itr3).GetDouble();
- if (++j == 2) {
- j = 0;
- i++;
+ for (Value::ConstValueIterator itr3 = s2.Begin();
+ itr3 != s2.End(); ++itr3) {
+ map[i][j] = (*itr3).GetDouble();
+ if (++j == 2) {
+ j = 0;
+ i++;
+ }
}
- }
- } else if (map_index == 1) {
- int i = 0, j = 0;
- point2Num = s2.Size()/2;
- map2 = (double (*)[2]) new double[s2.Size()][2];
+ } else if (map_index == 1) {
+ int i = 0, j = 0;
+ point2Num = s2.Size() / 2;
+ map2 = (double (*)[2]) new double[s2.Size()][2];
// map2 = (double (*)[2]) malloc(point2Num * 2 * sizeof(double));
- for (Value::ConstValueIterator itr3 = s2.Begin();
- itr3 != s2.End(); ++itr3) {
- map2[i][j] = (*itr3).GetDouble();
- if (++j == 2) {
- j = 0;
- i++;
+ for (Value::ConstValueIterator itr3 = s2.Begin();
+ itr3 != s2.End(); ++itr3) {
+ map2[i][j] = (*itr3).GetDouble();
+ if (++j == 2) {
+ j = 0;
+ i++;
+ }
}
}
}
}
- }
- AddMap(id, type, map, pointNum, map2, point2Num);
- if (map) delete []map;
- if (map2) delete []map2;
+ AddAreaMap(id, type, map, pointNum, map2, point2Num);
+
+ if (map) delete[]map;
+ if (map2) delete[]map2;
+ }
}
}
}
@@ -1006,7 +1010,6 @@
Value& s = doc["map_id"];
Value& s2 = doc["exam"];
- StartMapExam(s.GetInt(), s2.GetInt());
}
}
break;
diff --git a/lib/src/main/cpp/native-lib.cpp b/lib/src/main/cpp/native-lib.cpp
index cdc854e..206f071 100644
--- a/lib/src/main/cpp/native-lib.cpp
+++ b/lib/src/main/cpp/native-lib.cpp
@@ -9,7 +9,7 @@
#include "common/apptimer.h"
#include "rtk_platform/parse_net.h"
#include "native-lib.h"
-#include "Geometry.h"
+#include "test_common/Geometry.h"
#include "rtk_platform/platform.h"
#include "rtk_module/rtk.h"
#include "mcu/mcu_if.h"
diff --git a/lib/src/main/cpp/native-lib.h b/lib/src/main/cpp/native-lib.h
index 60b05cd..d109ac4 100644
--- a/lib/src/main/cpp/native-lib.h
+++ b/lib/src/main/cpp/native-lib.h
@@ -6,7 +6,7 @@
#define RTKBASESTATION_NATIVE_LIB_H
#include <cstdint>
-#include "Geometry.h"
+#include "test_common/Geometry.h"
char * GetImei(void);
diff --git a/lib/src/main/cpp/Geometry.cpp b/lib/src/main/cpp/test_common/Geometry.cpp
similarity index 95%
rename from lib/src/main/cpp/Geometry.cpp
rename to lib/src/main/cpp/test_common/Geometry.cpp
index 0a43ae1..db6deb5 100644
--- a/lib/src/main/cpp/Geometry.cpp
+++ b/lib/src/main/cpp/test_common/Geometry.cpp
@@ -2,7 +2,7 @@
// Created by YY on 2019/4/30.
//
-#include "defs.h"
+#include "../defs.h"
#include "Geometry.h"
#include <stdbool.h>
#include <stdint.h>
@@ -12,7 +12,7 @@
#include <initializer_list>
#include <cctype>
-#include "jni_log.h"
+#include "../jni_log.h"
using namespace std;
@@ -245,7 +245,6 @@
Relation IntersectionOf(Line line1, Line line2)
{
- // Fail if either line segment is zero-length.
if ((isEqual(line1.X1, line1.X2) && isEqual(line1.Y1, line1.Y2)) || (isEqual(line2.X1, line2.X2) && isEqual(line2.Y1, line2.Y2)))
return GM_None;
@@ -255,15 +254,14 @@
if ((isEqual(line1.X1, line2.X2) && isEqual(line1.Y1, line2.Y2)) || (isEqual(line1.X2, line2.X2) && isEqual(line1.Y2, line2.Y2)))
return GM_Intersection;
- // (1) Translate the system so that point A is on the origin.
+ // 鐩寸嚎鍧愭爣鍙樻崲閲嶅悎
line1.X2 -= line1.X1; line1.Y2 -= line1.Y1;
line2.X1 -= line1.X1; line2.Y1 -= line1.Y1;
line2.X2 -= line1.X1; line2.Y2 -= line1.Y1;
- // Discover the length of segment A-B.
double distAB = sqrt(line1.X2 * line1.X2 + line1.Y2 * line1.Y2);
- // (2) Rotate the system so that point B is on the positive X axis.
+ // 鏃嬭浆鍒癤杞�
double theCos = line1.X2 / distAB;
double theSin = line1.Y2 / distAB;
double newX = line2.X1 * theCos + line2.Y1 * theSin;
@@ -274,19 +272,16 @@
line2.Y2 = line2.Y2 * theCos - line2.X2 * theSin;
line2.X2 = newX;
- // Fail if segment C-D doesn't cross line A-B.
if ((line2.Y1 < 0 && line2.Y2 < 0) || (line2.Y1 >= 0 && line2.Y2 >= 0)) {
return GM_None;
}
- // (3) Discover the position of the intersection point along line A-B.
double posAB = line2.X2 + (line2.X1 - line2.X2) * line2.Y2 / (line2.Y2 - line2.Y1);
- // Fail if segment C-D crosses line A-B outside of segment A-B.
if (posAB < 0 || posAB > distAB) {
return GM_None;
}
- // (4) Apply the discovered position to line A-B in the original coordinate system.
+
return GM_Intersection;
}
diff --git a/lib/src/main/cpp/Geometry.h b/lib/src/main/cpp/test_common/Geometry.h
similarity index 100%
rename from lib/src/main/cpp/Geometry.h
rename to lib/src/main/cpp/test_common/Geometry.h
diff --git a/lib/src/main/cpp/test_items/area_exam.cpp b/lib/src/main/cpp/test_items/area_exam.cpp
index 8516dd1..e9bee0c 100644
--- a/lib/src/main/cpp/test_items/area_exam.cpp
+++ b/lib/src/main/cpp/test_items/area_exam.cpp
@@ -3,3 +3,339 @@
//
#include "area_exam.h"
+#include "../test_common/car_sensor.h"
+#include "../driver_test.h"
+#include "../jni_log.h"
+#include "park_bottom.h"
+#include "stop_and_start.h"
+#include "park_edge.h"
+#include "driving_curve.h"
+#include "turn_a90.h"
+#include "../utils/xconvert.h"
+#include "../common/apptimer.h"
+
+#define DEBUG(fmt, args...) LOGD("<area_exam> <%s>: " fmt, __func__, ##args)
+
+static int CurrExamStatus = EXAM_AREA_NONE; // 1 娴嬭瘯瀹屾垚 0 娴嬭瘯涓� -1 娴嬭瘯閿欒閫�鍑�
+static int CurrExamMapIndex = -1;
+static int CurrEnterMapIndex = -1;
+
+static void DetectEnterOrExitMap(const car_model *CarModel, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList);
+static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList);
+static bool ExitMap(const car_model *car, int index, LIST_AREA_MAP &mapList);
+static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList);
+static void ExecuteExam(int index, LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int move, double azimuth, const struct RtkTime* rtkTime);
+
+void TerminateAreaExam(void)
+{
+ CurrExamMapIndex = -1;
+}
+
+void InitAreaExam(void)
+{
+ CurrExamMapIndex = -1;
+}
+
+void TestAreaGeneral(LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, double azimuth, const struct RtkTime *rtkTime)
+{
+ DetectEnterOrExitMap(car, CarModelList, AreaMapList);
+
+ ExecuteExam(CurrExamMapIndex, AreaMapList, car, CarModelList, speed, moveDirect, azimuth, rtkTime);
+}
+
+static void DetectEnterOrExitMap(const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList)
+{
+ if (CurrExamMapIndex < 0) {
+ if (CurrEnterMapIndex < 0) {
+ CurrEnterMapIndex = EnterMap(car, CarModelList, mapList);
+ if (CurrEnterMapIndex >= 0) {
+ DEBUG("杩涘叆鏌愪釜瀛愰」鐩� idx = %d", CurrEnterMapIndex);
+ CurrExamMapIndex = CurrEnterMapIndex;
+ CurrExamStatus = EXAM_AREA_START;
+ }
+ } else {
+ if (ExitMap(car, CurrEnterMapIndex, mapList)) {
+ CurrEnterMapIndex = -1;
+ }
+ }
+ }
+}
+
+static void ExecuteExam(int index, LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int move, double azimuth, const struct RtkTime* rtkTime)
+{
+ if (index >= 0) {
+ if (CurrExamStatus == EXAM_AREA_START) {
+ DEBUG("CurrExamMapIndex %d mtype %d", AreaMapList[index].id, AreaMapList[index].type);
+
+ switch (AreaMapList[index].type) {
+ case MAP_TYPE_PARK_BUTTOM:
+ DEBUG("杩涘叆鍊掕溅鍏ュ簱鍦哄湴 %d", AreaMapList[index].id);
+
+ StartParkBottom(AreaMapList[index].id, move, rtkTime);
+ CurrExamStatus = EXAM_AREA_RUN;
+ break;
+ case MAP_TYPE_STOP_START:
+ DEBUG("杩涘叆涓婂潯璧锋鍦哄湴 %d", AreaMapList[index].id);
+
+ StartSAS(AreaMapList[index].id, move, rtkTime);
+ CurrExamStatus = EXAM_AREA_RUN;
+ break;
+ case MAP_TYPE_PART_EDGE:
+ DEBUG("杩涘叆渚ф柟浣嶅仠杞﹀満鍦� %d", AreaMapList[index].id);
+
+ StartParkEdge(AreaMapList[index].id, move, rtkTime);
+ CurrExamStatus = EXAM_AREA_RUN;
+ break;
+ case MAP_TYPE_CURVE:
+ DEBUG("杩涘叆鏇茬嚎琛岄┒鍦哄湴 %d", AreaMapList[index].id);
+
+ StartDrivingCurve(AreaMapList[index].id, move, rtkTime);
+ CurrExamStatus = EXAM_AREA_RUN;
+ break;
+ case MAP_TYPE_TURN_90:
+ DEBUG("杩涘叆鐩磋杞集鍦哄湴 %d", AreaMapList[index].id);
+
+ StartTurnA90(AreaMapList[index].id, move, azimuth, rtkTime);
+ CurrExamStatus = EXAM_AREA_RUN;
+ break;
+ default:break;
+ }
+ } else if (CurrExamStatus == EXAM_AREA_RUN) {
+ int testing = 0;
+ switch (AreaMapList[index].type) {
+ case MAP_TYPE_PARK_BUTTOM:
+ testing = TestParkBottom(&AreaMapList[index].map,
+ car, NULL, speed, move, rtkTime);
+ break;
+ case MAP_TYPE_STOP_START:
+ testing = TestSAS(&AreaMapList[index].map, car, NULL, speed, move, rtkTime);
+ break;
+ case MAP_TYPE_PART_EDGE:
+ testing = TestParkEdge(&AreaMapList[index].map, car, NULL, speed, move, rtkTime);
+ break;
+ case MAP_TYPE_CURVE:
+ testing = TestDrivingCurve(&AreaMapList[index].map, &AreaMapList[index].map2, car, NULL, speed, move, rtkTime);
+ break;
+ case MAP_TYPE_TURN_90:
+ testing = TestTurnA90(&AreaMapList[index].map, car, NULL, azimuth, speed, move, rtkTime);
+ break;
+ default:
+ break;
+ }
+
+ if (testing > 0) {
+ CurrExamStatus = EXAM_AREA_RUN;
+ } else {
+ CurrExamStatus = EXAM_AREA_END;
+ }
+ }
+ if (CurrExamStatus != EXAM_AREA_RUN) {
+ // 鏌愰」缁撴潫
+ CurrExamStatus = EXAM_AREA_NONE;
+ CurrExamMapIndex = -1;
+ }
+ }
+}
+
+static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList)
+{
+ for (int i = 0; i < mapList.size() && car != NULL; ++i) {
+ // 杞﹀墠杞垨鍚庤疆杞ㄨ抗瓒婅繃瑙﹀彂绾�
+ if (mapList[i].type == MAP_TYPE_STOP_START) {
+ // 鏋勯�犺櫄鎷熺殑宸︿笂瑙掔偣
+ double x9, y9, xo, yo;
+
+ xo = (mapList[i].map.point[0].X + mapList[i].map.point[7].X) / 2;
+ yo = (mapList[i].map.point[0].Y + mapList[i].map.point[7].Y) / 2;
+
+ x9 = 2*xo - mapList[i].map.point[8].X;
+ y9 = 2*yo - mapList[i].map.point[8].Y;
+
+ Line triggerLine;
+
+ triggerLine.X1 = mapList[i].map.point[0].X;
+ triggerLine.Y1 = mapList[i].map.point[0].Y;
+ triggerLine.X2 = x9;
+ triggerLine.Y2 = y9;
+
+ if (CrashTriggerLine(triggerLine, car, CarModelList))
+ return i;
+ }
+ if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
+ // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴
+ if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
+ Line enterLine1, enterLine2;
+
+ MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
+ MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
+
+ if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
+ DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
+ return i;
+ }
+ }
+ if (mapList[i].type == MAP_TYPE_PART_EDGE) {
+ // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴
+ if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
+ Line enterLine;
+
+ MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
+
+ if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine) > 0.1)
+ return i;
+ }
+ }
+ if (mapList[i].type == MAP_TYPE_TURN_90) {
+ // 杞﹀墠杞垨鍚庤疆杞ㄨ抗瓒婅繃瑙﹀彂绾�
+ Line triggerLine;
+
+ MakeLine(&triggerLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
+
+ if (CrashTriggerLine(triggerLine, car, CarModelList))
+ return i;
+ }
+ if (mapList[i].type == MAP_TYPE_CURVE) {
+ Line triggerLine;
+
+ MakeLine(&triggerLine, &mapList[i].map2.point[0], &mapList[i].map.point[0]);
+ if (CrashTriggerLine(triggerLine, car, CarModelList))
+ return i;
+ }
+ }
+ return -1;
+}
+
+static bool ExitMap(const car_model *car, int index, LIST_AREA_MAP &mapList)
+{
+ bool ret = false;
+ if (index < 0 || index >= mapList.size()) return true;
+
+ if (mapList[index].type == MAP_TYPE_PARK_BUTTOM ||
+ mapList[index].type == MAP_TYPE_PART_EDGE ||
+ mapList[index].type == MAP_TYPE_TURN_90) {
+ // 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
+ 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, &mapList[index].map) == GM_None) {
+ ret = true;
+ }
+
+ free(carBody.point);
+ }
+ if (mapList[index].type == MAP_TYPE_STOP_START) {
+ // 鏋勯�犺櫄鎷熺殑宸︿笂瑙掔偣
+ double x9, y9, xo, yo;
+
+ bool enter = false;
+
+ xo = (mapList[index].map.point[0].X + mapList[index].map.point[7].X) / 2;
+ yo = (mapList[index].map.point[0].Y + mapList[index].map.point[7].Y) / 2;
+
+ x9 = 2*xo - mapList[index].map.point[8].X;
+ y9 = 2*yo - mapList[index].map.point[8].Y;
+
+ Polygon map;
+
+ map.num = 4;
+ map.point = (PointF *) malloc(map.num * sizeof(PointF));
+
+ map.point[0] = mapList[index].map.point[0];
+ map.point[1] = mapList[index].map.point[8];
+ map.point[2] = mapList[index].map.point[7];
+ map.point[3].X = x9;
+ map.point[3].Y = y9;
+
+ // 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
+ 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) == GM_None) {
+ ret = true;
+ }
+
+ free(carBody.point);
+ free(map.point);
+ }
+ if (mapList[index].type == MAP_TYPE_CURVE) {
+ ret = ExitDrivingCurveArea(&mapList[index].map, &mapList[index].map2, car);
+ }
+
+ return ret;
+}
+
+static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList)
+{
+ bool trigger = false;
+
+ if (CarModelList.size() < 5)
+ return trigger;
+
+ Polygon trace, trace2;
+ int pn = 0;
+
+ trace2.num = trace.num = 5;
+ trace.point = (PointF *) malloc(sizeof(PointF) * trace.num);
+ trace2.point = (PointF *) malloc(sizeof(PointF) * trace2.num);
+
+ list<car_model *>::iterator iter = CarModelList.begin();
+
+ car_model *c1 = *iter;
+
+ trace.point[pn] = c1->carXY[c1->left_front_tire[TIRE_OUTSIDE]];
+ trace2.point[pn++] = c1->carXY[c1->left_rear_tire[TIRE_OUTSIDE]];
+
+ ++iter;
+
+ while (iter != CarModelList.end() && pn < trace.num) {
+ car_model *c2 = *iter;
+
+ uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);
+
+ if (tdiff >= D_SEC(1)) {
+ trace.point[pn] = c2->carXY[c2->left_front_tire[TIRE_OUTSIDE]];
+ trace2.point[pn++] = c2->carXY[c2->left_rear_tire[TIRE_OUTSIDE]];
+ c1 = c2;
+ }
+ ++iter;
+ }
+
+ PointF p1, p2;
+
+ p1.X = triggerLine.X1;
+ p1.Y = triggerLine.Y1;
+ p2.X = triggerLine.X2;
+ p2.Y = triggerLine.Y2;
+
+ int pp = 0;
+ for (int p = 1; p < pn; ++p) {
+ Line trace_line, trace2_line;
+
+ MakeLine(&trace_line, &trace.point[pp], &trace.point[p]);
+ MakeLine(&trace2_line, &trace2.point[pp], &trace2.point[p]);
+
+ if ((IntersectionOf(trace_line, triggerLine) == GM_Intersection || IntersectionOf(trace2_line, triggerLine) == GM_Intersection) &&
+ IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 &&
+ DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], triggerLine) > 0.1) {
+ // 纰板埌瑙﹀彂绾�
+ DEBUG("纰版挒瑙﹀彂绾� 寮曞彂鍦板浘");
+ trigger = true;
+ goto SEARCH_TRIGGER_LINE_END;
+ }
+ }
+
+ SEARCH_TRIGGER_LINE_END:
+ free(trace.point);
+
+ return trigger;
+}
diff --git a/lib/src/main/cpp/test_items/area_exam.h b/lib/src/main/cpp/test_items/area_exam.h
index a562acf..fe00467 100644
--- a/lib/src/main/cpp/test_items/area_exam.h
+++ b/lib/src/main/cpp/test_items/area_exam.h
@@ -5,4 +5,9 @@
#ifndef MYAPPLICATION2_AREA_EXAM_H
#define MYAPPLICATION2_AREA_EXAM_H
+#include "../driver_test.h"
+
+void InitAreaExam(void);
+void TerminateAreaExam(void);
+void TestAreaGeneral(LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, double azimuth, const struct RtkTime *rtkTime);
#endif //MYAPPLICATION2_AREA_EXAM_H
diff --git a/lib/src/main/cpp/test_items/park_bottom.cpp b/lib/src/main/cpp/test_items/park_bottom.cpp
index e200125..b4546f6 100644
--- a/lib/src/main/cpp/test_items/park_bottom.cpp
+++ b/lib/src/main/cpp/test_items/park_bottom.cpp
@@ -4,7 +4,7 @@
#include "park_bottom.h"
#include "../common/apptimer.h"
-#include "../Geometry.h"
+#include "../test_common/Geometry.h"
#include "../native-lib.h"
#include "../jni_log.h"
#include "../driver_test.h"
diff --git a/lib/src/main/cpp/test_items/park_bottom.h b/lib/src/main/cpp/test_items/park_bottom.h
index 69cd14e..695ba04 100644
--- a/lib/src/main/cpp/test_items/park_bottom.h
+++ b/lib/src/main/cpp/test_items/park_bottom.h
@@ -5,7 +5,7 @@
#ifndef RTKDRIVERTEST_PARK_BOTTOM_H
#define RTKDRIVERTEST_PARK_BOTTOM_H
-#include "../Geometry.h"
+#include "../test_common/Geometry.h"
#include "../driver_test.h"
#include <vector>
diff --git a/lib/src/main/cpp/test_items/park_edge.cpp b/lib/src/main/cpp/test_items/park_edge.cpp
index 6836600..9387896 100644
--- a/lib/src/main/cpp/test_items/park_edge.cpp
+++ b/lib/src/main/cpp/test_items/park_edge.cpp
@@ -4,7 +4,7 @@
#include "park_edge.h"
#include "../jni_log.h"
-#include "../Geometry.h"
+#include "../test_common/Geometry.h"
#include "../driver_test.h"
#include "../common/apptimer.h"
#include "../native-lib.h"
diff --git a/lib/src/main/cpp/test_items/park_edge.h b/lib/src/main/cpp/test_items/park_edge.h
index 8916ff8..e918c33 100644
--- a/lib/src/main/cpp/test_items/park_edge.h
+++ b/lib/src/main/cpp/test_items/park_edge.h
@@ -5,7 +5,7 @@
#ifndef RTKDRIVERTEST_PARK_EDGE_H
#define RTKDRIVERTEST_PARK_EDGE_H
-#include "../Geometry.h"
+#include "../test_common/Geometry.h"
#include "../driver_test.h"
#include <vector>
diff --git a/lib/src/main/cpp/test_items/turn_a90.cpp b/lib/src/main/cpp/test_items/turn_a90.cpp
index 4b3ba68..5f19758 100644
--- a/lib/src/main/cpp/test_items/turn_a90.cpp
+++ b/lib/src/main/cpp/test_items/turn_a90.cpp
@@ -3,7 +3,7 @@
//
#include "turn_a90.h"
-#include "../Geometry.h"
+#include "../test_common/Geometry.h"
#include "../driver_test.h"
#include "../common/apptimer.h"
#include "../jni_log.h"
diff --git a/lib/src/main/cpp/test_items2/through_something.cpp b/lib/src/main/cpp/test_items2/through_something.cpp
index 613a362..b67e989 100644
--- a/lib/src/main/cpp/test_items2/through_something.cpp
+++ b/lib/src/main/cpp/test_items2/through_something.cpp
@@ -4,7 +4,7 @@
#include "through_something.h"
#include "../driver_test.h"
-#include "../Geometry.h"
+#include "../test_common/Geometry.h"
#include "../native-lib.h"
#include "../jni_log.h"
#include "../test_common/car_sensor.h"
--
Gitblit v1.8.0