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/driving_curve.cpp | 383 +++++-------------------------------------------------
1 files changed, 36 insertions(+), 347 deletions(-)
diff --git a/lib/src/main/cpp/test_items/driving_curve.cpp b/lib/src/main/cpp/test_items/driving_curve.cpp
index 7ea497a..2da17c5 100644
--- a/lib/src/main/cpp/test_items/driving_curve.cpp
+++ b/lib/src/main/cpp/test_items/driving_curve.cpp
@@ -13,393 +13,82 @@
#include <vector>
#include <cstdlib>
+#include <tuple>
using namespace std;
#define DEBUG(fmt, args...) LOGD("<driving_curve> <%s>: " fmt, __func__, ##args)
-static bool testing = false;
-static int mapIndex = 0;
-static uint32_t stopTimepoint = 0;
-
-static bool reportStopCarTimeout;
-static int prevMoveDirect;
static bool crashRedLine;
-static struct scan_window_t {
- int leftStart;
- int leftEnd;
- int rightStart;
- int rightEnd;
-} scanWindow;
-static bool UpdateStartLine(struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
-static bool UpdateEndLine(bool mode, struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
-static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model_t *car, struct scan_window_t *zone, int &who);
+static bool CrashRedLine(prime_t &prime);
-void StartDrivingCurve(int index, int moveDirect, const struct RtkTime *rtkTime)
+void StartDrivingCurve(prime_t &prime)
{
DEBUG("杩涘叆鏇茬嚎琛岄┒鍦哄湴");
- testing = true;
- mapIndex = index;
-
- prevMoveDirect = moveDirect;
- if (moveDirect == 0) {
- stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- }
- reportStopCarTimeout = false;
crashRedLine = false;
- scanWindow.leftStart = scanWindow.leftEnd = scanWindow.rightStart = scanWindow.rightEnd = 0;
-
- MA_EnterMap(mapIndex, MAP_TYPE_CURVE, 1);
+ MA_EnterMap(prime.examing_area.idx, MAP_TYPE_CURVE, 1);
}
-// 鏇茬嚎鍦哄湴鍏抽敭鐐�
-// 鍏ュ彛宸﹀彸涓ょ偣锛屽嚭鍙ｅ乏鍙充袱鐐癸紝鍓嶅崐閮ㄥぇ灏忓渾鍦嗗績鍜岀洿寰勶紝鍚庡崐閮ㄥぇ灏忓渾鍦嗗績鍜岀洿寰勶紝瀹為檯娴嬮噺澶у皬鍦嗗渾蹇冨亸宸湪20cm鍐�
-int TestCurve(const curve_map_t *map, const car_model_t *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
+void TestDrivingCurve(prime_t &prime)
{
- // 鍒ゆ柇鏄惁鍦ㄨ寖鍥村唴锛屽鏋�4涓溅杞兘涓嶅湪鏇茬嚎鍐咃紝瑙嗕负杞﹁締绂诲紑鍦哄湴
- vector<PointF> pxs = {car->carXY[car->left_front_tire[TIRE_OUTSIDE]],
- car->carXY[car->left_rear_tire[TIRE_OUTSIDE]],
- car->carXY[car->right_front_tire[TIRE_OUTSIDE]],
- car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]};
+ if (prime.examing_area.type != MAP_TYPE_CURVE)
+ return;
- int collide = 0;
-
- for (auto px: pxs) {
- if (IntersectionOfLine(map->front_half_small_circle_centre, map->back_half_small_circle_centre, px) == RELATION_RIGHT) {
- // 浣嶄簬鍓嶅崐閮�,瀹為檯鍦哄湴宸﹀彸杈圭嚎璧风偣銆佺粓鐐瑰拰鍦嗗績鍩烘湰涓嶄細鍏辩嚎锛屽彲鑳藉樊寮傝繎涓�绫筹紝鎵�浠ヨ鍒嗗紑璁＄畻
- // 鍒ゆ柇鐨勭偣瓒呰繃宸﹀彸璧风偣锛屽嵆鍦ㄧ嚎鐨勫乏渚�
- if (AngleOf(map->front_half_small_circle_centre, map->left_start_point, px) < 180) {
- if (DistanceOf(map->front_half_small_circle_centre, px) < map->front_half_small_circle_radius) {
- collide = 1;
- continue;
- }
- }
- if (AngleOf(map->front_half_big_circle_centre, map->right_start_point, px) < 180) {
- if (DistanceOf(map->front_half_big_circle_centre, px) > map->front_half_big_circle_radius) {
- collide = 2;
- continue;
- }
- }
- } else {
- // 鍒ゆ柇鐨勭偣涓嶈秴杩囧乏鍙崇粓鐐癸紝鍗冲湪绾跨殑宸︿晶
- if (AngleOf(map->back_half_small_circle_centre, map->right_end_point, px) < 180) {
- if (DistanceOf(map->back_half_small_circle_centre, px) < map->back_half_small_circle_radius) {
- collide = 2;
- continue;
- }
- }
- if (AngleOf(map->back_half_big_circle_centre, map->left_end_point, px) < 180) {
- if (DistanceOf(map->back_half_big_circle_centre, px) > map->back_half_big_circle_radius) {
- collide = 1;
- continue;
- }
- }
- }
- }
-
-}
-
-int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model_t *car, const car_model_t *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
-{
- Polygon tireRect;
- int who = 0;
- vector<double> dtox;
- vector<Line> line_set;
- int s;
-
- MakePolygon(&tireRect, {car->carXY[car->left_front_tire[TIRE_OUTSIDE]],
- car->carXY[car->right_front_tire[TIRE_OUTSIDE]],
- car->carXY[car->right_rear_tire[TIRE_OUTSIDE]],
- car->carXY[car->left_rear_tire[TIRE_OUTSIDE]]});
-
- // 鏇存柊杞﹀ご鎵弿绾�
- if (!UpdateStartLine(&scanWindow, map, map2, &tireRect)) {
- DEBUG("绂诲紑鍦哄湴");
- testing = false;
- goto TEST_END;
- }
-
- // 鏇存柊杞﹀熬鎵弿绾�
- UpdateEndLine(false, &scanWindow, map, map2, &tireRect);
-
- // 璁＄畻杈硅窛
- s = scanWindow.leftStart;
- for (int e = scanWindow.leftStart - 1; e >= scanWindow.leftEnd; --e) {
- Line redLine;
- MakeLine(&redLine, &map->point[s], &map->point[e]);
- line_set.push_back(redLine);
- s = e;
- }
-
- s = scanWindow.rightStart;
- for (int e = scanWindow.rightStart - 1; e >= scanWindow.rightEnd; --e) {
- Line redLine;
- MakeLine(&redLine, &map2->point[s], &map2->point[e]);
- line_set.push_back(redLine);
- s = e;
- }
-
- DistanceOfTire2X(dtox, car, line_set);
-
- MA_SendDistance(dtox[0], dtox[1]);
-
- DEBUG("scanWindow leftStart %d leftEnd %d rightStart %d rightEnd %d", scanWindow.leftStart, scanWindow.leftEnd, scanWindow.rightStart, scanWindow.rightEnd);
-
- if (CrashRedLine(map, map2, car, &scanWindow, who)) {
+ if (CrashRedLine(prime)) {
if (!crashRedLine) {
- crashRedLine = true;
// 杞﹁疆鍘嬭竟绾匡紝涓嶅悎鏍�
- AddExamFault(20601, rtkTime);
DEBUG("杞﹁疆鍘嬭竟绾�");
- if (who == 1) {
- PlayTTS("鍘嬪乏鏇茬嚎", NULL);
- } else if (who == 2) {
- PlayTTS("鍘嬪彸鏇茬嚎", NULL);
- }
+ AddExamFault(20601);
}
+ crashRedLine = true;
} else {
crashRedLine = false;
}
-
- if (moveDirect != prevMoveDirect) {
- if (moveDirect == 0) {
- stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- reportStopCarTimeout = false;
-
- DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
- } else {
-
- }
- prevMoveDirect = moveDirect;
- } else if (moveDirect == 0) {
- uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
-
- if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.curve_pause_criteria) && !reportStopCarTimeout) {
- // 鍋滆溅瓒�2绉掞紝涓嶅悎鏍�
- AddExamFault(20602, rtkTime);
- DEBUG("涓�斿仠杞�");
- reportStopCarTimeout = true;
- }
- }
-
-TEST_END:
- CleanPolygon(&tireRect);
-
- if (!testing) {
- MA_EnterMap(mapIndex, MAP_TYPE_CURVE, 0);
- }
-
- return testing ? 1 : 0;
}
-static bool UpdateStartLine(struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect)
+void StopDrivingCurve(prime_t &prime)
{
- Line start;
+ if (prime.examing_area.type != MAP_TYPE_CURVE)
+ return;
+ DEBUG("绂诲紑鏇茶椹舵槸鍦哄湴");
- bool update = true;
- int direct = 0;
- int tempLeft = scanWindow.leftStart, tempRight = scanWindow.rightStart;
+ prime.examing_area.type = MAP_TYPE_NONE;
+}
- while (update) {
- update = false;
- MakeLine(&start, &map->point[scanWindow.leftStart], &map2->point[scanWindow.rightStart]);
- if (IntersectionOf(start, tireRect) == GM_None) {
- if (direct != 1) {
- direct = -1;
- // 鍏ュ満鏂瑰悜鎵弿
- tempLeft = scanWindow.leftStart;
- tempRight = scanWindow.rightStart;
+// 鏇茬嚎鍦哄湴鍏抽敭鐐�
+// 鍏ュ彛宸﹀彸涓ょ偣锛屽嚭鍙ｅ乏鍙充袱鐐癸紝鍓嶅崐閮ㄥぇ灏忓渾鍦嗗績鍜岀洿寰勶紝鍚庡崐閮ㄥぇ灏忓渾鍦嗗績鍜岀洿寰勶紝瀹為檯娴嬮噺澶у皬鍦嗗渾蹇冨亸宸湪20cm鍐�
+static bool CrashRedLine(prime_t &prime)
+{
+ // 鍒ゆ柇鏄惁鍦ㄨ寖鍥村唴锛�4涓溅杞彧瑕佹弧瓒冲湪灏忓渾澶栵紝澶у渾鍐咃紝灏辫鏄庢病鏈夊帇绾�
+ PointF tires[] = {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]],
+ 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]]};
- if (scanWindow.leftStart > 0) {
- update = true;
- scanWindow.leftStart--;
- }
- if (scanWindow.rightStart > 0) {
- update = true;
- scanWindow.rightStart--;
- }
+ for (int i = 0; i < 4; i++) {
+ if (IntersectionOfLine(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_small_circle_centre,
+ std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_small_circle_centre, tires[i]) == REL_POS_RIGHT) {
+ // 鍓嶅崐閮ㄥ垎
+ if (DistanceOf(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_small_circle_centre, tires[i]) > std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_small_circle_radius
+ && DistanceOf(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_big_circle_centre, tires[i]) < std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_big_circle_radius) {
- if (scanWindow.leftStart <= scanWindow.leftEnd && scanWindow.rightStart <= scanWindow.rightEnd) {
- DEBUG("杞﹁締涓㈠け锛岄噸鏂版悳绱� %d %d", scanWindow.leftStart, scanWindow.rightStart);
-
- // 杞﹁締涓㈠け锛岄噸鏂版悳绱�
- update = false;
- scanWindow.leftEnd = scanWindow.rightEnd = 0;
-
- if (UpdateEndLine(true, &scanWindow, map, map2, tireRect)) {
- DEBUG("鍖归厤鎴愬姛 %d %d", scanWindow.leftStart, scanWindow.leftEnd);
- direct = 0;
- update = true;
- } else {
- DEBUG("鍖归厤澶辫触");
- return false;
- }
- }
- }
- } else {
- if (direct != -1) {
- // 鍑哄満鏂瑰悜鎵弿
- direct = 1;
- if (scanWindow.leftStart < map->num - 1) {
- update = true;
- scanWindow.leftStart++;
- }
- if (scanWindow.rightStart < map2->num - 1) {
- update = true;
- scanWindow.rightStart++;
- }
} else {
- scanWindow.leftStart = tempLeft;
- scanWindow.rightStart = tempRight;
- }
- }
- }
-
- return true;
-}
-
-static bool UpdateEndLine(bool mode, struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect)
-{
- bool update = true;
- bool crash = false;
-
- int direct = 0;
- int tempLeft = zone->leftEnd;
- int tempRight = zone->rightEnd;
-
- Line end;
-
- while (update) {
- update = false;
- MakeLine(&end, &map->point[zone->leftEnd], &map2->point[zone->rightEnd]);
-
- if (IntersectionOf(end, tireRect) == GM_None) {
- if (direct != -1) {
- // 鍑哄満鏂瑰悜鎵弿
- direct = 1;
- tempLeft = zone->leftEnd;
- tempRight = zone->rightEnd;
-
- if (zone->leftEnd < map->num - 1) {
- update = true;
- zone->leftEnd++;
- }
- if (zone->rightEnd < map2->num - 1) {
- update = true;
- zone->rightEnd++;
- }
+ return true;
}
} else {
- if (!crash) {
- crash = true;
- if (mode) {
- zone->leftStart = zone->leftEnd;
- zone->rightStart = zone->rightEnd;
+ if (DistanceOf(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_small_circle_centre, tires[i]) > std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_small_circle_radius
+ && DistanceOf(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_big_circle_centre, tires[i]) < std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_big_circle_radius) {
- DEBUG("绗竴娆℃帴瑙� %d %d %d", zone->leftStart, zone->leftEnd, tempLeft);
- }
- }
-
- if (direct != 1) {
- // 鍏ュ満鏂瑰悜鎵弿
- direct = -1;
- if (zone->leftEnd > 0) {
- update = true;
- zone->leftEnd--;
- }
- if (zone->rightEnd > 0) {
- update = true;
- zone->rightEnd--;
- }
} else {
- zone->leftEnd = tempLeft;
- zone->rightEnd = tempRight;
+ return true;
}
}
}
- return crash;
-}
-
-bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model_t *car)
-{
-// 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
- bool ret = false;
-
- Polygon carBody;
- Polygon bigMap;
-
- bigMap.num = map->num + map2->num;
- bigMap.point = (PointF *) malloc(bigMap.num * sizeof(PointF));
-
- int i = 0;
- for (; i < map->num; ++i) {
- bigMap.point[i] = map->point[i];
- }
- for (int j = map2->num; j > 0; --j) {
- bigMap.point[i++] = map2->point[j-1];
- }
-
- 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, &bigMap) == GM_None) {
- ret = true;
- }
-
- free(carBody.point);
- free(bigMap.point);
-
- return ret;
-}
-
-// 杞﹁疆鏄惁鍘嬭竟绾�
-static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model_t *car, struct scan_window_t *zone, int &who)
-{
- bool ret = false;
-
- Line frontTireAxial, rearTireAxial;
- Line redLine;
-
- MakeLine(&frontTireAxial, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
- MakeLine(&rearTireAxial, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
-
- int s = zone->leftStart;
- for (int e = zone->leftStart - 1; e >= zone->leftEnd; --e) {
- MakeLine(&redLine, &map->point[s], &map->point[e]);
- if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
- who = 1;
- return true;
- }
- if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
- who = 1;
- return true;
- }
- s = e;
- }
-
- s = zone->rightStart;
- for (int e = zone->rightStart - 1; e >= zone->rightEnd; --e) {
- MakeLine(&redLine, &map2->point[s], &map2->point[e]);
- if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
- who = 2;
- return true;
- }
- if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
- who = 2;
- return true;
- }
- s = e;
- }
-
- return ret;
+ return false;
}
--
Gitblit v1.8.0