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