From 0820c9f0ffa19ba234bcc6858961c9e824f0ffa9 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期四, 16 四月 2020 18:10:26 +0800
Subject: [PATCH] 科目三
---
lib/src/main/cpp/test_items2/road_exam.cpp | 209 ++++++++++++++++++++++++++++++++++++++++++++++++++++
1 files changed, 209 insertions(+), 0 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index 1a86093..ee1456b 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -33,6 +33,8 @@
START_CAR_DONE
};
+static const int INVALID_ROAD = -1;
+
static const int TURN_THRESHOLD = 1;
static const int TURN_CHECK_INTERVAL = 500;
const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
@@ -981,5 +983,212 @@
}
return -1;
}
+#if 0
+void TestRoadGeneral(struct road_exam2_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
+{
+}
+/**************************************************
+ * 杞﹁締褰撳墠鎵�鍦ㄨ矾娈碉紝杞﹀ご闇�瓒婅繃璧峰绾夸竴瀹氳窛绂�
+ * @param currIndex
+ * @param map
+ * @param car
+ */
+static void EnterRoad(int &currIndex, struct road_exam2_map &map, const car_model *car)
+{
+ 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]];
+ }
+
+ Polygon roadArea;
+ roadArea.num = 0;
+ roadArea.point = NULL;
+
+ for (int i = 0; i < map.roads.size(); ++i) {
+ if (roadArea.point != NULL) {
+ free(roadArea.point);
+ }
+
+ for (int x = 0; x < map.roads[i].leftEdge.size(); ++x) {
+ roadArea.num += map.roads[i].leftEdge[x].points.size();
+ }
+ for (int x = 0; x < map.roads[i].rightEdge.size(); ++x) {
+ roadArea.num += map.roads[i].rightEdge[x].points.size();
+ }
+ roadArea.point = (PointF *) malloc(roadArea.num * sizeof(PointF));
+
+ int n = 0;
+ for (int j = 0; j < map.roads[i].leftEdge.size(); j++) {
+ for (int x = 0; x < map.roads[i].leftEdge[j].points.size(); ++x) {
+ if (n > 0 && x == 0 && isEqual2(roadArea.point[n-1].X, map.roads[i].leftEdge[j].points[x].X) &&
+ isEqual2(roadArea.point[n-1].Y, map.roads[i].leftEdge[j].points[x].Y)) {
+ // 绗竴涓偣宸茬粡瀛樺湪浜庝笂涓�鏉＄嚎
+ } else {
+ roadArea.point[n++] = map.roads[i].leftEdge[j].points[x];
+ }
+ }
+ }
+ for (int j = map.roads[i].rightEdge.size(); j > 0; j--) {
+ for (int x = map.roads[i].rightEdge[j].points.size(); x > 0; --x) {
+ if (n > 0 && x == map.roads[i].rightEdge[j].points.size() &&
+ isEqual2(roadArea.point[n-1].X, map.roads[i].rightEdge[j - 1].points[x-1].X) &&
+ isEqual2(roadArea.point[n-1].Y, map.roads[i].rightEdge[j - 1].points[x-1].Y)) {
+ // 绗竴涓偣宸茬粡瀛樺湪浜庝笂涓�鏉＄嚎
+ } else {
+ roadArea.point[n++] = map.roads[i].rightEdge[j - 1].points[x - 1];
+ }
+ }
+ }
+ roadArea.num = n;
+
+ if (IntersectionOf(car->carXY[car->axial[AXIAL_FRONT]], &roadArea) == GM_Containment) {
+ currIndex = i;
+ goto CHECK_CAR_ON_ROAD_END;
+ }
+ }
+ currIndex = INVALID_ROAD;
+
+ CHECK_CAR_ON_ROAD_END:
+ if (roadArea.point != NULL) {
+ free(roadArea.point);
+ }
+ free(carBody.point);
+}
+
+/******************************************************
+ * 鍏ㄨ溅閮介渶绂诲紑杩欎釜鍖哄煙
+ * @param currIndex
+ * @param map
+ * @param car
+ * @return
+ */
+static bool ExitRoad(int currIndex, struct road_exam2_map &map, const car_model *car)
+{
+ // 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
+ bool ret = false;
+
+ if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
+ return ret;
+
+ Polygon roadArea;
+
+ roadArea.num = map.roads[currIndex].leftEdge.size() + map.roads[currIndex].rightEdge.size();;
+ roadArea.point = (PointF *) malloc(roadArea.num * sizeof(PointF));;
+
+ int n = 0;
+ for (int j = 0; j < map.roads[currIndex].leftEdge.size(); j++) {
+ roadArea.point[n++] = map.roads[i].leftEdge[j];
+ }
+ for (int j = map.roads[currIndex].rightEdge.size(); j > 0; j--) {
+ roadArea.point[n++] = map.roads[i].rightEdge[j - 1];
+ }
+
+ 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, &roadArea) == GM_None) {
+ ret = true;
+ }
+
+ free(carBody.point);
+ free(roadArea.point);
+ return ret;
+}
+
+static bool CrashSeparateLine(int currIndex, struct road_exam2_map &map, const car_model *car)
+{
+ Line frontAxle, rearAxle;
+
+ if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
+ return false;
+
+ MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
+ MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
+
+ // 鍒嗘
+ for (int i = 0; i < map.roads[currIndex].separate.size(); i++) {
+ // 鍒嗘涓殑姣忔潯绾�
+ for (int j = 0; j < map.roads[currIndex].separate[i].line.size(); j++) {
+ Line theLine;
+ int p1 = 0;
+ for (int p2 = 1; p2 < map.roads[currIndex].separate[i].line[j].num; ++p2) {
+ MakeLine(&theLine, &map.roads[currIndex].separate[i].line[j].point[p1], &map.roads[currIndex].separate[i].line[j].point[p2]);
+ if (IntersectionOf(theLine, frontAxle) == GM_Intersection ||
+ IntersectionOf(theLine, rearAxle) == GM_Intersection) {
+ return true;
+ }
+ p1 = p2;
+ }
+ }
+ }
+ return false;
+}
+
+struct {
+ int road;
+ int segment;
+ int track;
+} CarOnTrackInfo;
+
+static void DetectSeparate(int currIndex, struct road_exam2_map &map, const car_model *car)
+{
+ int segment;
+ int track = -1;
+
+ if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
+ return;
+
+ for (int i = 0; i < map.roads[currIndex].separate.size(); i++) {
+ if (map.roads[currIndex].separate[i].character == 0) {
+ int separate_line_num = map.roads[currIndex].separate[i].line.size();
+
+ // 閬嶅巻褰撳墠鍒嗘鐨勬瘡涓�鏉＄嚎
+ for (int j = 0; j < separate_line_num; ++j) {
+ Line theLine;
+ int p1 = 0;
+ bool match_line = false;
+ // 鍗曠嫭鐨勪竴鏉¤櫄绾�
+ for (int p2 = 1; p2 < map.roads[currIndex].separate[i].line[j].num; ++p2) {
+ MakeLine(&theLine, &map.roads[currIndex].separate[i].line[j].point[p1], &map.roads[currIndex].separate[i].line[j].point[p2]);
+ if (p1 == 0 || p2 == map.roads[currIndex].separate[i].line[j].num - 1) {
+ // 棣栧熬涓ょ锛屽簲閲囩敤寤堕暱绾跨殑鏂瑰紡
+ }
+ if (VerticalPointOnLine(car->basePoint, theLine)) {
+ match_line = true;
+ int rel = IntersectionOfLine(map.roads[currIndex].separate[i].line[j].point[p1],
+ map.roads[currIndex].separate[i].line[j].point[p2],
+ car->basePoint);
+ // 璁板綍鎵�鍦ㄩ亾璺紝鍒嗛亾娈碉紝鍒嗛亾鎯呭喌
+ if (rel == 1) { // 鍦ㄥ乏渚�
+ segment = i;
+ track = j;
+ goto DETECT_SEPARATE_END;
+ } else if (rel != -1) { // 鍦ㄧ嚎涓�
+ segment = i;
+ track = j;
+ goto DETECT_SEPARATE_END;
+ }
+ break;
+ }
+ p1 = p2;
+ }
+
+ track = separate_line_num;
+ }
+ }
+ }
+
+ DETECT_SEPARATE_END:
+ ;
+}
+#endif
--
Gitblit v1.8.0