[6.0.0][Opt]更新轨迹擦除算法

core/mogo-core-utils/src/main/java/com/mogo/eagle/core/utilcode/util/DrivingDirectionUtils.java

@@ -153,4 +153,43 @@ public class DrivingDirectionUtils {
         return 180 - Math.abs(Math.abs(angle1 - angle2) - 180);
     }
 
+    /**
+     *
+     * @param dx1
+     * @param dy1
+     * @param dx2
+     * @param dy2
+     * @return 两点组成的线段与正北的夹角
+     */
+    public static double getLineAngle(double dx1, double dy1, double dx2, double dy2) {
+        return getAngleToNorth((dx2 - dx1), (dy2 - dy1));
+    }
+
+    public static double getAngleToNorth(double x, double y) {
+        double resAngle = 0;
+
+        if (0 == x && 0 == y) {
+            resAngle = -400;
+        } else if (0 == y) {
+            if (0 < x) {
+                resAngle = 90;
+            } else {
+                resAngle = 270;
+            }
+        } else if (0 == x) {
+            if (0 < y) {
+                resAngle = 0;
+            } else {
+                resAngle = 180;
+            }
+        } else if (0 < y) {
+            resAngle = (Math.atan((x / y)) * 180 / PI);
+            if (0 > resAngle) {
+                resAngle += 360;
+            }
+        } else {
+            resAngle = (180 + Math.atan((x / y)) * 180 / PI);
+        }
+        return resAngle;
+    }
 }

core/mogo-core-utils/src/main/java/com/mogo/eagle/core/utilcode/util/LocationUtils.java

@@ -68,6 +68,8 @@ public class LocationUtils {
         }
         double p = (a + b + c) / 2;// 半周长
         double s = Math.sqrt(p * (p - a) * (p - b) * (p - c));// 海伦公式求面积
+        // 如果三个点在一条直线上则求出来的面积为NaN
+        if (Double.isNaN(s)) return 0.0;
         space = 2 * s / a;// 返回点到线的距离（利用三角形面积公式求高）
         return space;
     }