From fb7b0660a319a9c54ff35c3944548348fae11b60 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期一, 24 八月 2020 18:53:23 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/through_something.cpp | 40 ++++++++++++++++++++++++++++++++--------
1 files changed, 32 insertions(+), 8 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/through_something.cpp b/lib/src/main/cpp/test_items2/through_something.cpp
index 7372634..f75d5db 100644
--- a/lib/src/main/cpp/test_items2/through_something.cpp
+++ b/lib/src/main/cpp/test_items2/through_something.cpp
@@ -66,10 +66,12 @@
TargetReduceRec2.clear();
}
-void ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime)
+double ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
+ vector<double> nearbyTarget; // 瀛︽牎銆佸叕浜ょ珯銆佷汉琛岄亾鍖哄煙鐨勮窛绂�
+
if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
- return;
+ return 100000;
// 璺彛
for (int i = 0; i < RoadMap.roads[roadIndex].stopLine.size(); ++i) {
PointF point;
@@ -106,12 +108,19 @@
continue;
if (RoadMap.specialAreas[i].type == ZEBRA_CROSSING || RoadMap.specialAreas[i].type == BUS_STATION_AREA) {
- double distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge);
+ 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 (distance > 1e-3 && distance < LASTEST_BREAK_POINT) {
+ if (distance1 < -1e-3 && distance2 > 1e-3) {
+ nearbyTarget.push_back(0);
+ } else if (distance1 > 1e-3 && distance2 > 1e-3) {
+ nearbyTarget.push_back(distance1);
+ }
+
+ if (distance1 > 1e-3 && distance1 < LASTEST_BREAK_POINT) {
if (rec == NOT_ENTER) {
SetTargetReduceRec(TargetReduceRec2, key, ENTER_Z);
}
@@ -119,30 +128,45 @@
if (dobreak && !(rec & REDUCE_SPEED)) {
SetTargetReduceRec(TargetReduceRec2, key, rec | REDUCE_SPEED);
}
- } else if (distance > LASTEST_BREAK_POINT + 5 && rec != NOT_ENTER) {
+ } else if (distance1 > 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 (distance1 < -1e-3 && distance2 > 1e-3) {
+ nearbyTarget.push_back(0);
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("閫氳繃瀛︽牎鍖哄煙瓒呴��");
+ DEBUG("閫氳繃瀛︽牎鍖哄煙瓒呴�� %f kmh", ConvertMs2KMh(speed));
AddExamFault(49, rtkTime);
}
- } else if (rec != NOT_ENTER) {
- RemoveTargetReduceRec(TargetReduceRec2, key);
+ } else if (distance1 < -1e-3 && distance2 < -1e-3) {
+ if (rec != NOT_ENTER) {
+ RemoveTargetReduceRec(TargetReduceRec2, key);
+ }
+ } else {
+ nearbyTarget.push_back(distance1);
+ if (rec != NOT_ENTER) {
+ RemoveTargetReduceRec(TargetReduceRec2, key);
+ }
}
}
}
+
+ if (nearbyTarget.size() > 0) {
+ sort(nearbyTarget.begin(), nearbyTarget.end());
+ return nearbyTarget[0];
+ }
+ return 100000;
}
void ExitTarget(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, const struct RtkTime *rtkTime)
--
Gitblit v1.8.0