From 25bdbfbf6f4c9f1530be74fc194ad1dd0a13e8c9 Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期日, 08 三月 2020 23:12:49 +0800
Subject: [PATCH] 角度修正。

---
 lib/src/main/cpp/driver_test.cpp |   24 ++++++------------------
 1 files changed, 6 insertions(+), 18 deletions(-)

diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 4ee19db..c961a25 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -202,7 +202,7 @@
 
 void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
                     int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
-                    int *body, int bodyNum,double (*point)[2], int pointNum)
+                    int *body, int bodyNum,double (*point)[2], int pointNum, double antPitch)
 {
     DEBUG("鍔犲叆杞﹁締淇℃伅 pointNum %d", pointNum);
 
@@ -264,6 +264,8 @@
             CarModel->body[i] = body[i];
         }
     }
+
+    CarModel->antPitch = antPitch;
 
     CarModel->pointNum = pointNum;
     CarModel->carDesc = (struct car_desc_ *)malloc(sizeof(struct car_desc_) * pointNum);
@@ -962,23 +964,7 @@
             free(map.point);
     }
     if (mapList[index].type == MAP_TYPE_CURVE) {
-        Line carAxial;
-        Line scanLine;
-
-        MakeLine(&carAxial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);
-
-        int startL = 0, startR = 0;
-        while (startL < mapList[index].map.num || startR < mapList[index].map2.num) {
-            MakeLine(&scanLine, &mapList[index].map.point[startL], &mapList[index].map.point[startR]);
-            if (IntersectionOf(scanLine, carAxial) == GM_Intersection) {
-                break;
-            }
-            if (startL < mapList[index].map.num)
-                startL++;
-            if (startR < mapList[index].map2.num)
-                startR++;
-        }
-        ret = true;
+        ret = ExitDrivingCurveArea(&mapList[index].map, &mapList[index].map2, car);
     }
 
     return ret;
@@ -1032,6 +1018,8 @@
 {
     carModel->basePoint = main_ant;
 
+    pitch = pitch - carModel->antPitch;
+
     for (int i = 0; i < carModel->pointNum; ++i) {
         double qrx = carModel->carDesc[i].distance * sin(toRadians(carModel->carDesc[i].angle));
         double qry =

--
Gitblit v1.8.0