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