From 0701276b4fec856d5427e4776eec3cc7c56ec065 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期二, 25 八月 2020 17:51:10 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_common/Geometry.cpp | 41 +++++++++++++++++++++++++++++++++++++++++
1 files changed, 41 insertions(+), 0 deletions(-)
diff --git a/lib/src/main/cpp/test_common/Geometry.cpp b/lib/src/main/cpp/test_common/Geometry.cpp
index 242f55f..03c9d4b 100644
--- a/lib/src/main/cpp/test_common/Geometry.cpp
+++ b/lib/src/main/cpp/test_common/Geometry.cpp
@@ -14,6 +14,7 @@
#include <iosfwd>
#include <iomanip>
#include <sstream>
+#include <vector>
#include "../jni_log.h"
@@ -626,3 +627,43 @@
{
return isEqual(p1.X, p2.X) && isEqual(p1.Y, p2.Y);
}
+
+double AvgYaw(std::vector<double> &angles)
+{
+ double x = 0, y = 0;
+ double deg = 0;
+
+ for (int i = 0; i < angles.size(); ++i) {
+ x += sin(toRadians(angles[i]));
+ y += cos(toRadians(angles[i]));
+ }
+
+ if (fabs(y) <= EPSILON) {
+ if (x > EPSILON) {
+ deg = 90;
+ } else {
+ deg = 270;
+ }
+ }
+ else if (fabs(x) <= EPSILON) {
+ if (y > EPSILON) {
+ deg = 0;
+ } else {
+ deg = 180;
+ }
+ } else {
+ deg = toDegree(atan(fabs(x / y)));
+
+ if (x < -EPSILON && y > EPSILON) {
+ deg = 360 - deg;
+ }
+ else if (x < -EPSILON && y < -EPSILON) {
+ deg = 180 + deg;
+ }
+ else if (x > EPSILON && y < -EPSILON) {
+ deg = 180 - deg;
+ }
+ }
+
+ return deg;
+}
\ No newline at end of file
--
Gitblit v1.8.0