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/driving_curve.cpp | 55 +++++++++++++++++++++++++++++++++++++++++++++++++++----
1 files changed, 51 insertions(+), 4 deletions(-)
diff --git a/lib/src/main/cpp/test_items/driving_curve.cpp b/lib/src/main/cpp/test_items/driving_curve.cpp
index 447ef11..7ea497a 100644
--- a/lib/src/main/cpp/test_items/driving_curve.cpp
+++ b/lib/src/main/cpp/test_items/driving_curve.cpp
@@ -34,7 +34,7 @@
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 *car, struct scan_window_t *zone, int &who);
+static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model_t *car, struct scan_window_t *zone, int &who);
void StartDrivingCurve(int index, int moveDirect, const struct RtkTime *rtkTime)
{
@@ -55,7 +55,54 @@
MA_EnterMap(mapIndex, MAP_TYPE_CURVE, 1);
}
-int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
+// 鏇茬嚎鍦哄湴鍏抽敭鐐�
+// 鍏ュ彛宸﹀彸涓ょ偣锛屽嚭鍙ｅ乏鍙充袱鐐癸紝鍓嶅崐閮ㄥぇ灏忓渾鍦嗗績鍜岀洿寰勶紝鍚庡崐閮ㄥぇ灏忓渾鍦嗗績鍜岀洿寰勶紝瀹為檯娴嬮噺澶у皬鍦嗗渾蹇冨亸宸湪20cm鍐�
+int TestCurve(const curve_map_t *map, const car_model_t *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
+{
+ // 鍒ゆ柇鏄惁鍦ㄨ寖鍥村唴锛屽鏋�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]]};
+
+ 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;
@@ -279,7 +326,7 @@
return crash;
}
-bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model *car)
+bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model_t *car)
{
// 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
bool ret = false;
@@ -316,7 +363,7 @@
}
// 杞﹁疆鏄惁鍘嬭竟绾�
-static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone, int &who)
+static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model_t *car, struct scan_window_t *zone, int &who)
{
bool ret = false;
--
Gitblit v1.8.0