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