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