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/park_edge.cpp | 128 ++++++++++++++++++------------------------
1 files changed, 55 insertions(+), 73 deletions(-)
diff --git a/lib/src/main/cpp/test_items/park_edge.cpp b/lib/src/main/cpp/test_items/park_edge.cpp
index 4a4160c..9ac9060 100644
--- a/lib/src/main/cpp/test_items/park_edge.cpp
+++ b/lib/src/main/cpp/test_items/park_edge.cpp
@@ -1,6 +1,17 @@
//
// Created by YY on 2019/10/23.
//
+//
+// 4 ___________________________ 3
+// | |
+// | |
+// | |
+// ________________| |___________1
+// 6 5 2
+//
+//
+// 7______________________________________________________0
+//
#include "park_edge.h"
#include "../jni_log.h"
@@ -29,7 +40,6 @@
const uint32_t CHECK_PARK_DELAY = 400;
-static int mapIndex = 0;
static bool reportExamTimeout;
static bool reportParkFail;
@@ -45,24 +55,23 @@
static int currGear;
static double odo;
-static bool CrashRedLine1(const Polygon *map, const car_model *car);
-static bool CrashRedLine2(const Polygon *map, const car_model *car);
-static bool CrashRedLine3(const Polygon *map, const car_model *car);
-static bool EnterParking(const Polygon *map, const car_model *car);
-static bool ExitParkArea(const Polygon *map, const car_model *car);
-static bool ExitParkArea2(const Polygon *map, const car_model *car);
+static bool CrashRedLine1(const Polygon *map, const car_model_t *car);
+static bool CrashRedLine2(const Polygon *map, const car_model_t *car);
+static bool CrashRedLine3(const Polygon *map, const car_model_t *car);
+static bool EnterParking(const Polygon *map, const car_model_t *car);
+static bool ExitParkArea(const Polygon *map, const car_model_t *car);
+static bool ExitParkArea2(const Polygon *map, const car_model_t *car);
-void StartParkEdge(int index, int moveStatus, const struct RtkTime *rtkTime)
+void StartParkEdge(prime_t &prime)
{
DEBUG("杩涘叆渚ф柟鍋滆溅鍦哄湴");
testStatus = TESTING;
- mapIndex = index;
occurCrashRedLine1 = occurCrashRedLine2 = occurCrashRedLine3 = false; // 杩欎釜绉戠洰瑙勫畾鐗规畩鐐癸紝鍙戠敓涓�娆℃墸10鍒嗭紝鑰屼笉鐩存帴娣樻卑
reportExamTimeout = false;
reportParkFail = false;
- prevMoveStatus = moveStatus;
+ prevMoveStatus = prime.pMotion->move;
parkSuccess = false;
parkStatus = 0;
occurMoveBack = false;
@@ -82,7 +91,7 @@
}*/
}
-int TestParkEdge(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime)
+int TestParkEdge(prime_t &prime)
{
vector<double> dtox;
vector<Line> line_set;
@@ -128,7 +137,7 @@
occurMoveBack = true;
moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss,
rtkTime->mss * 10); // 寮�濮嬭鏃�
- MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 1);
+ MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 1);
}
} else {
if (occurMoveBack && !checkPark) {
@@ -307,7 +316,7 @@
DEBUG("寮�濮嬪�掕溅");
occurMoveBack = true;
moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 1);
+ MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 1);
}*/
}
@@ -328,14 +337,43 @@
TEST_END:
if (testStatus == TEST_FINISH) {
DEBUG("渚ф柟鍋滆溅缁撴潫");
- MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 0);
+ MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 0);
return 0;
}
return 1;
}
+int EnterParkEdge(prime_t &prime) {
+ if (prime.prev_modeling_index == -1 || prime.curr_modeling_index == -1) {
+ return -1;
+ }
+
+ 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);
+
+ // 涓嶅悓鍦板尯宸﹀彸鍊掑簱娆″簭鍙兘涓嶅悓锛屾墍浠ヤ袱涓柟鍚戦兘鍙互杩涘叆
+ for (int i = 0; i < prime.pMap->park_button_map.size(); ++i) {
+ Line left_entrance_line;
+
+ MAKE_LINE(left_entrance_line, prime.pMap->park_button_map[i].map[1], prime.pMap->park_button_map[i].map[0]);
+
+ if (IntersectionOf(left_trace, left_entrance_line) == GM_Intersection
+ && IntersectionOfLine(lp1, left_entrance_line) == RELATION_RIGHT) {
+ return i;
+ }
+ }
+ return -1;
+}
+
// 杞﹁疆鏄惁鍘嬮亾璺竟绾�
-static bool CrashRedLine1(const Polygon *map, const car_model *car)
+static bool CrashRedLine1(const Polygon *map, const car_model_t *car)
{
bool ret = false;
@@ -360,7 +398,7 @@
}
// 杞﹁韩鏄惁鍘嬪簱浣嶇嚎
-static bool CrashRedLine2(const Polygon *map, const car_model *car)
+static bool CrashRedLine2(const Polygon *map, const car_model_t *car)
{
bool ret = false;
@@ -387,7 +425,7 @@
return ret;
}
-static bool CrashRedLine3(const Polygon *map, const car_model *car) {
+static bool CrashRedLine3(const Polygon *map, const car_model_t *car) {
bool ret = false;
if (!occurMoveBack) {
@@ -408,62 +446,6 @@
free(car_body.point);
}
-
- return ret;
-}
-
-static bool EnterParking(const Polygon *map, const car_model *car) {
- DEBUG("妫�鏌ュ仠杞﹀埌浣�...");
-
- bool succ = false;
-
- Polygon parking;
- Polygon car_body;
-
- car_body.num = car->bodyNum;
- car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
- for (int i = 0; i < car_body.num; ++i) {
- car_body.point[i] = car->carXY[car->body[i]];
- }
-
- MakePolygon(&parking, {map->point[2], map->point[3], map->point[4], map->point[5]});
-
- if (IntersectionOf(&car_body, &parking) == GM_Containment) {
- succ = true;
- }
-
- CleanPolygon(&parking);
- free(car_body.point);
-
- return succ;
-}
-
-// 杞﹀ご瑕侀┒杩囧墠搴撲綅绾�
-static bool ExitParkArea(const Polygon *map, const car_model *car)
-{
- if (IntersectionOfLine(map->point[6], map->point[7], car->carXY[ car->axial[AXIAL_FRONT] ]) == -1)
- return true;
- return false;
-}
-
-static bool ExitParkArea2(const Polygon *map, const car_model *car)
-{
-// 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
- bool ret = false;
-
- 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);
return ret;
}
--
Gitblit v1.8.0