From 27fc91fbe8f88b6885356e68828cfe1ce1db7601 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期三, 28 二月 2024 09:53:13 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items/park_edge.cpp | 519 +++++++++++++++------------------------------------------
1 files changed, 135 insertions(+), 384 deletions(-)
diff --git a/lib/src/main/cpp/test_items/park_edge.cpp b/lib/src/main/cpp/test_items/park_edge.cpp
index 9ac9060..9c2a5c6 100644
--- a/lib/src/main/cpp/test_items/park_edge.cpp
+++ b/lib/src/main/cpp/test_items/park_edge.cpp
@@ -6,7 +6,7 @@
// | |
// | |
// | |
-// ________________| |___________1
+// ________________|9 8|___________1
// 6 5 2
//
//
@@ -27,425 +27,176 @@
#include <vector>
#include <cstdlib>
+#include <tuple>
#define DEBUG(fmt, args...) LOGD("<park_edge> <%s>: " fmt, __func__, ##args)
using namespace std;
-enum {
- TESTING,
- TEST_FAIL, // 鍥犺Е鍙戞煇浜涜鍒欙紝鍦ㄨ溅韬湭瀹屽叏绔嬪満鎯呭喌涓嬶紝鎻愬墠缁堟閮ㄥ垎娴嬭瘯
- TEST_FINISH
-};
+static bool overpass_parkingspace;
+static bool check_parking_space;
const uint32_t CHECK_PARK_DELAY = 400;
-static bool reportExamTimeout;
-static bool reportParkFail;
+static bool occurCrashRedLine;
+static bool firstMoveBack, checkLight;
-static uint32_t stopTimepoint = 0;
-static bool occurCrashRedLine1, occurCrashRedLine2, occurCrashRedLine3;
-static int prevMoveStatus, storeMoveStatusBeforeStop;
-static int parkStatus;
-static int gearAtStop;
-static bool occurMoveBack, checkPark, parkSuccess, checkLight;
-static uint32_t moveBackTimePoint;
-static int testStatus;
-static int exitAreaCfm;
-static int currGear;
-static double odo;
+static void ParkTimeout(apptimer_var_t val);
+static bool CrashRedLine(prime_t &prime);
+static bool CheckParkspace(prime_t &prime);
+static bool OverpassParkspace(prime_t &prime);
+static void MotionChange(move_status_t mv);
-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);
+enum {
+ PE_PREPARE_PARK,
+ PE_PARKING,
+ PE_EXIT
+};
void StartParkEdge(prime_t &prime)
{
DEBUG("杩涘叆渚ф柟鍋滆溅鍦哄湴");
- testStatus = TESTING;
-
- occurCrashRedLine1 = occurCrashRedLine2 = occurCrashRedLine3 = false; // 杩欎釜绉戠洰瑙勫畾鐗规畩鐐癸紝鍙戠敓涓�娆℃墸10鍒嗭紝鑰屼笉鐩存帴娣樻卑
- reportExamTimeout = false;
- reportParkFail = false;
- prevMoveStatus = prime.pMotion->move;
- parkSuccess = false;
- parkStatus = 0;
- occurMoveBack = false;
- checkPark = false;
+ occurCrashRedLine = false; // 杩欎釜绉戠洰瑙勫畾鐗规畩鐐癸紝鍙戠敓涓�娆℃墸10鍒嗭紝鑰屼笉鐩存帴娣樻卑
+ firstMoveBack = false;
checkLight = false;
- gearAtStop = -1;
- stopTimepoint = 0;
- odo = ReadOdo();
- currGear = ReadCarStatus(GEAR);
- exitAreaCfm = 0;
-
+ check_parking_space = false;
+ prime.examing_area.stage = PE_PREPARE_PARK;
PlayTTS("鎮ㄥ凡杩涘叆渚ф柟鍋滆溅鍖哄煙", NULL);
- // 浠呭綋鍙戠敓鍊掕溅锛屾墠鎰忓懗鐫�椤圭洰寮�濮�
- /*if (moveStatus == -1) {
- occurMoveBack = true;
- moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- }*/
}
-int TestParkEdge(prime_t &prime)
+void StopParkEdge(prime_t &prime)
{
- vector<double> dtox;
- vector<Line> line_set;
- Line distance_line;
+ if (prime.examing_area.type != MAP_TYPE_PARK_EDGE)
+ return;
+ DEBUG("绂诲紑渚ф柟鍋滆溅鍦哄湴");
- bool gear_change = false;
- int gear = ReadCarStatus(GEAR);
-
- if (gear == GEAR_R) {
- if (currGear != GEAR_R) {
- gear_change = true;
- currGear = GEAR_R;
- }
- } else if (currGear == GEAR_R) {
- gear_change = true;
- currGear = gear;
+ if (!check_parking_space) {
+ DEBUG("鐩存帴椹剁娴嬭瘯鍖�,涓嶆寜鑰冭瘯鍛樻寚浠ら┚椹�");
+ AddExamFault(10103);
}
+ prime.examing_area.type = MAP_TYPE_NONE;
- if (gear_change) {
- // 妫�鏌ヤ笂涓�娆℃尅浣嶇殑琛岄┒璺濈锛岃繃灏忓氨鏀惧純锛岄伩寮�瀛﹀憳鍘熷湴鎸傛尅閲嶈瘯
- double run_distance = ReadOdo() - odo;
-
- DEBUG("2娆℃尅浣嶈繍琛岃窛绂� %f", run_distance);
-
- if (run_distance < 1) {
- gear_change = false;
- DEBUG("2娆℃尅浣嶈繍琛岃窛绂昏繃灏忥紝蹇界暐");
- }
- odo = ReadOdo();
- }
-
- // 棣栨鎸傚�掓尅锛� 鎵嶆剰鍛崇潃椤圭洰寮�濮�
- if (testStatus == TESTING && gear_change) {
- if (currGear == GEAR_R) {
- if (!occurMoveBack) {
- DEBUG("棣栨渚ф柟鍋滆溅");
- } else {
- DEBUG("鍐嶆渚ф柟鍋滆溅");
- }
-
- {
- checkPark = false;
- occurMoveBack = true;
- moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss,
- rtkTime->mss * 10); // 寮�濮嬭鏃�
- MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 1);
- }
- } else {
- if (occurMoveBack && !checkPark) {
- // 妫�鏌ヨ溅韬叆搴撴儏鍐�
- DEBUG("妫�鏌ヨ溅韬叆搴撴儏鍐�");
- checkPark = true;
- checkLight = false;
- if (EnterParking(map, car)) {
- parkStatus = 1;
- parkSuccess = true;
- } else {
- // 鍋滄鍚庯紝杞﹁韩鍑虹嚎锛屼笉鍚堟牸
- AddExamFault(20401, rtkTime);
- parkSuccess = false;
- DEBUG("绉诲簱涓嶅叆");
- }
- }
- }
- }
-
- if (CrashRedLine1(map, car)) {
- if (!occurCrashRedLine1 /*&& occurMoveBack*/) {
- // 杞﹁疆鍘嬭竟绾匡紝姣忔鎵�10鍒�
- AddExamFault(20403, rtkTime);
- DEBUG("杞﹁疆鍘嬭竟绾�");
- occurCrashRedLine1 = true;
- }
- } else {
- occurCrashRedLine1 = false;
- }
-
- if (CrashRedLine2(map, car)) {
- if (!occurCrashRedLine2 /*&& occurMoveBack*/) {
- // 杞﹁韩鍘嬪簱浣嶇嚎锛屾瘡娆℃墸10鍒�
- AddExamFault(20404, rtkTime);
- DEBUG("杞﹁韩鍘嬪簱浣嶇嚎");
- occurCrashRedLine2 = true;
- }
- } else {
- occurCrashRedLine2 = false;
- }
-
- if (CrashRedLine3(map, car)) {
- if (!occurCrashRedLine3 && !occurMoveBack && moveStatus == 1) {
- // 杞﹁韩鍘嬪簱浣嶇嚎锛屾瘡娆℃墸10鍒�
- AddExamFault(20407, rtkTime);
- DEBUG("杞﹁韩鍘嬪簱浣嶇嚎");
- occurCrashRedLine3 = true;
- }
- } else {
- occurCrashRedLine3 = false;
- }
-
- if (ExitParkArea2(map, car)) {
- /*if (!parkSuccess && occurMoveBack && !reportParkFail) {
- // 鐩存帴椹剁娴嬭瘯鍖猴紝璁や负绉诲簱涓嶅叆
- AddExamFault(10103, rtkTime);
- reportParkFail = true;
- DEBUG("鐩存帴椹剁娴嬭瘯鍖�,涓嶆寜鑰冭瘯鍛樻寚浠ら┚椹�");
- }*/
- if (occurMoveBack && !checkPark) {
- // 鍊掕溅鐩存帴椹剁娴嬭瘯鍖�
- AddExamFault(10103, rtkTime);
- DEBUG("鐩存帴椹剁娴嬭瘯鍖�,涓嶆寜鑰冭瘯鍛樻寚浠ら┚椹�");
- }
-
- testStatus = TEST_FINISH;
- goto TEST_END;
- }
-
- // 璺濈妫�娴�
- MakeLine(&distance_line, &map->point[0], &map->point[7]);
- line_set.push_back(distance_line);
- MakeLine(&distance_line, &map->point[1], &map->point[2]);
- line_set.push_back(distance_line);
- MakeLine(&distance_line, &map->point[2], &map->point[3]);
- line_set.push_back(distance_line);
- MakeLine(&distance_line, &map->point[3], &map->point[4]);
- line_set.push_back(distance_line);
- MakeLine(&distance_line, &map->point[4], &map->point[5]);
- line_set.push_back(distance_line);
- MakeLine(&distance_line, &map->point[5], &map->point[6]);
- line_set.push_back(distance_line);
- DistanceOfTire2X(dtox, car, line_set);
- MA_SendDistance(dtox[0], dtox[1]);
-
- if (testStatus == TESTING && !occurMoveBack && ExitParkArea(map, car)) {
- // 鍏ュ簱鍚庝竴鐩村墠杩涳紝杞﹀ご绉诲嚭椹剁绾�
- if (++exitAreaCfm >= 4) { // 閬垮厤淇″彿婕傜Щ閫犳垚鐨勮鍒�
- AddExamFault(10103, rtkTime);
- DEBUG("鐩存帴椹剁娴嬭瘯鍖�,涓嶆寜鑰冭瘯鍛樻寚浠ら┚椹�");
- testStatus = TEST_FAIL;
- }
- } else {
- exitAreaCfm = 0;
- }
-
- if (occurMoveBack) {
- uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
-
- if (!reportExamTimeout && tp - moveBackTimePoint >= examParam.park_edge_limit_time) {
- // 瓒呮椂90绉掞紝涓嶅悎鏍�
- AddExamFault(20402, rtkTime);
- reportExamTimeout = true;
- DEBUG("绉诲簱90绉掕秴鏃�");
- }
- }
-
- if (moveStatus != prevMoveStatus) {
- if (moveStatus == 0) {
- parkStatus = 0;
- stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- storeMoveStatusBeforeStop = prevMoveStatus;
- gearAtStop = (currGear == GEAR_R ? 1 : 0);
- DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
- DEBUG("鍋滆溅鏃舵尅浣� = %d", gearAtStop);
- } else if (prevMoveStatus == 0 && stopTimepoint > 0) {
- DEBUG("缁х画琛岄┒ %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
- uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
-
- DEBUG("鍋滆溅鏃堕棿 %ld", tp - stopTimepoint);
- DEBUG("鍐嶆绉诲姩鏃舵尅浣� = %d", currGear == GEAR_R ? 1 : 0);
-
- if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_edge_pause_criteria)
- && occurMoveBack
- && gearAtStop == (currGear == GEAR_R ? 1 : 0)) {
- // 鍋滆溅瓒�2绉掞紝姣忔鎵�5鍒�
- AddExamFault(20406, rtkTime);
- DEBUG("鍋滆溅瓒呮椂");
- }
-
- if (moveStatus == 1 && checkPark && !checkLight) {
- // 鍦ㄨ繖閲屾鏌ヨ浆鍚戠伅鐘舵��
- checkLight = true;
- if (ReadCarStatus(TURN_SIGNAL_LAMP) != LEFT_TURN_LIGHT) {
- // 涓嶅紑杞悜鐏紝鎵�10鍒�
- AddExamFault(20405, rtkTime);
- DEBUG("鏈紑鍚浆鍚戠伅");
- }
- }
-
- /*if (moveStatus == storeMoveStatusBeforeStop) {
- // 鍚屾柟鍚戝啀鍚姩锛岀户缁垽鏂槸鍚﹀仠杞﹁秴鏃�
- if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_edge_pause_criteria) && occurMoveBack) {
- // 鍋滆溅瓒�2绉掞紝姣忔鎵�5鍒�
- AddExamFault(20406, rtkTime);
- DEBUG("鍋滆溅瓒呮椂");
- }
- } else {
- // 鍊掕溅鍒囨崲鍒板墠杩�
- if (moveStatus == 1 && tp - stopTimepoint >= CHECK_PARK_DELAY) {
- if (parkStatus == 0) {
- if (EnterParking(map, car)) {
- parkStatus = 1;
- parkSuccess = true;
- }
- }
-
- if (!parkSuccess && !reportParkFail) {
- // 鍋滄鍚庯紝杞﹁韩鍑虹嚎锛屼笉鍚堟牸
- AddExamFault(20401, rtkTime);
- reportParkFail = true;
- DEBUG("绉诲簱涓嶅叆");
- }
-
- // 鍦ㄨ繖閲屾鏌ヨ浆鍚戠伅鐘舵��
- if (ReadCarStatus(TURN_SIGNAL_LAMP) != LEFT_TURN_LIGHT) {
- // 涓嶅紑杞悜鐏紝鎵�10鍒�
- AddExamFault(20405, rtkTime);
- DEBUG("鏈紑鍚浆鍚戠伅");
- }
- }
- }*/
-
- /* if (moveStatus == -1 && !occurMoveBack) {
- DEBUG("寮�濮嬪�掕溅");
- occurMoveBack = true;
- moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 1);
- }*/
- }
-
- prevMoveStatus = moveStatus;
- } /*else if (moveStatus == 0 && parkStatus == 0) {
- uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
-
- if (tp - stopTimepoint >= CHECK_PARK_DELAY) {
- if (EnterParking(map, car)) {
- parkStatus = 1;
- parkSuccess = true;
- } else {
- parkStatus = -1;
- }
- }
- }*/
-
-TEST_END:
- if (testStatus == TEST_FINISH) {
- DEBUG("渚ф柟鍋滆溅缁撴潫");
- MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 0);
- return 0;
- }
- return 1;
+ AppTimer_delete(ParkTimeout);
}
-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_t *car)
+static void ParkTimeout(apptimer_var_t val)
{
- bool ret = false;
+ // 瓒呮椂90绉掞紝涓嶅悎鏍�
+ DEBUG("绉诲簱90绉掕秴鏃�");
+ AddExamFault(20402);
+}
- Line red_line;
- const int red_lines[][2] = {{0, 7}, {1, 2}, {5, 6}};
+void TestParkEdge(prime_t &prime)
+{
+ if (prime.examing_area.type != MAP_TYPE_PARK_EDGE)
+ return;
+ // 妫�娴嬪悗杞槸鍚﹂┒杩囧簱浣嶅墠娈�
+ if (!overpass_parkingspace) {
+ overpass_parkingspace = OverpassParkspace(prime);
+ }
+
+ if (prime.pMotion->move == BACKWARD && prime.sensor.gear == GEAR_R) {
+ if (!firstMoveBack) {
+ // 寮�濮嬫寕鍊掓尅鍊掕溅
+ firstMoveBack = true;
+ if (!overpass_parkingspace) {
+ DEBUG("鍚庤疆娌℃湁寮�杩囧簱浣嶇嚎灏卞紑濮嬪�掕溅");
+ AddExamFault(10103);
+ }
+ prime.examing_area.stage = PE_PARKING;
+ AppTimer_delete(ParkTimeout);
+ AppTimer_add(ParkTimeout, prime.examParam.park_edge_limit_time);
+ }
+ }
+
+ //浠庡�掕溅鐘舵�侊紝鐪嬫槸鍚︿粠鍊掓尅閫�鍑�(濡傛灉鍙槸鐪嬪仠杞︾姸鎬侊紝鏃犳硶鍜屼腑閫斿仠杞︽湁鏁堝尯鍒�)
+ if (firstMoveBack && prime.sensor.gear != GEAR_R) {
+ if (!check_parking_space) {
+ check_parking_space = true;
+ if (!CheckParkspace(prime)) {
+ // 鍋滄鍚庯紝杞﹁韩鍑虹嚎锛屼笉鍚堟牸
+ DEBUG("杞﹁韩鍑虹嚎");
+ AddExamFault(20401);
+ }
+ prime.examing_area.stage = PE_EXIT;
+ }
+ }
+
+ // 妫�娴嬪乏杞悜鐏�
+ if (check_parking_space && prime.pMotion->move == FORWARD && !checkLight) {
+ checkLight = true;
+ if (prime.sensor.turn_signal_lamp != LEFT_TURN_LIGHT) {
+ // 涓嶅紑杞悜鐏紝鎵�10鍒�
+ DEBUG("鏈紑鍚浆鍚戠伅");
+ AddExamFault(20405);
+ }
+ }
+
+ if (CrashRedLine(prime)) {
+ if (!occurCrashRedLine) {
+ // 杞﹁韩鍘嬪簱浣嶇嚎锛屾瘡娆℃墸10鍒�
+ DEBUG("杞﹁韩鍘嬪簱浣嶇嚎");
+ AddExamFault(20404);
+ occurCrashRedLine = true;
+ }
+ } else {
+ occurCrashRedLine = false;
+ }
+}
+
+// 杞﹁疆鎴栬溅韬槸鍚﹀帇閬撹矾杈圭嚎
+static bool CrashRedLine(prime_t &prime)
+{
+ const int red_lines[][2] = {{0, 7}, {1, 2}, {2, 3}, {3, 4}, {4, 5}, {5, 6}};
Line frontAxle, rearAxle;
- 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]]);
+ MAKE_LINE(frontAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]]);
+ MAKE_LINE(rearAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]], prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]);
- for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
- MakeLine(&red_line, &map->point[red_lines[i][0]], &map->point[red_lines[i][1]]);
- if (IntersectionOf(red_line, frontAxle) == GM_Intersection ||
- IntersectionOf(red_line, rearAxle) == GM_Intersection) {
- ret = true;
- break;
- }
- }
-
- return ret;
-}
-
-// 杞﹁韩鏄惁鍘嬪簱浣嶇嚎
-static bool CrashRedLine2(const Polygon *map, const car_model_t *car)
-{
- bool ret = false;
-
- Line red_line;
- const int red_lines[][2] = {{0, 7}, {2, 3}, {3, 4}, {4, 5}};
-
- 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 body(prime.pModel->body.size());
+ for (int i = 0; i < prime.pModel->body.size(); ++i) {
+ body.AddPoint(prime.pModeling->points[prime.pModel->body[i]]);
}
for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
- MakeLine(&red_line, &map->point[red_lines[i][0]], &map->point[red_lines[i][1]]);
- if (IntersectionOf(red_line, &car_body) != GM_None) {
- ret = true;
- break;
- }
- }
-
- free(car_body.point);
- return ret;
-}
-
-static bool CrashRedLine3(const Polygon *map, const car_model_t *car) {
- bool ret = false;
-
- if (!occurMoveBack) {
- // 鍊掕溅鍓嶏紝杞﹁韩涓嶅緱鍘嬪簱浣嶈櫄绾�
- Polygon car_body;
Line red_line;
-
- 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]];
+ MAKE_LINE(red_line, std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[red_lines[i][0]], std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[red_lines[i][1]]);
+ if (IntersectionOf(red_line, frontAxle) == GM_Intersection
+ || IntersectionOf(red_line, rearAxle) == GM_Intersection
+ || IntersectionOf(red_line, body.GetPolygon()) != GM_None) {
+ return true;
}
-
- MakeLine(&red_line, &map->point[2], &map->point[5]);
- if (IntersectionOf(red_line, &car_body) != GM_None) {
- ret = true;
- }
-
- free(car_body.point);
}
- return ret;
+ return false;
+}
+
+static bool CheckParkspace(prime_t &prime) {
+ DEBUG("妫�鏌ュ仠杞﹀埌浣�...");
+
+ MakePolygon area({std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[8],
+ std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[3],
+ std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[4],
+ std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[9]});
+ MakePolygon car_body(prime.pModel->body.size());
+ for (int i = 0; i < prime.pModel->body.size(); ++i) {
+ car_body.AddPoint(prime.pModeling->points[prime.pModel->body[i]]);
+ }
+
+ return (IntersectionOf(car_body.GetPolygon(), area.GetPolygon()) == GM_Containment)? true : false;
+}
+
+static bool OverpassParkspace(prime_t &prime)
+{
+ Line parkspace_top;
+
+ MAKE_LINE(parkspace_top, std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[4], std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[5]);
+
+ if (IntersectionOfLine(prime.pModeling->points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]], parkspace_top) == REL_POS_RIGHT
+ && IntersectionOfLine(prime.pModeling->points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]], parkspace_top) == REL_POS_RIGHT) {
+ return true;
+ }
+ return false;
}
--
Gitblit v1.8.0