From 4ff195404b21f74ca11e26a69cbf0418eaa4595f Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期二, 17 三月 2020 17:57:03 +0800
Subject: [PATCH] 科目三草案实现
---
lib/src/main/cpp/Geometry.cpp | 48 +++++++++++++++++++++++++++++++++++++++++++++++-
1 files changed, 47 insertions(+), 1 deletions(-)
diff --git a/lib/src/main/cpp/Geometry.cpp b/lib/src/main/cpp/Geometry.cpp
index 39531c1..2693543 100644
--- a/lib/src/main/cpp/Geometry.cpp
+++ b/lib/src/main/cpp/Geometry.cpp
@@ -301,6 +301,52 @@
return fabs(point.X*line.Y1 + point.Y*line.X2 + line.X1*line.Y2 - line.X2*line.Y1 - line.X1*point.Y - point.X*line.Y2) / c;
}
+/*********************************************************
+ * p2----------->p1 绾跨鍜孻杞寸殑澶硅
+ * @param p1
+ * @param p2
+ * @return yaw
+ */
+double YawOf(PointF p1, PointF p2)
+{
+ double deg = 0.0;
+
+ if (fabs(p1.Y - p2.Y) <= GLB_EPSILON) {
+ if (p1.X > p2.X) {
+ deg = 90;
+ } else {
+ deg = 270;
+ }
+ } else if (fabs(p1.X - p2.X) <= GLB_EPSILON) {
+ if (p1.Y > p2.Y) {
+ deg = 0;
+ } else {
+ deg = 180;
+ }
+ } else {
+ deg = atan(fabs(p1.X - p2.X) /
+ fabs(p1.Y - p2.Y));
+
+ deg = toDegree(deg);
+
+ if (p1.X > p2.X &&
+ p1.Y > p2.Y) {
+
+ } else if (p1.X < p2.X &&
+ p1.Y > p2.Y) {
+ deg = 360 - deg;
+ } else if (p1.X < p2.X &&
+ p1.Y < p2.Y) {
+ deg = 180 + deg;
+ } else if (p1.X > p2.X &&
+ p1.Y < p2.Y) {
+ deg = 180 - deg;
+ }
+ }
+
+ return deg;
+}
+
/**********************************************************
* base 鍜� dest鐨勭浜岀偣閲嶅悎鏃跺舰鎴愮殑澶硅
* @param base
@@ -364,7 +410,7 @@
}
/***************************************************************
- * @brief p3浣嶄簬鐢眕1->p2鏋勬垚鐨勫皠绾匡紝宸︿晶杩樻槸鍙充晶锛屽悓鍚戯紝鍙嶅悜
+ * @brief p3浣嶄簬鐢眕1--------->p2鏋勬垚鐨勫皠绾匡紝宸︿晶杩樻槸鍙充晶锛屽悓鍚戯紝鍙嶅悜
* @param p1
* @param p2
* @param p3
--
Gitblit v1.8.0