From 148254bb1dc170db320bcb208ca79b0e252751d8 Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期三, 28 四月 2021 19:26:00 +0800
Subject: [PATCH] 修改科目三道路识别问题
---
lib/src/main/cpp/test_items/area_exam.cpp | 79 +++++++++++++++++++++++++++++----------
1 files changed, 59 insertions(+), 20 deletions(-)
diff --git a/lib/src/main/cpp/test_items/area_exam.cpp b/lib/src/main/cpp/test_items/area_exam.cpp
index 8c95819..1eba15d 100644
--- a/lib/src/main/cpp/test_items/area_exam.cpp
+++ b/lib/src/main/cpp/test_items/area_exam.cpp
@@ -22,8 +22,8 @@
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 void DetectEnterOrExitMap(int moveDirect, const car_model *CarModel, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList);
+static int EnterMap(int moveDirect, 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);
@@ -43,7 +43,7 @@
{
UpdataOdo(speed, moveDirect, rtkTime);
- DetectEnterOrExitMap(car, CarModelList, AreaMapList);
+ DetectEnterOrExitMap(moveDirect, car, CarModelList, AreaMapList);
ExecuteExam(CurrExamMapIndex, AreaMapList, car, CarModelList, speed, moveDirect, azimuth, rtkTime);
}
@@ -103,11 +103,11 @@
array.push_back(rd);
}
-static void DetectEnterOrExitMap(const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList)
+static void DetectEnterOrExitMap(int moveDirect, const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList)
{
if (CurrExamMapIndex < 0) {
if (CurrEnterMapIndex < 0) {
- CurrEnterMapIndex = EnterMap(car, CarModelList, mapList);
+ CurrEnterMapIndex = EnterMap(moveDirect, car, CarModelList, mapList);
if (CurrEnterMapIndex >= 0) {
DEBUG("杩涘叆鏌愪釜瀛愰」鐩� idx = %d", CurrEnterMapIndex);
CurrExamMapIndex = CurrEnterMapIndex;
@@ -199,7 +199,7 @@
}
}
-static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList)
+static int EnterMap(int moveDirect, const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList)
{
vector<int> score(mapList.size(), 0); // 鍦哄湴閲嶅悎鏃讹紝杞﹀ご灏鹃兘鍦ㄥ唴鐨勪紭鍏�
@@ -222,7 +222,7 @@
triggerLine.X2 = x9;
triggerLine.Y2 = y9;
- if (CrashTriggerLine(triggerLine, car, CarModelList))
+ if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
return i;
}
if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
@@ -230,14 +230,33 @@
MakeLine(&triggerLine, &(mapList[i].map.point[1]), &(mapList[i].map.point[0]));
- if (CrashTriggerLine(triggerLine, car, CarModelList))
+ if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
return i;
MakeLine(&triggerLine, &(mapList[i].map.point[7]), &(mapList[i].map.point[6]));
- if (CrashTriggerLine(triggerLine, car, CarModelList))
+ if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
return i;
+ // 鍏ㄨ溅鍦ㄥ満鍦板唴
+ if (moveDirect == 0) {
+ Polygon carBody;
+ bool ret = false;
+ 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[i].map) == GM_Containment) {
+ ret = true;
+ }
+
+ free(carBody.point);
+
+ if (ret)
+ return i;
+ }
// // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴
// if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
// Line enterLine1, enterLine2;
@@ -266,8 +285,28 @@
MakeLine(&triggerLine, &(mapList[i].map.point[1]), &(mapList[i].map.point[0]));
- if (CrashTriggerLine(triggerLine, car, CarModelList))
+ if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
return i;
+
+ // 鍏ㄨ溅鍦ㄥ満鍦板唴
+ if (moveDirect == 0) {
+ Polygon carBody;
+ bool ret = false;
+ 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[i].map) == GM_Containment) {
+ ret = true;
+ }
+
+ free(carBody.point);
+
+ if (ret)
+ return i;
+ }
// // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴
// if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
// Line enterLine;
@@ -293,7 +332,7 @@
MakeLine(&triggerLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
- if (CrashTriggerLine(triggerLine, car, CarModelList))
+ if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
return i;
}
if (mapList[i].type == MAP_TYPE_CURVE) {
@@ -301,7 +340,7 @@
Line triggerLine;
MakeLine(&triggerLine, &mapList[i].map2.point[0], &mapList[i].map.point[0]);
- if (CrashTriggerLine(triggerLine, car, CarModelList))
+ if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
return i;
}
}
@@ -424,8 +463,8 @@
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]];
+ trace.point[pn] = centerOfTwoPoint(c1->carXY[c1->left_front_tire[TIRE_OUTSIDE]], c1->carXY[c1->right_front_tire[TIRE_OUTSIDE]]);
+ trace2.point[pn++] = centerOfTwoPoint(c1->carXY[c1->left_rear_tire[TIRE_OUTSIDE]], c1->carXY[c1->right_rear_tire[TIRE_OUTSIDE]]);
++iter;
@@ -435,8 +474,8 @@
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]];
+ trace.point[pn] = centerOfTwoPoint(c2->carXY[c2->left_front_tire[TIRE_OUTSIDE]], c2->carXY[c2->right_front_tire[TIRE_OUTSIDE]]);
+ trace2.point[pn++] = centerOfTwoPoint(c2->carXY[c2->left_rear_tire[TIRE_OUTSIDE]], c2->carXY[c2->right_rear_tire[TIRE_OUTSIDE]]);
c1 = c2;
}
++iter;
@@ -457,11 +496,11 @@
MakeLine(&trace2_line, &trace2.point[pp], &trace2.point[p]);
if ((IntersectionOf(trace_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) ||
+ IntersectionOfLine(p1, p2, centerOfTwoPoint(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], car->carXY[car->right_front_tire[TIRE_OUTSIDE]])) == -1 &&
+ DistanceOf(centerOfTwoPoint(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]), triggerLine) > 0.1) ||
(IntersectionOf(trace2_line, triggerLine) == GM_Intersection &&
- IntersectionOfLine(p1, p2, car->carXY[car->left_rear_tire[TIRE_OUTSIDE]]) == -1 &&
- DistanceOf(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], triggerLine) > 0.1) ) {
+ IntersectionOfLine(p1, p2, centerOfTwoPoint(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], car->carXY[car->right_rear_tire[TIRE_OUTSIDE]])) == -1 &&
+ DistanceOf(centerOfTwoPoint(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]), triggerLine) > 0.1) ) {
// 纰板埌瑙﹀彂绾�
DEBUG("纰版挒瑙﹀彂绾� 寮曞彂鍦板浘");
trigger = true;
--
Gitblit v1.8.0