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