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