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