[RoutingOpt]修正车前引导线拖尾问题

[RouterOpt]优化代码逻辑

modules/mogo-module-common/src/main/java/com/mogo/module/common/utils/DrivingDirectionUtils.java

@@ -2,6 +2,8 @@ package com.mogo.module.common.utils;
 
 import static java.lang.Math.PI;
 
+import android.util.Pair;
+
 /**
  * author : donghongyu
  * e-mail : 1358506549@qq.com
@@ -11,6 +13,40 @@ import static java.lang.Math.PI;
  */
 public class DrivingDirectionUtils {
 
+    public static long getDegreeOfCar2Poi2(double x1, double y1, double x2, double y2, double x1_angle) {
+        Pair<Double, Double> newPoint = calculateNewPoint(x1, y1, x1_angle, 10);
+        if (newPoint != null) {
+            double angle = getAngle(x1, y1, newPoint.first, newPoint.second, x2, y2);
+            return Math.round(angle + 0.5);
+        }
+        return 0;
+    }
+
+    public static double distance(double x1, double y1, double x2, double y2) {
+        return Math.sqrt(Math.pow(x1 - x2, 2.0) + Math.pow(y1 - y2, 2.0));
+    }
+
+    private static double getAngle(double sx, double sy, double x1, double y1, double x2, double y2) {
+        x1 = x1 - sx;
+        y1 = y1 - sy;
+        x2 = x2 - sx;
+        y2 = y2 - sy;
+        double product = x1 * x2 + y1 * y2;
+        double radians = Math.acos(product / (Math.sqrt(Math.pow(x1, 2.0) + Math.pow(y1, 2.0)) * Math.sqrt(Math.pow(x2, 2.0) + Math.pow(y2, 2.0))));
+        return Math.toDegrees(radians);
+    }
+
+    public static Pair<Double, Double> calculateNewPoint(double x, double y, double angle, double distance) {
+        if (distance == 0) {
+            return null;
+        }
+        double radian = Math.toRadians(angle);
+        double radianCandle = Math.toRadians(90.0 - angle);
+        double nX = x + distance * Math.sin(radian) / 100000.0;
+        double nY = y + distance * Math.sin(radianCandle) / 100000.0;
+        return Pair.create(nX, nY);
+    }
+
     /**
      * 计算车辆行驶方向 与 poi点到车辆的连线 间的夹角
      *