判断当前位置是否偏移指定路线

import java.awt.geom.Point2D;

/**
    * 计算点到线段的距离
    *
    * @param x： 需要判断点的横坐标
    * @param y 需要
    * @param x1
    * @param y1
    * @param x2
    * @param y2
    * @return
    */
   public static double PointToSegDist(double x, double y, double x1, double y1, double x2, double y2) {
      double cross = (x2 - x1) * (x - x1) + (y2 - y1) * (y - y1);
      if (cross <= 0) {
         return Math.sqrt((x - x1) * (x - x1) + (y - y1) * (y - y1));
      }

      double d2 = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
      if (cross >= d2) {
         return Math.sqrt((x - x2) * (x - x2) + (y - y2) * (y - y2));
      }

      double r = cross / d2;
      double px = x1 + (x2 - x1) * r;
      double py = y1 + (y2 - y1) * r;
      return Math.sqrt((x - px) * (x - px) + (py - y1) * (py - y1));
   }

   /**
    * 下面是判断路线偏移方法
    * @param pointSet queue:指定路线数据
    * @param locationPoint 当前要判断的经纬度坐标
    * @param distanceTolerate 判定偏移的距离
    * @return ：true 表示偏移，false 没有偏移
    */



   public static boolean isRouteOffset(Queue<Point2D.Double> pointSet, Point2D.Double locationPoint,
                              double distanceTolerate) {

      /**
       经纬度距离换算
       a）在纬度相等的情况下：
       经度每隔0.00001度，距离相差约1米；每隔0.0001度，距离相差约10米；每隔0.001度，距离相差约100米；每隔0.01度，距离相差约1000米；
       每隔0.1度，距离相差约10000米。
       b）在经度相等的情况下：
       纬度每隔0.00001度，距离相差约1.1米；每隔0.0001度，距离相差约11米；每隔0.001度，距离相差约111米；每隔0.01度，距离相差约1113米；
       每隔0.1度，距离相差约11132米。
       */
      //    这里将距离准换为与经纬的对应关系，粗略的转化

      Double tolerate = distanceTolerate * 0.00001;

      Point2D.Double leftPoint = null ;
      Point2D.Double rightPoint = null;
      int i = 0;
      while(!pointSet.isEmpty()) {
         if(i ==0){
            leftPoint = pointSet.poll();
            i++;
            continue;
         }
         rightPoint = pointSet.poll();
         if (abs(locationPoint.x - leftPoint.x) <= tolerate
               && abs(locationPoint.y - leftPoint.y) < distanceTolerate) {
            System.out.printf("找到所在的路段，循环%d次",i);
            return false;
         } else if (abs(locationPoint.x - rightPoint.x) <= tolerate
               && abs(locationPoint.y - rightPoint.y) < distanceTolerate) {
            System.out.printf("找到所在的路段，循环%d次",i);
            return false;
         } else if (PointToSegDist(locationPoint.x, locationPoint.y, leftPoint.x, leftPoint.y, rightPoint.x,
               rightPoint.y) < tolerate) {
            System.out.printf("找到所在的路段，循环%d次",i);
            return false;
         }

         i++;
         leftPoint = rightPoint;

      }

      System.out.printf("共循环了%d次,没有找到匹配到所在的路线，我尽力了！",i);

      return true;
   }


}