From dc2a01d4764efd33a23afcaf4f1d7543dc35c4fa Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期三, 19 二月 2020 15:32:57 +0800
Subject: [PATCH] 上坡地图修正
---
lib/src/main/cpp/test_items/park_bottom.cpp | 471 ++++++++++++++++++++++++----------------------------------
1 files changed, 195 insertions(+), 276 deletions(-)
diff --git a/lib/src/main/cpp/test_items/park_bottom.cpp b/lib/src/main/cpp/test_items/park_bottom.cpp
index 2b87406..79b0678 100644
--- a/lib/src/main/cpp/test_items/park_bottom.cpp
+++ b/lib/src/main/cpp/test_items/park_bottom.cpp
@@ -8,6 +8,7 @@
#include "../native-lib.h"
#include "../jni_log.h"
#include "../driver_test.h"
+#include "../utils/xconvert.h"
#include <vector>
#include <cstdlib>
@@ -24,317 +25,211 @@
THIRD_TOUCH_CTRL_LINE
};
-const int PARK_TIMEOUT = 210;
+const int PARK_TIMEOUT = D_SEC(210);
-static bool PBTesting = false;
-static bool trigLeaveTestAreaDetect = false;
-static bool leaveTestArea = false;
-static bool stopCar2S = false;
+static bool testing = false;
-static int currTarget;
-static bool leftTireCrossLeftLine, leftTireCrossRightLine, rightTireCrossLeftLine, rightTireCrossRightLine;
-static char first_ctrl_line_id;
-static bool carStopEvent; // 涓�斿仠杞︽爣璁�
-static bool carParkSuccess; // 鏄惁鍋滃湪搴撲綅
-static bool parkTimeout;
+const uint32_t STOP_CAR_TIME = D_SEC(2);
+static uint32_t stopTimepoint;
+static int prevMoveDirect, storeMoveDirectBeforeStop;
+static bool occurCrashRedLine;
+static bool checkPartStatus;
+static uint32_t firstReverseTimepoint;
+static bool reportExamTimeout, reportParkFail;
+static bool crossCtrlLineSw;
+static int parkCount;
+static char carray[3];
+static int darray[3];
+static int parkStatus[3];
-static void StopCarTimeout(union sigval sig);
-static void LeaveTestAreaLongtime(union sigval sig);
-static void CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car);
+static char CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car);
static bool EnterParking(const Polygon *map, const car_model *car);
-static void ParkBottomTimeout(union sigval sig);
static bool CrashRedLine(const Polygon *map, const car_model *car);
+static bool ExitParkArea(const Polygon *map, const car_model *car);
-void StartParkBottom(void)
+void StartParkBottom(int moveDirect, const struct RtkTime *rtkTime)
{
- stopCar2S = false;
- trigLeaveTestAreaDetect = false;
- leaveTestArea = false;
- PBTesting = true;
- parkTimeout = false;
- first_ctrl_line_id = 0;
- currTarget = FIRST_TOUCH_CTRL_LINE;
- leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;
-
DEBUG("StartParkBottom");
+ testing = true;
+ memset(carray, 0, sizeof(carray));
+ memset(darray, 0, sizeof(darray));
+ memset(parkStatus, 0, sizeof(parkStatus));
+ prevMoveDirect = moveDirect;
+
+ checkPartStatus = false;
+ firstReverseTimepoint = 0;
+ reportExamTimeout = false;
+ parkCount = 0;
+ crossCtrlLineSw = false;
+ reportParkFail = false;
+ occurCrashRedLine = false;
}
-void StopParkBottom(void)
+int TestParkBottom(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
- PBTesting = false;
- AppTimer_delete(StopCarTimeout);
- AppTimer_delete(ParkBottomTimeout);
- AppTimer_delete(LeaveTestAreaLongtime);
- currTarget = NONE;
+ char crossCtrlLine = 0;
+ uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- DEBUG("ParkBottom End");
-}
-
-int TestParkBottom(vector<int>&err, const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int run_status, const struct RtkTime *rtkTime)
-{
- int status = 0;
-
- if (!PBTesting)
- return -2;
-
- DEBUG("TestParkBottom speed %f dir %d", speed, run_status);
-
- if (currTarget > FIRST_TOUCH_CTRL_LINE) {
- // 鏄惁瓒呮椂
- if (parkTimeout) {
- // 涓嶅悎鏍硷細鍔ㄤ綔瓒呮椂
- DEBUG("涓嶅悎鏍硷細鍔ㄤ綔瓒呮椂");
- err.push_back(10);
- AddExamFault(10, rtkTime);
- status = -1;
+ if (ExitParkArea(map, car)) {
+ DEBUG("绂诲紑鍦哄湴");
+ // 绂诲紑鍦哄湴
+ testing = false;
+ if ((parkStatus[0] != 1 || parkStatus[1] != 1) && !reportParkFail) {
+ // 鍊掑簱涓嶅叆锛屼笉鍚堟牸
+ reportParkFail = true;
+ AddExamFault(8, rtkTime);
+ DEBUG("鍊掑簱涓嶅叆");
}
- // 鏄惁鍘嬬嚎
- if (CrashRedLine(map, car)) {
- // 涓嶅悎鏍硷細杞﹁韩鍑虹嚎
- DEBUG("涓嶅悎鏍硷細杞﹁韩鍑虹嚎");
- err.push_back(7);
+ goto TEST_END;
+ }
+
+ if (CrashRedLine(map, car)) {
+ if (!occurCrashRedLine) {
+ occurCrashRedLine = true;
+ // 杞﹁韩鍑虹嚎锛屼笉鍚堟牸
AddExamFault(7, rtkTime);
- status = -1;
+ DEBUG("杞﹁疆鍘嬬嚎");
}
-
- if (trigLeaveTestAreaDetect) {
- if (IntersectionOf(car->carXY[ car->left_front_tire[TIRE_OUTSIDE] ], map) == GM_Containment &&
- IntersectionOf(car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ], map) == GM_Containment) {
- trigLeaveTestAreaDetect = false;
- AppTimer_delete(LeaveTestAreaLongtime);
- }
- }
+ } else {
+ occurCrashRedLine = false;
}
- if (currTarget == FIRST_TOUCH_CTRL_LINE ||
- currTarget == SECOND_TOUCH_CTRL_LINE ||
- currTarget == THIRD_TOUCH_CTRL_LINE) {
- if (run_status > 0) {
- if (stopCar2S && currTarget != FIRST_TOUCH_CTRL_LINE) {
- // 鎵�5鍒嗭細涓�斿仠杞﹁秴杩�2绉�
- DEBUG("鎵�5鍒嗭細涓�斿仠杞﹁秴杩�2绉�");
- err.push_back(11);
- AddExamFault(11, rtkTime);
- }
-
- if (!((leftTireCrossLeftLine && rightTireCrossLeftLine) ||
- (leftTireCrossRightLine && rightTireCrossRightLine))) {
- DEBUG("CrossCtrlLine");
- CrossCtrlLine(map, car, carPrev);
- } else if (currTarget == FIRST_TOUCH_CTRL_LINE || currTarget == SECOND_TOUCH_CTRL_LINE) {
- // 璺ㄨ繃鎺у埗绾垮悗锛岃溅杈嗘寔缁悜鍓嶈椹讹紝澶勭悊杩欎簺涔辨悶鎯呭喌
- // 鏁翠釜杞﹂兘绂诲紑娴嬭瘯鍖哄悗锛屽鏋滄寔缁�15绉掞紝杩樻病鍥炲埌娴嬭瘯鍖猴紝灏卞拷鐣ヨ娴嬭瘯鎴栬�呮窐姹�
- if (leaveTestArea) {
- if (currTarget == FIRST_TOUCH_CTRL_LINE) {
- status = -2;
- DEBUG("鏉ラ亾鍦虹帺鐨�");
- } else {
- // 涓嶅悎鏍硷細鏈寜瑙勫畾绾胯矾琛岄┒锛堢洿鎺ヨ窇鍑烘祴璇曞尯浜嗭級
- DEBUG("涓嶅悎鏍硷細鏈寜瑙勫畾绾胯矾琛岄┒锛堢洿鎺ヨ窇鍑烘祴璇曞尯浜嗭級");
- err.push_back(6);
- AddExamFault(6, rtkTime);
- status = -1;
- DEBUG("鐩存帴璺戝嚭娴嬭瘯鍖轰簡");
- }
- } else if (!trigLeaveTestAreaDetect) {
- 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]];
- }
-
- if (IntersectionOf(map, &car_body) == GM_None) {
- trigLeaveTestAreaDetect = true;
- AppTimer_delete(LeaveTestAreaLongtime);
- AppTimer_add(LeaveTestAreaLongtime, D_SEC(15));
- DEBUG("寮�濮嬬鍦鸿鏃�");
- }
-
- free(car_body.point);
- }
- }
-
- if (currTarget == THIRD_TOUCH_CTRL_LINE) {
- char the_ctrl_line_crossed = 0;
-
- if (leftTireCrossLeftLine && rightTireCrossLeftLine) {
- the_ctrl_line_crossed = 'L';
- } else if (leftTireCrossRightLine && rightTireCrossRightLine) {
- the_ctrl_line_crossed = 'R';
- }
-
- if (the_ctrl_line_crossed != 0 && the_ctrl_line_crossed == first_ctrl_line_id) {
- // 椤圭洰瀹屾垚
- status = 1;
- } else if (the_ctrl_line_crossed != 0) {
- // 涓嶅悎鏍硷細鏈寜瑙勫畾绾胯矾琛岄┒锛堟湭鍥炲埌璧峰鐐癸級
- DEBUG("涓嶅悎鏍硷細鏈寜瑙勫畾绾胯矾琛岄┒锛堟湭鍥炲埌璧峰鐐癸級");
- err.push_back(6);
- AddExamFault(6, rtkTime);
- status = -1;
- }
- }
- if (carStopEvent)
- AppTimer_delete(StopCarTimeout);
- carStopEvent = false;
- stopCar2S = false;
- } else if (run_status < 0) {
- // 宸﹀彸鍊掑簱澶х翰骞舵湭瑕佹眰璋佸厛瀹屾垚锛屾晠浠ュ厛瓒婅繃鐨勬帶鍒剁嚎涓哄噯锛屼笅娆″緱瓒婅繃鍙﹀涓�鏉�
- char the_ctrl_line_crossed = 0;
-
- if (leftTireCrossLeftLine && rightTireCrossLeftLine) {
- the_ctrl_line_crossed = 'L';
- } else if (leftTireCrossRightLine && rightTireCrossRightLine) {
- the_ctrl_line_crossed = 'R';
- }
-
- if (first_ctrl_line_id > 0 && first_ctrl_line_id == the_ctrl_line_crossed) {
- // 涓嶅悎鏍硷細鏈寜瑙勫畾绾胯矾琛岄┒锛堣瘯鍥惧仛2娆″悓鏂瑰悜鐨勫�掑簱锛�
- DEBUG("涓嶅悎鏍硷細鏈寜瑙勫畾绾胯矾琛岄┒锛堣瘯鍥惧仛2娆″悓鏂瑰悜鐨勫�掑簱锛�");
- err.push_back(6);
+ crossCtrlLine = CrossCtrlLine(map, car, carPrev);
+ if (crossCtrlLine > 0 && !crossCtrlLineSw) {
+ crossCtrlLineSw = true;
+ if (parkCount == 0) {
+ carray[0] = crossCtrlLine;
+ } else if (parkCount == 1) {
+ if (carray[0] == crossCtrlLine) {
+ // 涓嶆寜瑙勫畾绾胯矾锛岄『搴忓舰寮忥紝涓嶅悎鏍�
AddExamFault(6, rtkTime);
- status = -1;
- } else if (the_ctrl_line_crossed > 0 && first_ctrl_line_id == 0) {
- first_ctrl_line_id = the_ctrl_line_crossed;
- // 椤圭洰姝ｅ紡寮�濮嬶紝210绉掑唴瀹屾垚
- AppTimer_delete(ParkBottomTimeout);
- AppTimer_add(ParkBottomTimeout, D_SEC(PARK_TIMEOUT));
- currTarget = FIRST_PARK;
- carParkSuccess = false;
- parkTimeout = false;
- leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;
-
- DEBUG("绗竴娆″�掑簱");
- } else if (the_ctrl_line_crossed > 0) {
- currTarget = SECOND_PARK;
- carParkSuccess = false;
- leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;
- DEBUG("绗簩娆″�掑簱");
- } else if (currTarget != THIRD_TOUCH_CTRL_LINE) {
- // 涓嶅悎鏍硷細鍊掕溅鍓嶏紝2鍓嶈疆娌￠┒杩囨帶鍒剁嚎
- DEBUG("涓嶅悎鏍硷細鍊掕溅鍓嶏紝2鍓嶈疆娌￠┒杩囨帶鍒剁嚎");
- err.push_back(9);
- AddExamFault(9, rtkTime);
- status = -1;
+ DEBUG("涓嶆寜瑙勫畾绾胯矾锛岄『搴忓舰寮�");
}
- if (carStopEvent)
- AppTimer_delete(StopCarTimeout);
- carStopEvent = false;
- stopCar2S = false;
- } else {
- if (!carStopEvent) {
- AppTimer_delete(StopCarTimeout);
- AppTimer_add(StopCarTimeout, D_SEC(2));
+ carray[1] = crossCtrlLine;
+ } else if (parkCount == 2) {
+ if (carray[0] != crossCtrlLine) {
+ // 涓嶆寜瑙勫畾绾胯矾锛岄『搴忓舰寮忥紝涓嶅悎鏍�
+ AddExamFault(6, rtkTime);
+ DEBUG("涓嶆寜瑙勫畾绾胯矾锛岄『搴忓舰寮�");
+ } else {
+ // 绂诲紑娴嬭瘯鍖猴紝鍋滄璁℃椂
+ DEBUG("绂诲紑娴嬭瘯鍖猴紝鍋滄璁℃椂");
+ testing = false;
+ goto TEST_END;
}
- carStopEvent = true;
+ carray[2] = crossCtrlLine;
}
- } else if (currTarget == FIRST_PARK || currTarget == SECOND_PARK) {
- if (run_status < 0) {
- if (stopCar2S) {
- // 鎵�5鍒嗭細涓�斿仠杞�
- DEBUG("鎵�5鍒嗭細涓�斿仠杞�");
- err.push_back(11);
- AddExamFault(11, rtkTime);
+ }
+
+ if (testing && darray[0] > 0 && tp - firstReverseTimepoint >= PARK_TIMEOUT) {
+ // 瀹屾垚瓒呮椂锛屼笉鍚堟牸
+ if (!reportExamTimeout) {
+ reportExamTimeout = true;
+ AddExamFault(10, rtkTime);
+ DEBUG("椤圭洰瓒呮椂");
+ }
+ }
+
+ if (moveDirect != prevMoveDirect) {
+ if (moveDirect == 0) {
+ stopTimepoint = tp;
+ storeMoveDirectBeforeStop = prevMoveDirect;
+ if (prevMoveDirect == -1) {
+ checkPartStatus = true;
}
- carStopEvent = false;
- stopCar2S = false;
- } else if (run_status == 0) {
- // 绔嬪嵆妫�鏌ユ槸鍚﹀仠杞﹀埌浣嶏紝涔熻鏄腑閫斿仠杞︼紝鍏堜笉绠★紝寰呭彂鐢熷墠杩涗簨浠跺悗锛屽啀鏂畾鏄惁鍋滆溅鍒颁綅
- if (!carStopEvent) {
- carStopEvent = true;
- carParkSuccess = EnterParking(map, car);
- AppTimer_delete(StopCarTimeout);
- AppTimer_add(StopCarTimeout, D_SEC(2));
- }
+ DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
} else {
- if (carStopEvent) {
- if (!carParkSuccess) {
- // 涓嶅悎鏍硷細鍊掑簱涓嶅叆
- DEBUG("涓嶅悎鏍硷細鍊掑簱涓嶅叆");
- err.push_back(8);
- AddExamFault(8, rtkTime);
- status = -1;
- } else if (currTarget == FIRST_PARK) {
- currTarget = SECOND_TOUCH_CTRL_LINE;
- DEBUG("杩囧彟涓�鏍规帶鍒剁嚎");
- } else {
- currTarget = THIRD_TOUCH_CTRL_LINE;
- DEBUG("鍐嶈繃绗竴鏍规帶鍒剁嚎");
+ DEBUG("缁х画琛岄┒ %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
+
+ DEBUG("鍋滆溅鏃堕棿 %ld", tp - stopTimepoint);
+
+ if (moveDirect == storeMoveDirectBeforeStop) {
+ // 鍚屾柟鍚戝啀鍚姩锛岀户缁垽鏂槸鍚﹀仠杞﹁秴鏃�
+ if (tp - stopTimepoint >= STOP_CAR_TIME) {
+ // 鍋滆溅瓒�2绉掞紝姣忔鎵�5鍒�
+ AddExamFault(11, rtkTime);
+ DEBUG("涓�斿仠杞�");
+ }
+ } else if (moveDirect == -1) {
+ // 鍒囨崲涓哄�掕溅
+ if (darray[parkCount] == 0) {
+ if (!crossCtrlLineSw) {
+ // 鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎
+ AddExamFault(9, rtkTime);
+ DEBUG("鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎");
+ }
+ darray[parkCount] = 1;
+ firstReverseTimepoint = tp;
+ }
+ } else {
+ // 鍒囨崲涓哄墠杩�
+ if (tp - stopTimepoint >= STOP_CAR_TIME) {
+ if (crossCtrlLineSw) {
+ if (checkPartStatus) {
+ if (EnterParking(map, car)) {
+ parkStatus[parkCount] = 1;
+ }
+ checkPartStatus = false;
+ }
+ if (parkStatus[parkCount] != 1) {
+ // 鍊掑簱涓嶅叆锛屼笉鍚堟牸
+ reportParkFail = true;
+ AddExamFault(8, rtkTime);
+ DEBUG("鍊掑簱涓嶅叆");
+ }
+ }
+
+ crossCtrlLineSw = false;
+
+ if (parkCount < 2)
+ parkCount++;
}
}
- carStopEvent = false;
- stopCar2S = false;
+ }
+ prevMoveDirect = moveDirect;
+ } else if (moveDirect == -1) {
+ if (darray[parkCount] == 0) {
+ // 鍒囨崲涓哄�掕溅
+ if (!crossCtrlLineSw) {
+ // 鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎
+ AddExamFault(9, rtkTime);
+ DEBUG("鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎");
+ }
+ darray[parkCount] = 1;
+ firstReverseTimepoint = tp;
+ }
+ } else if (moveDirect == 0 && crossCtrlLineSw) {
+ if (tp - stopTimepoint >= STOP_CAR_TIME && checkPartStatus) {
+ if (EnterParking(map, car)) {
+ parkStatus[parkCount] = 1;
+ }
+ checkPartStatus = false;
}
}
- if (status != 0) {
- StopParkBottom();
- }
-
- return status;
-}
-
-static void StopCarTimeout(union sigval sig) {
- AppTimer_delete(StopCarTimeout);
-
- stopCar2S = true;
-}
-
-static void LeaveTestAreaLongtime(union sigval sig) {
- AppTimer_delete(LeaveTestAreaLongtime);
- leaveTestArea = true;
- trigLeaveTestAreaDetect = false;
-}
-
-static void ParkBottomTimeout(union sigval sig) {
- AppTimer_delete(ParkBottomTimeout);
- parkTimeout = true;
+TEST_END:
+ return testing ? 0 : 1;
}
// 妫�娴�2鍓嶈疆鏄惁姝ｅ悜瓒婅繃宸﹀彸鎺у埗绾�
-static void CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car)
+static char CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car)
{
- Line leftCtrlLine, rightCtrlLine;
- Line track1;
- PointF p1, p2;
+ // 杩囧彸鎺у埗绾�
+ if ((IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1) &&
+ (IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]) == -1) &&
+ (IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->axial[AXIAL_REAR]]) == 1)) {
+ return 'R';
+ }
- MakeLine(&leftCtrlLine, &(map->point[0]), &(map->point[1]));
- MakeLine(&rightCtrlLine, &(map->point[6]), &map->point[7]);
- // 宸﹀墠杞紝鍙栬疆瀹界殑涓偣
- p1.X = (car->carXY[car->left_front_tire[TIRE_OUTSIDE]].X + car->carXY[car->left_front_tire[TIRE_INSIDE]].X) / 2;
- p1.Y = (car->carXY[car->left_front_tire[TIRE_OUTSIDE]].Y + car->carXY[car->left_front_tire[TIRE_INSIDE]].Y) / 2;
- p2.X = (prev_car->carXY[car->left_front_tire[TIRE_OUTSIDE]].X + prev_car->carXY[car->left_front_tire[TIRE_INSIDE]].X) / 2;
- p2.Y = (prev_car->carXY[car->left_front_tire[TIRE_OUTSIDE]].Y + prev_car->carXY[car->left_front_tire[TIRE_INSIDE]].Y) / 2;
- MakeLine(&track1, &p1, &p2);
- if (IntersectionOf(track1, leftCtrlLine) == GM_Intersection &&
- IntersectionOfLine(map->point[0], map->point[1], p1) == -1) {
- leftTireCrossLeftLine = true;
- DEBUG("宸﹁疆璺ㄨ秺宸︽帶鍒剁嚎");
+ // 杩囧乏鎺у埗绾�
+ if ((IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == 1) &&
+ (IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]) == 1) &&
+ (IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->axial[AXIAL_REAR]]) == -1)) {
+ return 'L';
}
- if (IntersectionOf(track1, rightCtrlLine) == GM_Intersection &&
- IntersectionOfLine(map->point[6], map->point[7], p1) == -1) {
- leftTireCrossRightLine = true;
- DEBUG("宸﹁疆璺ㄨ秺鍙虫帶鍒剁嚎");
- }
- // 鍙冲墠杞�
- p1.X = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].X + car->carXY[car->right_front_tire[TIRE_INSIDE]].X) / 2;
- p1.Y = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].Y + car->carXY[car->right_front_tire[TIRE_INSIDE]].Y) / 2;
- p2.X = (prev_car->carXY[car->right_front_tire[TIRE_OUTSIDE]].X + prev_car->carXY[car->right_front_tire[TIRE_INSIDE]].X) / 2;
- p2.Y = (prev_car->carXY[car->right_front_tire[TIRE_OUTSIDE]].Y + prev_car->carXY[car->right_front_tire[TIRE_INSIDE]].Y) / 2;
- MakeLine(&track1, &p1, &p2);
- if (IntersectionOf(track1, leftCtrlLine) == GM_Intersection &&
- IntersectionOfLine(map->point[0], map->point[1], p1) == -1) {
- rightTireCrossLeftLine = true;
- DEBUG("鍙宠疆璺ㄨ秺宸︽帶鍒剁嚎");
- }
- if (IntersectionOf(track1, rightCtrlLine) == GM_Intersection &&
- IntersectionOfLine(map->point[6], map->point[7], p1) == -1) {
- rightTireCrossRightLine = true;
- DEBUG("鍙宠疆璺ㄨ秺鍙虫帶鍒剁嚎");
- }
+
+ return 0;
}
static bool EnterParking(const Polygon *map, const car_model *car) {
@@ -357,6 +252,8 @@
CleanPolygon(&parking);
free(car_body.point);
+
+ DEBUG("妫�鏌ュ�掑簱鐘舵�� %s", succ ? "鎴愬姛" : "澶辫触");
return succ;
}
@@ -387,3 +284,25 @@
free(car_body.point);
return ret;
}
+
+static bool ExitParkArea(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