From 9f309437fcafaebcc33224abd951a6b6fdea8e41 Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期日, 23 二月 2020 15:20:48 +0800
Subject: [PATCH] 角度修正。
---
lib/src/main/cpp/driver_test.cpp | 40 +++++++++++++++++++++++-----------------
1 files changed, 23 insertions(+), 17 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index a78aa18..f960aa6 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -977,25 +977,31 @@
carModel->basePoint = main_ant;
for (int i = 0; i < carModel->pointNum; ++i) {
- double tx = carModel->carDesc[i].distance*sin(toRadians(azimuth));
- double ty = carModel->carDesc[i].distance*cos(toRadians(azimuth));
+ double qrx = carModel->carDesc[i].distance * sin(toRadians(carModel->carDesc[i].angle));
+ double qry = carModel->carDesc[i].distance * cos(toRadians(carModel->carDesc[i].angle)) * cos(toRadians(pitch));
-// double qrx = carModel->carDesc[i].distance * sin(toRadians(carModel->carDesc[i].angle));
-// double qry = carModel->carDesc[i].distance * cos(toRadians(carModel->carDesc[i].angle)) * cos(toRadians(pitch));
-//
-// double projDis = sqrt(pow(qrx, 2) + pow(pry, 2));
-// double projAng;
-//
-// if (fabs(qry) <= EPSILON) {
-// projAng = 90;
-// } else {
-// projAng = toDegree(atan(qrx / qry));
-// }
+ double projectDistance = sqrt(pow(qrx, 2) + pow(qry, 2));
+ double projectAngle = toDegree(acos(qry / projectDistance));
- carModel->carXY[i].X = (tx)*cos(toRadians(carModel->carDesc[i].angle)) -
- (ty)*sin(toRadians(carModel->carDesc[i].angle)) + main_ant.X;
- carModel->carXY[i].Y = (tx)*sin(toRadians(carModel->carDesc[i].angle)) +
- (ty)*cos(toRadians(carModel->carDesc[i].angle)) + main_ant.Y;
+ if (carModel->carDesc[i].angle > 180) {
+ projectAngle = 360 - projectAngle;
+ }
+
+ double tx = projectDistance*sin(toRadians(azimuth));
+ double ty = projectDistance*cos(toRadians(azimuth));
+
+ carModel->carXY[i].X = tx * cos(toRadians(projectAngle)) -
+ ty*sin(toRadians(projectAngle)) + main_ant.X;
+ carModel->carXY[i].Y = tx * sin(toRadians(projectAngle)) +
+ ty * cos(toRadians(projectAngle)) + main_ant.Y;
+
+// double tx = carModel->carDesc[i].distance*sin(toRadians(azimuth));
+// double ty = carModel->carDesc[i].distance*cos(toRadians(azimuth));
+//
+// carModel->carXY[i].X = (tx)*cos(toRadians(carModel->carDesc[i].angle)) -
+// (ty)*sin(toRadians(carModel->carDesc[i].angle)) + main_ant.X;
+// carModel->carXY[i].Y = (tx)*sin(toRadians(carModel->carDesc[i].angle)) +
+// (ty)*cos(toRadians(carModel->carDesc[i].angle)) + main_ant.Y;
}
}
--
Gitblit v1.8.0