From 576d819c4d9aabd1a8d102ae9c3bc13c9165ee1f Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期五, 14 八月 2020 19:15:45 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/through_something.cpp | 308 ++++++++++++++++++++++++++++++--------------------
1 files changed, 183 insertions(+), 125 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/through_something.cpp b/lib/src/main/cpp/test_items2/through_something.cpp
index 9c70cff..c86aaec 100644
--- a/lib/src/main/cpp/test_items2/through_something.cpp
+++ b/lib/src/main/cpp/test_items2/through_something.cpp
@@ -12,151 +12,209 @@
#include "../jni_log.h"
#include "../test_common/car_sensor.h"
#include "road_exam.h"
+#include "../utils/xconvert.h"
#define DEBUG(fmt, args...) LOGD("<through_something> <%s>: " fmt, __func__, ##args)
-#if 0
using namespace std;
-static const double LASTEST_BREAK_POINT = 30.0;
-static const double NEXT_ROAD_TIP = 100.0; // 鍒拌揪璺彛鍓嶆彁绀轰笅涓�涓�庝箞璧�
-static const double DISTANCE_STOP_CAR_TO_STOP_LINE = 5.0;
+static map <int, int> TargetReduceRec;
+static map <int, int> TargetReduceRec2;
+
+#define NOT_ENTER 1
+#define ENTER_Z 2
+#define REDUCE_SPEED 4
+#define STOP_CAR 8
+#define OVER_SPEED 16
+
+static const double DISTANCE_STOP_CAR_TO_STOP_LINE = 3.0;
static const double PASS_SCHOOL_MAX_SPEED = 30.0; // kmh
+static const double LASTEST_BREAK_POINT = 30.0;
-static int breakActive;
-static int stopActive;
-static bool crashRedLine;
-
-
-
-void StartThroughExam(int index, LIST_ROAD_MAP &RoadMapList)
+static void SetTargetReduceRec(map<int, int> &table, int key, int status)
{
- if (index == -1)
+ auto it = table.find(key);
+
+ if (it != table.end()) {
+ table.erase(it);
+ }
+ table.insert(pair<int, int>(key, status));
+}
+
+static int GetTargetReduceRec(map<int, int> &table, int key)
+{
+ auto it = table.find(key);
+
+ if (it != table.end()) {
+ return it->second;
+ }
+ return NOT_ENTER;
+}
+
+static void RemoveTargetReduceRec(map<int, int> &table, int key)
+{
+ auto it = table.find(key);
+
+ if (it != table.end()) {
+ table.erase(it);
+ }
+}
+
+void ResetTarget(road_exam_map &RoadMap)
+{
+ TargetReduceRec.clear();
+ TargetReduceRec2.clear();
+}
+
+void ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime)
+{
+ if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
return;
- DEBUG("杩涘叆璺�冮�氳繃something鍦板浘 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type);
- if (!RoadMapList[index].tts.empty()) {
- DEBUG("鎾斁TTS %s", RoadMapList[index].tts.c_str());
- PlayTTS(RoadMapList[index].tts.c_str());
- } else {
- DEBUG("娌℃湁TTS");
- }
- breakActive = 0;
- stopActive = 0;
- crashRedLine = false;
-}
+ // 璺彛
+ for (int i = 0; i < RoadMap.roads[roadIndex].stopLine.size(); ++i) {
+ PointF point;
+ double distance;
-void InitThroughSomething(road_exam_map &RoadMap)
-{
- breakRecord.clear();
- // 涓烘瘡涓�涓埞杞︽娴嬬偣
- for (int i = 0; i < RoadMap.specialAreas.size(); ++i) {
+ point.X = RoadMap.roads[roadIndex].stopLine[i].line.X1;
+ point.Y = RoadMap.roads[roadIndex].stopLine[i].line.Y1;
+
+ distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, RoadMap.roads[roadIndex].rightEdge);
+
+ int key = roadIndex * 100 + i;
+ int rec = GetTargetReduceRec(TargetReduceRec, key);
+
+ if (distance > 1e-3 && distance < LASTEST_BREAK_POINT) {
+ if (rec == NOT_ENTER) {
+ SetTargetReduceRec(TargetReduceRec, key, ENTER_Z);
+ }
+ // 璁板綍鍒硅溅
+ if (dobreak && !(rec & REDUCE_SPEED)) {
+ SetTargetReduceRec(TargetReduceRec, key, rec | REDUCE_SPEED);
+ }
+ } else if (distance > 1e-3 && distance < DISTANCE_STOP_CAR_TO_STOP_LINE) {
+ // 璺彛鍋滆溅瑙傚療
+ if (moveDirect == 0 && !(rec & STOP_CAR)) {
+ SetTargetReduceRec(TargetReduceRec, key, rec | STOP_CAR);
+ }
+ } else if (distance > LASTEST_BREAK_POINT + 5 && rec != NOT_ENTER) {
+ RemoveTargetReduceRec(TargetReduceRec, key);
+ }
+ }
+ // 浜鸿閬撱�佸叕浜ょ珯銆佸鏍�
+ for (int i = 0; i < RoadMap.specialAreas.size(); i++) {
+ if (RoadMap.specialAreas[i].type == GRID_AREA || RoadMap.specialAreas[i].road != RoadMap.roads[roadIndex].id)
+ continue;
+
if (RoadMap.specialAreas[i].type == ZEBRA_CROSSING || RoadMap.specialAreas[i].type == BUS_STATION_AREA) {
- breakRecord.insert(pair<int, bool>(RoadMap.specialAreas[i].id, false));
- }
- }
- for (int i = 0; i < RoadMap.roads.size(); ++i) {
- breakRecord.insert(pair<int, bool>(RoadMap.roads[i].id, false));
- }
-}
+ double distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge);
-int ExecuteThroughExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
- LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
-{
- PointF p1, p2;
+ int key = i;
+ int rec = GetTargetReduceRec(TargetReduceRec2, key);
- double distance2StopLine = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]],
- RoadMapList[index].stopLine);
- p1.X = RoadMapList[index].stopLine.X1;
- p1.Y = RoadMapList[index].stopLine.Y1;
- p2.X = RoadMapList[index].stopLine.X2;
- p2.Y = RoadMapList[index].stopLine.Y2;
-
- if (IntersectionOfLine(p1, p2, car->carXY[car->axial[AXIAL_FRONT]]) == -1) {
- distance2StopLine = -distance2StopLine;
- }
-
- // 璺濈鍋滄绾�30绫冲唴鏄惁鏈夊埞杞﹀姩浣�
- if (breakActive == 0) {
- if (distance2StopLine > 0 && distance2StopLine <= LASTEST_BREAK_POINT) {
- if (ReadCarStatus(BREAK) == BREAK_ACTIVE) {
- breakActive = 1;
+ if (distance > 1e-3 && distance < LASTEST_BREAK_POINT) {
+ if (rec == NOT_ENTER) {
+ SetTargetReduceRec(TargetReduceRec2, key, ENTER_Z);
+ }
+ // 璁板綍鍒硅溅锛屽仠杞�
+ if (dobreak && !(rec & REDUCE_SPEED)) {
+ SetTargetReduceRec(TargetReduceRec2, key, rec | REDUCE_SPEED);
+ }
+ } else if (distance > LASTEST_BREAK_POINT + 5 && rec != NOT_ENTER) {
+ RemoveTargetReduceRec(TargetReduceRec2, key);
}
- }
+ } else if (RoadMap.specialAreas[i].type == SCHOOL_AREA) {
+ double distance1 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge);
+ double distance2 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[1], RoadMap.roads[roadIndex].rightEdge);
+ int key = i;
+ int rec = GetTargetReduceRec(TargetReduceRec2, key);
- if (distance2StopLine < 0 && breakActive != 1){
- breakActive = -1;
- // 涓嶆寜瑙勫畾鍑忛�燂紝涓嶅悎鏍�
- DEBUG("涓嶈俯涓嬪埞杞︼紝涓嶅悎鏍�");
- switch (RoadMapList[index].type) {
- case THROUGH_INTERSECTION_MAP:
- AddExamFault(41, rtkTime);
- break;
- case TURN_LEFT_MAP:
- AddExamFault(43, rtkTime);
- break;
- case TURN_RIGHT_MAP:
- AddExamFault(46, rtkTime);
- break;
- case THROUGH_ZEBRA_CROSSING_MAP:
- AddExamFault(48, rtkTime);
- break;
- case THROUGH_SCHOOL_MAP:
+ if (distance1 < -1e-3 && distance2 > 1e-3) {
+ if (rec == NOT_ENTER) {
+ SetTargetReduceRec(TargetReduceRec2, key, ENTER_Z);
+ }
+ if (ConvertMs2KMh(speed) > PASS_SCHOOL_MAX_SPEED && !(rec & OVER_SPEED)) {
+ SetTargetReduceRec(TargetReduceRec2, key, rec | OVER_SPEED);
+
+ DEBUG("閫氳繃瀛︽牎鍖哄煙瓒呴��");
AddExamFault(49, rtkTime);
- break;
- case THROUGH_BUS_STATION_MAP:
- AddExamFault(50, rtkTime);
- break;
- default:
- break;
+ }
+ } else if (rec != NOT_ENTER) {
+ RemoveTargetReduceRec(TargetReduceRec2, key);
}
}
}
-
- // 鏈夊仠姝㈡爣璁扮殑锛屾槸鍚﹀仠杞︼紙鍋滄绾�5绫冲唴鏈夊仠杞﹀姩浣滐級
- if (RoadMapList[index].flagStop != 0 && stopActive == 0) {
- if (distance2StopLine < 0.0) {
- // 涓嶅仠杞︾灜鏈涳紝涓嶅悎鏍�
- DEBUG("涓嶅仠杞︾灜鏈�");
- switch (RoadMapList[index].type) {
- case THROUGH_INTERSECTION_MAP:
- AddExamFault(42, rtkTime);
- break;
- case TURN_LEFT_MAP:
- AddExamFault(44, rtkTime);
- break;
- case TURN_RIGHT_MAP:
- AddExamFault(47, rtkTime);
- break;
- default:
- break;
- }
- stopActive = -1;
- } else if (distance2StopLine < DISTANCE_STOP_CAR_TO_STOP_LINE) {
- if (moveDirect == 0) {
- stopActive = 1;
- }
- }
- }
-
- if (CrashSonRedLine(index, RoadMapList, car, CarModelList)) {
- if (!crashRedLine) {
- DEBUG("涓嶆寜鑰冭瘯鍛樻寚浠よ椹�");
- crashRedLine = true;
- // 涓嶆寜鑰冭瘯鍛樻寚浠よ椹�
- AddExamFault(3, rtkTime);
- }
- } else {
- crashRedLine = false;
- }
-
- if (ExitSonArea(index, RoadMapList, car)) {
- DEBUG("绂诲紑閫氳繃something鍖哄煙");
- return -1;
- }
-
- return index;
}
-#endif
+void ExitTarget(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, const struct RtkTime *rtkTime)
+{
+ RECHECK:
+ for (auto it = TargetReduceRec.begin(); it != TargetReduceRec.end(); ++it) {
+ int road = it->first / 100;
+ int x = it->first % 100;
+ if (CrashTheLine(RoadMap.roads[road].stopLine[x].line, car, CarModelList)) {
+ DEBUG("瓒婅繃璺彛 road %d 璺彛 %d", road, x);
+ if (!(it->second & REDUCE_SPEED)) {
+ // 涓嶆寜瑙勫畾鍑忛�燂紝涓嶅悎鏍�
+ DEBUG("涓嶆寜瑙勫畾鍑忛��");
+ if (RoadMap.roads[road].stopLine[x].active == ROAD_ACTIVE_FORWARD) {
+ AddExamFault(41, rtkTime);
+ } else if (RoadMap.roads[road].stopLine[x].active ==
+ ROAD_ACTIVE_TURN_LEFT) {
+ AddExamFault(43, rtkTime);
+ } else if (RoadMap.roads[road].stopLine[x].active ==
+ ROAD_ACTIVE_TURN_RIGHT) {
+ AddExamFault(46, rtkTime);
+ }
+ }
+ if (!(it->second & STOP_CAR) && RoadMap.roads[road].stopLine[x].stopFlag) {
+ // 涓嶅仠杞︾灜鏈涳紝涓嶅悎鏍�
+ DEBUG("涓嶅仠杞︾灜鏈�");
+ if (RoadMap.roads[road].stopLine[x].active == ROAD_ACTIVE_FORWARD) {
+ AddExamFault(42, rtkTime);
+ } else if (RoadMap.roads[road].stopLine[x].active ==
+ ROAD_ACTIVE_TURN_LEFT) {
+ AddExamFault(44, rtkTime);
+ } else if (RoadMap.roads[road].stopLine[x].active ==
+ ROAD_ACTIVE_TURN_RIGHT) {
+ AddExamFault(47, rtkTime);
+ }
+ }
+
+ RemoveTargetReduceRec(TargetReduceRec, it->first);
+ goto RECHECK;
+ }
+ }
+
+ RECHECK2:
+ for (auto it = TargetReduceRec2.begin(); it != TargetReduceRec2.end(); ++it) {
+ int x = it->first;
+ Line line;
+ int roadIndex = 0;
+
+ for (; roadIndex < RoadMap.roads.size(); ++roadIndex) {
+ if (RoadMap.roads[roadIndex].id == RoadMap.specialAreas[x].road)
+ break;
+ }
+
+ PointF point2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].leftEdge,
+ RoadMap.specialAreas[x].area[1]);
+ MakeLine(&line, &RoadMap.specialAreas[x].area[1], &point2);
+
+ if (CrashTheLine(line, car, CarModelList)) {
+ if (RoadMap.specialAreas[x].type == ZEBRA_CROSSING &&
+ !(it->second & REDUCE_SPEED)) {
+ DEBUG("涓嶆寜瑙勫畾鍑忛��");
+ AddExamFault(48, rtkTime);
+ }
+ if (RoadMap.specialAreas[x].type == BUS_STATION_AREA &&
+ !(it->second & REDUCE_SPEED)) {
+ DEBUG("涓嶆寜瑙勫畾鍑忛��");
+ AddExamFault(50, rtkTime);
+ }
+ RemoveTargetReduceRec(TargetReduceRec2, it->first);
+ goto RECHECK2;
+ }
+ }
+}
--
Gitblit v1.8.0