From 76859aa4b23ea8ebd90bd7553fd70e144bdc96ba Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期五, 15 五月 2020 15:53:20 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/stop_car.cpp | 78 +++++++++++++++++++++++---------------
1 files changed, 47 insertions(+), 31 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/stop_car.cpp b/lib/src/main/cpp/test_items2/stop_car.cpp
index 4e93662..514b2e4 100644
--- a/lib/src/main/cpp/test_items2/stop_car.cpp
+++ b/lib/src/main/cpp/test_items2/stop_car.cpp
@@ -33,12 +33,8 @@
static void PlayTTSTimeout(union sigval sig);
-void StartStopCarExam(int index, LIST_ROAD_MAP &RoadMapList) {
- if (index == -1)
- return;
-
- DEBUG("杩涘叆闈犺竟鍋滆溅鍦板浘 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type);
-
+void StartStopCarExam(std::string tts) {
+ DEBUG("闈犺竟鍋滆溅");
ttsPlayEnd = 0;
moveDistance = 0;
prevMoveDirect = 0;
@@ -49,8 +45,8 @@
doorNotClose = false;
checkRoadDistance = false;
- if (!RoadMapList[index].tts.empty()) {
- examTtsSeq = PlayTTS(RoadMapList[index].tts.c_str());
+ if (!tts.empty()) {
+ examTtsSeq = PlayTTS(tts.c_str());
} else {
examTtsSeq = PlayTTS("璇烽潬杈瑰仠杞�");
}
@@ -73,7 +69,7 @@
AppTimer_delete(PlayTTSTimeout);
}
-int ExecuteStopCarExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
+int ExecuteStopCarExam(int index, road_t &road, const car_model *car,
LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) {
if (ttsPlayEnd == 1) {
ttsPlayEnd = 2;
@@ -114,42 +110,62 @@
if (tp - stopTimepoint >= STOP_CAR_TIME && !checkRoadDistance) {
// 鍋滆溅瓒�2绉掞紝寮�濮嬪垽鏂�
DEBUG("妫�娴嬪拰璺竟鐨勮窛绂�");
- PointF pc;
+ PointF pc, vp;
+ bool get_vp = false;
+ double dis2roadEdge = 0;
checkRoadDistance = true;
-
+ // 鍓嶅悗杞殑涓偣
pc.X = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].X + car->carXY[car->right_rear_tire[TIRE_OUTSIDE]].X) / 2;
pc.Y = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].Y + car->carXY[car->right_rear_tire[TIRE_OUTSIDE]].Y) / 2;
// 妫�娴嬮亾璺竟缂樼嚎
- for (int i = 0; i < RoadMapList[index].roadEdgeLineNum; ++i) {
- PointF p1, p2, pv;
+ for (int i = 0; i < road.rightEdge.size(); ++i) {
+ PointF p1, p2;
+ Line edge;
- p1 = RoadMapList[index].roadEdgeLine[i].point[0];
- for (int j = 1; j < RoadMapList[index].roadEdgeLine[i].num; ++j) {
- p2 = RoadMapList[index].roadEdgeLine[i].point[j];
- pv = GetVerticalPoint(p1, p2, pc);
+ p1 = road.rightEdge[i].points[0];
+ for (int j = 1; j < road.rightEdge[i].points.size(); ++j) {
+ p2 = road.rightEdge[i].points[j];
+ MakeLine(&edge, &p1, &p2);
- if (isEqual2(pv.X, MIN(p1.X, p2.X)) || isEqual2(pv.X, MAX(p1.X, p2.X)) ||
- (pv.X > MIN(p1.X, p2.X) && pv.X < MAX(p1.X, p2.X))) {
- double dis2roadEdge = DistanceOf(pc, pv);
- DEBUG("鍋滆溅璺濊矾杈硅窛绂� %f", dis2roadEdge);
-
- if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_1) {
- DEBUG("鍋滆溅瓒呭嚭璺竟0.5绫�");
- // 鍋滆溅璺濈瓒呰繃50鍘樼背锛屼笉鍚堟牸
- AddExamFault(36, rtkTime);
- } else if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_2) {
- DEBUG("鍋滆溅瓒呭嚭璺竟0.3绫�");
- // 鍋滆溅璺濈瓒呰繃30鍘樼背锛屾墸10鍒�
- AddExamFault(37, rtkTime);
- }
+ if (VerticalPointOnLine(pc, edge, vp)) {
+ get_vp = true;
+ goto FIND_VP_END;
}
p1 = p2;
}
}
+
+FIND_VP_END:
+ if (get_vp) {
+ dis2roadEdge = DistanceOf(pc, vp);
+ } else {
+ // 娌℃湁鎵惧埌鍖归厤绾跨锛屾寜鏈�灏忚窛绂婚《鐐硅绠�
+ dis2roadEdge = 100;
+ for (int i = 0; i < road.rightEdge.size(); ++i) {
+ for (int j = 0; j < road.rightEdge[i].points.size(); ++j) {
+ double dis;
+ if (dis2roadEdge > (dis = DistanceOf(pc, road.rightEdge[i].points[j]))) {
+ dis2roadEdge = dis;
+ }
+ }
+ }
+ }
+
+ DEBUG("鍋滆溅璺濊矾杈硅窛绂� %f", dis2roadEdge);
+
+ if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_1) {
+ DEBUG("鍋滆溅瓒呭嚭璺竟0.5绫�");
+ // 鍋滆溅璺濈瓒呰繃50鍘樼背锛屼笉鍚堟牸
+ AddExamFault(36, rtkTime);
+ } else if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_2) {
+ DEBUG("鍋滆溅瓒呭嚭璺竟0.3绫�");
+ // 鍋滆溅璺濈瓒呰繃30鍘樼背锛屾墸10鍒�
+ AddExamFault(37, rtkTime);
+ }
}
}
--
Gitblit v1.8.0