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/test_items/area_exam.cpp | 336 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 files changed, 336 insertions(+), 0 deletions(-) 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; +} -- Gitblit v1.8.0