From 25777013517d1bd398a98504826a417236706af2 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期二, 24 三月 2020 19:01:29 +0800
Subject: [PATCH] 车辆信号完善,路边停车完善。

---
 lib/src/main/cpp/Geometry.cpp |   40 ++++++++++++++++++++++++++++++++++++++++
 1 files changed, 40 insertions(+), 0 deletions(-)

diff --git a/lib/src/main/cpp/Geometry.cpp b/lib/src/main/cpp/Geometry.cpp
index 2693543..01e15cd 100644
--- a/lib/src/main/cpp/Geometry.cpp
+++ b/lib/src/main/cpp/Geometry.cpp
@@ -16,10 +16,16 @@
 using namespace std;
 
 const double EPSILON = 1e-6;
+const double EPSILON2 = 1e-3;
 
 inline bool isEqual(double a, double b)
 {
     return (fabs(a - b) <= EPSILON);
+}
+
+inline bool isEqual2(double a, double b)
+{
+    return (fabs(a - b) <= EPSILON2);
 }
 
 inline double toRadians(double degree)
@@ -434,3 +440,37 @@
         return -1;
     }
 }
+
+/***************************************************************
+ * 寰楀埌p3浜巔1,p2缁勬垚鐨勭洿绾夸笂鐨勫瀭鐐�
+ * @param p1
+ * @param p2
+ * @param p3
+ * @return
+ */
+PointF GetVerticalPoint(PointF p1, PointF p2, PointF p3)
+{
+    PointF p4;
+
+    if (isEqual2(p1.X, p2.X)) {
+        p4.Y = p3.Y;
+        p4.X = p1.X;
+        return p4;
+    }
+    if (isEqual2(p1.Y, p2.Y)) {
+        p4.X = p3.X;
+        p4.Y = p1.Y;
+        return p4;
+    }
+
+    double k = (p2.Y - p1.Y) / (p2.X - p1.X);
+    double b = p1.Y - k * p1.X;
+
+    double k2 = (p2.X - p1.X) / (p1.Y - p2.Y);
+    double b2 = p3.Y - p3.X * k2;
+
+    p4.X = (b2 - b) / (k - k2);
+    p4.Y = k2 * p4.X + b2;
+
+    return p4;
+}

--
Gitblit v1.8.0