From 4bd08f0355b6b2cf3c027202d5ad301b4e182953 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期五, 31 三月 2023 17:16:22 +0800
Subject: [PATCH] 科目二修改
---
lib/src/main/cpp/test_items/area_exam.cpp | 682 ++++++++++++++++----------------------------------------
1 files changed, 199 insertions(+), 483 deletions(-)
diff --git a/lib/src/main/cpp/test_items/area_exam.cpp b/lib/src/main/cpp/test_items/area_exam.cpp
index 75f9da1..bed7a7c 100644
--- a/lib/src/main/cpp/test_items/area_exam.cpp
+++ b/lib/src/main/cpp/test_items/area_exam.cpp
@@ -16,17 +16,208 @@
#include "../common/apptimer.h"
#include "../test_common/odo_graph.h"
#include "../test_common/Geometry.h"
+#include "../common/observer.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;
+ilovers::Observer<std::function<void(move_status_t)>> CarMoveEvent;
-static void DetectEnterOrExitMap(int moveDirect, const car_model *CarModel, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList);
-static int EnterMap(int moveDirect, 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);
+static bool ProximityArea(Line &base_line, Line &line);
+static bool CrossingStartLine(Line &trace, Line &start_line);
+static void ProximityReminders(prime_t &prime);
+static void DetectCarMove(prime_t &prime);
+
+void AreaExam(prime_t &prime)
+{
+ switch (prime.curr_exam_map.type) {
+ case MAP_TYPE_PARK_BUTTOM:
+ break;
+ case MAP_TYPE_PARK_EDGE:
+ break;
+ case MAP_TYPE_TURN_90:
+ break;
+ case MAP_TYPE_UPHILL:
+ break;
+ case MAP_TYPE_CURVE:
+ break;
+ default:
+ if (EnterMap()) {
+
+ }
+ break;
+ }
+ DetectCarMove(prime);
+}
+
+static void DetectCarMove(prime_t &prime)
+{
+ static move_status_t prevMove = STOP;
+ if (prime.pMotion->move != prevMove) {
+ // Notify
+ CarMoveEvent.Notify(prime.pMotion->move);
+ prevMove = prime.pMotion->move;
+ }
+}
+
+void RegisterCarMoveObserver(void (*ptr)(move_status_t))
+{
+ CarMoveEvent.Connect(ptr);
+}
+
+void UnregisterCarMoveObserver(int handle)
+{
+ CarMoveEvent.Disconnect(handle);
+}
+
+void EnterMap(prime_t &prime)
+{
+ if (prime.curr_exam_map.type != 0) {
+ return;
+ }
+
+ if (prime.prev_modeling_index == -1 || prime.curr_modeling_index == -1) {
+ return;
+ }
+
+ PointF &lp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
+ PointF &lp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
+ PointF &rp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
+ PointF &rp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
+
+ Line left_trace, right_trace;
+
+ MAKE_LINE(left_trace, lp1, lp2);
+ MAKE_LINE(right_trace, rp1, rp2);
+
+ Line start_line;
+ for (int i = 0; i < prime.pMap->park_button_map.size(); ++i) {
+ MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[1], prime.pMap->park_button_map[i].map[0]);
+ if (CrossingStartLine(left_trace, start_line)) {
+ prime.curr_exam_map.type = MAP_TYPE_PARK_BUTTOM;
+ prime.curr_exam_map.map_idx = i;
+ StartParkBottom(prime);
+ return;
+ }
+ MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[7], prime.pMap->park_button_map[i].map[6]);
+ if (CrossingStartLine(left_trace, start_line)) {
+ prime.curr_exam_map.type = MAP_TYPE_PARK_BUTTOM;
+ prime.curr_exam_map.map_idx = i;
+ StartParkBottom(prime);
+ return;
+ }
+ }
+
+ for (int i = 0; i < prime.pMap->park_edge_map.size(); ++i) {
+ MAKE_LINE(start_line, prime.pMap->park_edge_map[i].map[1], prime.pMap->park_edge_map[i].map[0]);
+ if (CrossingStartLine(left_trace, start_line)) {
+ prime.curr_exam_map.type = MAP_TYPE_PARK_EDGE;
+ prime.curr_exam_map.map_idx = i;
+ return;
+ }
+ }
+
+ for (int i = 0; i < prime.pMap->uphill_map.size(); ++i) {
+ PointF vPoint = Calc3Point(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[0], DistanceOf(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[7]), 'R');
+ MAKE_LINE(start_line, prime.pMap->uphill_map[i].map[0], vPoint);
+ if (CrossingStartLine(left_trace, start_line)) {
+ prime.curr_exam_map.type = MAP_TYPE_UPHILL;
+ prime.curr_exam_map.map_idx = i;
+ return;
+ }
+ }
+ for (int i = 0; i < prime.pMap->curve_map.size(); ++i) {
+ MAKE_LINE(start_line, prime.pMap->curve_map[i].right_start_point, prime.pMap->curve_map[i].left_start_point);
+ if (CrossingStartLine(left_trace, start_line)) {
+ prime.curr_exam_map.type = MAP_TYPE_CURVE;
+ prime.curr_exam_map.map_idx = i;
+ return;
+ }
+ }
+ for (int i = 0; i < prime.pMap->turn_a90_map.size(); ++i) {
+ MAKE_LINE(start_line, prime.pMap->turn_a90_map[i].map[0], prime.pMap->turn_a90_map[i].map[1]);
+ if (CrossingStartLine(left_trace, start_line)) {
+ prime.curr_exam_map.type = MAP_TYPE_TURN_90;
+ prime.curr_exam_map.map_idx = i;
+ return;
+ }
+ }
+}
+
+// 杞﹁疆椹惰繃绾匡紝涓旇溅澶翠綅浜庡彸渚�
+static bool CrossingStartLine(Line &trace, Line &start_line)
+{
+ PointF head = {.X = trace.X1, .Y = trace.Y1};
+
+ if (IntersectionOf(trace, start_line) == GM_Intersection
+ && IntersectionOfLine(head, start_line) == RELATION_RIGHT) {
+ return true;
+ }
+ return false;
+}
+
+void FarawayMap(prime_t &prime)
+{
+ if (prime.arriving_map.type != 0) {
+ PointF &car_head = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]];
+ PointF car_head_trend = PointExtend(car_head, 7, prime.pModeling->yaw);
+ Line car_head_line;
+ MAKE_LINE(car_head_line, car_head, car_head_trend);
+
+
+ }
+}
+
+static void ProximityReminders(prime_t &prime)
+{
+ if (prime.curr_exam_map.type != MAP_TYPE_NONE && prime.arriving_map.type != MAP_TYPE_NONE) {
+ return;
+ }
+
+ PointF &car_head = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]];
+ PointF car_head_trend = PointExtend(car_head, 6, prime.pModeling->yaw);
+ Line car_head_line;
+ MAKE_LINE(car_head_line, car_head, car_head_trend);
+
+ Line start_line;
+ for (int i = 0; i < prime.pMap->park_button_map.size(); ++i) { // 宸﹀彸2鏉℃帶鍒剁嚎閮藉彲浣滀负鍏ュ彛
+ MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[1], prime.pMap->park_button_map[i].map[0]);
+
+ MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[7], prime.pMap->park_button_map[i].map[6]);
+
+ }
+ for (int i = 0; i < prime.pMap->park_edge_map.size(); ++i) {
+ MAKE_LINE(start_line, prime.pMap->park_edge_map[i].map[1], prime.pMap->park_edge_map[i].map[0]);
+ }
+
+ for (int i = 0; i < prime.pMap->uphill_map.size(); ++i) {
+ PointF vPoint = Calc3Point(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[0], DistanceOf(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[7]), 'R');
+ MAKE_LINE(start_line, prime.pMap->uphill_map[i].map[0], vPoint);
+ }
+ for (int i = 0; i < prime.pMap->curve_map.size(); ++i) {
+ MAKE_LINE(start_line, prime.pMap->curve_map[i].right_start_point, prime.pMap->curve_map[i].left_start_point);
+ }
+ for (int i = 0; i < prime.pMap->turn_a90_map.size(); ++i) {
+ MAKE_LINE(start_line, prime.pMap->turn_a90_map[i].map[0], prime.pMap->turn_a90_map[i].map[1]);
+ }
+}
+
+// 鍒ゆ柇鏄惁鎺ヨ繎鍦哄湴鐨勮捣濮嬬嚎
+// 杞﹀ご瓒嬪娍绾挎槸鍚﹀拰鍏ュ彛绾跨浉浜�
+// 鍜屽叆鍙ｇ嚎鐨勫す瑙�
+static bool ProximityArea(Line &base_line, Line &line)
+{
+ PointF head = {.X = line.X1, .Y = line.Y1};
+
+ if (IntersectionOf(base_line, line) == GM_Intersection
+ && DistanceOf(head, base_line) > 1) {
+ double angle = AngleOfTowLine(base_line, line);
+ if (angle >= 240 && angle <= 300) {
+ return true;
+ }
+ }
+
+ return false;
+}
void TerminateAreaExam(void)
{
@@ -37,479 +228,4 @@
{
CurrExamMapIndex = -1;
ResetOdo();
-}
-
-void TestAreaGeneral(LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, double azimuth, const struct RtkTime *rtkTime)
-{
- UpdataOdo(speed, moveDirect, rtkTime);
-
- DetectEnterOrExitMap(moveDirect, car, CarModelList, AreaMapList);
-
- ExecuteExam(CurrExamMapIndex, AreaMapList, car, CarModelList, speed, moveDirect, azimuth, rtkTime);
-}
-
-void DistanceOfTire2X(std::vector<double> &array, const car_model *car, std::vector<Line> line_set)
-{
- double ld = 100, rd = 100;
-
- array.clear();
-
- for (auto line = line_set.begin(); line != line_set.end(); line++) {
- double ld_t = 100, rd_t = 100;
- double d;
- if (VerticalPointOnLine(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], *line)) {
- d = DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], *line);
- if (d < ld_t) {
- ld_t = d;
- }
- }
- if (VerticalPointOnLine(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], *line)) {
- d = DistanceOf(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], *line);
- if (d < ld_t) {
- ld_t = d;
- }
- }
-
- if (VerticalPointOnLine(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], *line)) {
- d = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], *line);
- if (d < rd_t) {
- rd_t = d;
- }
- }
- if (VerticalPointOnLine(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], *line)) {
- d = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], *line);
- if (d < rd_t) {
- rd_t = d;
- }
- }
-
- if (isEqual2(ld_t, rd_t)) {
- if (!isEqual(ld_t, 100)) {
- if (rd_t < rd)
- rd = rd_t;
- if (ld_t < ld)
- ld = ld_t;
- }
- } else if (ld_t > rd_t) {
- if (rd_t < rd)
- rd = rd_t;
- } else {
- if (ld_t < ld)
- ld = ld_t;
- }
- }
-
- array.push_back(ld);
- array.push_back(rd);
-}
-
-static void DetectEnterOrExitMap(int moveDirect, const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList)
-{
- if (CurrExamMapIndex < 0) {
- if (CurrEnterMapIndex < 0) {
- CurrEnterMapIndex = EnterMap(moveDirect, car, CarModelList, mapList);
- if (CurrEnterMapIndex >= 0) {
- DEBUG("杩涘叆鏌愪釜瀛愰」鐩� idx = %d", CurrEnterMapIndex);
- CurrExamMapIndex = CurrEnterMapIndex;
- CurrExamStatus = EXAM_AREA_START;
- }
- } else {
- if (ExitMap(car, CurrEnterMapIndex, mapList)) {
- DEBUG("绂诲紑鏌愪釜瀛愰」鐩� idx = %d", CurrEnterMapIndex);
- 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, &AreaMapList[index].map, car, 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;
- DEBUG("缁撴潫褰撳墠椤圭洰");
- }
- }
-}
-
-static int EnterMap(int moveDirect, const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList)
-{
- vector<int> score(mapList.size(), 0); // 鍦哄湴閲嶅悎鏃讹紝杞﹀ご灏鹃兘鍦ㄥ唴鐨勪紭鍏�
-
- 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 (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
- return i;
- }
- if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
- Line triggerLine;
-
- MakeLine(&triggerLine, &(mapList[i].map.point[1]), &(mapList[i].map.point[0]));
-
- if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
- return i;
-
- MakeLine(&triggerLine, &(mapList[i].map.point[7]), &(mapList[i].map.point[6]));
-
- if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
- return i;
-
- // 鍏ㄨ溅鍦ㄥ満鍦板唴
- if (moveDirect == 0) {
- Polygon carBody;
- bool ret = false;
- 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[i].map) == GM_Containment) {
- ret = true;
- }
-
- free(carBody.point);
-
- if (ret)
- return i;
- }
-// // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴
-// 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)
-// score[i]++;
-// }
-// // 杞﹀熬椤剁偣鍦ㄥ満鍦板唴
-// if (IntersectionOf(car->carXY[ car->axial[AXIAL_REAR] ], &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_REAR]], enterLine1) > 0.1 &&
-// DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine2) > 0.1)
-// score[i]++;
-// }
- }
- if (mapList[i].type == MAP_TYPE_PART_EDGE) {
- Line triggerLine;
-
- MakeLine(&triggerLine, &(mapList[i].map.point[1]), &(mapList[i].map.point[0]));
-
- if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
- return i;
-
- // 鍏ㄨ溅鍦ㄥ満鍦板唴
- if (moveDirect == 0) {
- Polygon carBody;
- bool ret = false;
- 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[i].map) == GM_Containment) {
- ret = true;
- }
-
- free(carBody.point);
-
- if (ret)
- return i;
- }
-// // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴
-// 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)
-// score[i]++;
-// }
-// // 杞﹀熬椤剁偣鍦ㄥ満鍦板唴
-// if (IntersectionOf(car->carXY[ car->axial[AXIAL_REAR] ], &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_REAR]], enterLine) > 0.1)
-// score[i]++;
-// }
- }
- if (mapList[i].type == MAP_TYPE_TURN_90) {
- // 杞﹀墠杞垨鍚庤疆杞ㄨ抗瓒婅繃瑙﹀彂绾�
- Line triggerLine;
-
- MakeLine(&triggerLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
-
- if (moveDirect > 0 && 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 (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
- return i;
- }
- }
-
- for (int i = 0; i < score.size(); ++i) {
- if (score[i] == 2) {
- return i;
- }
- }
-
- for (int i = 0; i < score.size(); ++i) {
- if (score[i] == 1) {
- 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 x10, y10, 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;
-
- if (mapList[index].map.num > 9) {
- // 鏋勯�犺櫄鎷熺殑鍙充笂瑙掔偣
- xo = (mapList[index].map.point[9].X + mapList[index].map.point[7].X) / 2;
- yo = (mapList[index].map.point[9].Y + mapList[index].map.point[7].Y) / 2;
- x10 = 2*xo - mapList[index].map.point[8].X;
- y10 = 2*yo - mapList[index].map.point[8].Y;
- }
-
- Polygon map;
-
- map.num = 4;
- map.point = (PointF *) malloc(map.num * sizeof(PointF));
-
- if (mapList[index].map.num <= 9) {
- 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;
- } else {
- map.point[0] = mapList[index].map.point[0];
- map.point[1] = mapList[index].map.point[9];
- map.point[2].X = x10;
- map.point[2].Y = y10;
- 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] = centerOfTwoPoint(c1->carXY[c1->left_front_tire[TIRE_OUTSIDE]], c1->carXY[c1->right_front_tire[TIRE_OUTSIDE]]);
- trace2.point[pn++] = centerOfTwoPoint(c1->carXY[c1->left_rear_tire[TIRE_OUTSIDE]], c1->carXY[c1->right_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] = centerOfTwoPoint(c2->carXY[c2->left_front_tire[TIRE_OUTSIDE]], c2->carXY[c2->right_front_tire[TIRE_OUTSIDE]]);
- trace2.point[pn++] = centerOfTwoPoint(c2->carXY[c2->left_rear_tire[TIRE_OUTSIDE]], c2->carXY[c2->right_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 &&
- IntersectionOfLine(p1, p2, centerOfTwoPoint(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], car->carXY[car->right_front_tire[TIRE_OUTSIDE]])) == -1 &&
- DistanceOf(centerOfTwoPoint(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]), triggerLine) > 0.1) ||
- (IntersectionOf(trace2_line, triggerLine) == GM_Intersection &&
- IntersectionOfLine(p1, p2, centerOfTwoPoint(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], car->carXY[car->right_rear_tire[TIRE_OUTSIDE]])) == -1 &&
- DistanceOf(centerOfTwoPoint(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], car->carXY[car->right_rear_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