From 6b136af26a51b3d53051cd3e0e7d467ed2ab8a6b Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期四, 05 三月 2020 13:33:21 +0800
Subject: [PATCH] 更新地图
---
lib/src/main/cpp/test_items/driving_curve.cpp | 192 +++++++++++++++++++++++++++++-------------------
1 files changed, 116 insertions(+), 76 deletions(-)
diff --git a/lib/src/main/cpp/test_items/driving_curve.cpp b/lib/src/main/cpp/test_items/driving_curve.cpp
index b5294a1..3a3cd49 100644
--- a/lib/src/main/cpp/test_items/driving_curve.cpp
+++ b/lib/src/main/cpp/test_items/driving_curve.cpp
@@ -26,15 +26,14 @@
static bool reportStopCarTimeout;
static int prevMoveDirect;
static bool crashRedLine;
-static struct {
+static struct calc_zone_t {
int leftStart;
int leftEnd;
int rightStart;
int rightEnd;
} calcZone;
-static bool CrashRedLine(const Polygon *left, const Polygon *right, const car_model_cache_t *car);
-static bool ExitTestArea(const Polygon *left, const Polygon *right, const car_model_cache_t *car);
+static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct calc_zone_t *zone);
void StartDrivingCurve(int moveDirect, const struct RtkTime *rtkTime)
{
@@ -49,104 +48,145 @@
reportStopCarTimeout = false;
crashRedLine = false;
- calcZone.leftStart = calcZone.leftEnd = calcZone.rightStart = calcZone.rightEnd = -1;
+ calcZone.leftStart = calcZone.leftEnd = calcZone.rightStart = calcZone.rightEnd = 0;
}
int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
Line start, end;
- Polygon zone;
+ Line axial;
- if (calcZone.leftStart == -1) {
- calcZone.leftStart = 0;
- }
- if (calcZone.rightStart == -1) {
- calcZone.rightStart = 0;
+ MakeLine(&axial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);
+
+ MakeLine(&start, &map->point[calcZone.leftStart], &map->point[calcZone.rightStart]);
+ MakeLine(&end, &map->point[calcZone.leftEnd], &map->point[calcZone.rightEnd]);
+
+ if (IntersectionOf(start, axial) == GM_None) {
+ // 鍚戣捣鐐规煡鎵�
+ do {
+ if (calcZone.leftStart == 0 && calcZone.rightStart == 0) {
+ break;
+ }
+ if (calcZone.leftStart > 0)
+ calcZone.leftStart--;
+ if (calcZone.rightStart > 0)
+ calcZone.rightStart--;
+ MakeLine(&start, &map->point[calcZone.leftStart], &map->point[calcZone.rightStart]);
+ } while (IntersectionOf(start, axial) == GM_None);
+ } else {
+ // 鍚戠粓鐐规煡鎵�
+ do {
+ if (calcZone.leftStart >= map->num && calcZone.rightStart >= map2->num) {
+ break;
+ }
+ if (calcZone.leftStart < map->num)
+ calcZone.leftStart++;
+ if (calcZone.rightStart < map2->num)
+ calcZone.rightStart++;
+ MakeLine(&start, &map->point[calcZone.leftStart], &map->point[calcZone.rightStart]);
+ } while (IntersectionOf(start, axial) != GM_None);
}
- start.X1 = map->point[calcZone.leftStart].X;
- start.Y1 = map->point[calcZone.leftStart].Y;
-
- start.X2 = map->point[calcZone.rightStart].X;
- start.Y2 = map->point[calcZone.rightStart].Y;
-
- zone.num = 4;
- zone.point = (PointF *) malloc(zone.num * sizeof(PointF));
- zone.point[0] = car->carXY[car->left_front_tire[TIRE_OUTSIDE]];
- zone.point[1] = car->carXY[car->right_front_tire[TIRE_OUTSIDE]];
- zone.point[2] = car->carXY[car->right_rear_tire[TIRE_OUTSIDE]];
- zone.point[3] = car->carXY[car->left_rear_tire[TIRE_OUTSIDE]];
-
- if (IntersectionOf(start, &zone) == GM_None) {
-
+ if (IntersectionOf(end, axial) == GM_None) {
+ // 鍚戠粓鐐规煡鎵�
+ do {
+ if (calcZone.leftEnd >= map->num && calcZone.rightEnd >= map2->num) {
+ break;
+ }
+ if (calcZone.leftEnd < map->num)
+ calcZone.leftEnd++;
+ if (calcZone.rightEnd < map2->num)
+ calcZone.rightEnd++;
+ MakeLine(&end, &map->point[calcZone.leftEnd], &map->point[calcZone.rightEnd]);
+ } while (IntersectionOf(end, axial) == GM_None);
+ } else {
+ // 鍚戣捣鐐规煡鎵�
+ do {
+ if (calcZone.leftEnd == 0 && calcZone.rightEnd == 0) {
+ break;
+ }
+ if (calcZone.leftEnd > 0)
+ calcZone.leftEnd--;
+ if (calcZone.rightEnd > 0)
+ calcZone.rightEnd--;
+ MakeLine(&end, &map->point[calcZone.leftEnd], &map->point[calcZone.rightEnd]);
+ } while (IntersectionOf(end, axial) == GM_None);
}
- free(zone.point);
+ if (calcZone.leftStart <= calcZone.leftEnd || calcZone.rightStart <= calcZone.rightEnd) {
+ // 绂诲紑鍦哄湴
+ testing = false;
+ }
+
+ if (CrashRedLine(map, map2, car, &calcZone)) {
+ if (!crashRedLine) {
+ crashRedLine = true;
+ // 杞﹁疆鍘嬭竟绾匡紝涓嶅悎鏍�
+ AddExamFault(27, rtkTime);
+ DEBUG("杞﹁疆鍘嬭竟绾�");
+ }
+ } 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 >= STOP_CAR_TIME && !reportStopCarTimeout) {
+ // 鍋滆溅瓒�2绉掞紝涓嶅悎鏍�
+ AddExamFault(28, rtkTime);
+ DEBUG("涓�斿仠杞�");
+ reportStopCarTimeout = true;
+ }
+ }
return testing ? 1 : 0;
}
// 杞﹁疆鏄惁鍘嬭竟绾�
-static bool CrashRedLine(const Polygon *left, const Polygon *right, const car_model_cache_t *car)
+static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct calc_zone_t *zone)
{
bool ret = false;
- car_model_cache_t *prev_car = GetCarModelCache(1);
- if (prev_car == NULL)
- return false;
+ Line frontTireAxial, rearTireAxial;
+ Line redLine;
- // 鎸�4涓疆瀛愮殑澶栦晶璁＄畻
- Line front_left_tire_track, front_right_tire_track, rear_left_tire_track, rear_right_tire_track;
- MakeLine(&front_left_tire_track, &car->points[car->desc->front_left_tire[TIRE_OUTSIDE]], &prev_car->points[car->desc->front_left_tire[TIRE_OUTSIDE]]);
- MakeLine(&front_right_tire_track, &car->points[car->desc->front_right_tire[TIRE_OUTSIDE]], &prev_car->points[car->desc->front_right_tire[TIRE_OUTSIDE]]);
- MakeLine(&rear_left_tire_track, &car->points[car->desc->rear_left_tire[TIRE_OUTSIDE]], &prev_car->points[car->desc->rear_left_tire[TIRE_OUTSIDE]]);
- MakeLine(&rear_right_tire_track, &car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]], &prev_car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]]);
+ 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]]);
- Line line;
-
- line.X1 = left->point[0].X;
- line.Y1 = left->point[0].Y;
- for (int i = 1; i < left->num; ++i) {
- line.X2 = left->point[i].X;
- line.Y2 = left->point[i].Y;
-
- if (IntersectionOf(line, front_left_tire_track) == GM_Intersection ||
- IntersectionOf(line, front_right_tire_track) == GM_Intersection ||
- IntersectionOf(line, rear_left_tire_track) == GM_Intersection ||
- IntersectionOf(line, rear_right_tire_track) == GM_Intersection) {
- ret = true;
- break;
+ 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) {
+ return true;
}
- line.X1 = line.X2;
- line.Y1 = line.Y2;
+ if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
+ return true;
+ }
+ s = e;
}
- line.X1 = right->point[0].X;
- line.Y1 = right->point[0].Y;
- for (int i = 1; !ret && i < right->num; ++i) {
- line.X2 = right->point[i].X;
- line.Y2 = right->point[i].Y;
-
- if (IntersectionOf(line, front_left_tire_track) == GM_Intersection ||
- IntersectionOf(line, front_right_tire_track) == GM_Intersection ||
- IntersectionOf(line, rear_left_tire_track) == GM_Intersection ||
- IntersectionOf(line, rear_right_tire_track) == GM_Intersection) {
- ret = true;
- break;
+ 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) {
+ return true;
}
- line.X1 = line.X2;
- line.Y1 = line.Y2;
+ if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
+ return true;
+ }
+ s = e;
}
return ret;
-}
-
-// 鏁翠釜杞﹁締閮借椹剁璇ユ祴璇曞尯鍩�
-static bool ExitTestArea(const Polygon *left, const Polygon *right, const car_model_cache_t *car)
-{
- for (int i = 0; i < car->point_num; ++i) {
- if (IntersectionOfLine(left->point[0], right->point[0], car->points[i]) != 1)
- return false;
- }
- return true;
}
--
Gitblit v1.8.0