潘钊 / info_publish_v1 · Files

package com.bsth.util.geo;

import com.bsth.data.geo.GeoCacheData;
import com.bsth.data.gps.GpsCacheData;
import com.bsth.entity.*;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

import java.math.BigDecimal;
import java.math.RoundingMode;
import java.text.DecimalFormat;
import java.util.*;

/**
 * Created by panzhao on 2016/12/23.
 */
public class GeoUtils {

    private final static double EARTH_RADIUS = 6378137;

    static DecimalFormat df = new DecimalFormat("#.00");

    public static double getDistance(Point p1, Point p2) {
        double lng1 = getLoop(p1.getLon(), -180, 180), lat1 = getRange(
                p1.getLat(), -74, 74);
        double lng2 = getLoop(p2.getLon(), -180, 180), lat2 = getRange(
                p2.getLat(), -74, 74);

        double x1, x2, y1, y2;
        x1 = degreeToRad(lng1);
        y1 = degreeToRad(lat1);
        x2 = degreeToRad(lng2);
        y2 = degreeToRad(lat2);
        return EARTH_RADIUS
                * Math.acos((Math.sin(y1) * Math.sin(y2) + Math.cos(y1)
                * Math.cos(y2) * Math.cos(x2 - x1)));
    }

    private static double getLoop(double v, double a, double b) {
        while (v > b) {
            v -= b - a;
        }
        while (v < a) {
            v += b - a;
        }
        return v;
    }

    private static double getRange(double v, double a, double b) {
        v = Math.min(Math.max(v, a), b);
        return v;
    }

    private static double degreeToRad(double degree) {
        return Math.PI * degree / 180;
    }

    /**
     * 计算点 到 线的距离
     *
     * @param
     * @param p
     * @return
     */
    public static double getDistanceFromLine(Point s, Point e, Point p) {
        if (isPointInLine(p, s, e))
            return 0.5;

        double d1 = getDistance(s, p);
        double d2 = getDistance(p, e);
        double d3 = getDistance(s, e);
        double distance = 0;

        double alpha = Math.acos((d1 * d1 + d3 * d3 - d2 * d2) / (2 * d1 * d3));
        double beta = Math.acos((d2 * d2 + d3 * d3 - d1 * d1) / (2 * d2 * d3));

        if (alpha > Math.PI / 2) {
            distance = d1;
        } else if (beta > Math.PI / 2) {
            distance = d2;
        } else {
            distance = Math.sin(alpha) * d1;
        }
        return distance;
    }

    /**
     * 点 和 线是否垂直相交
     *
     * @param s
     * @param e
     * @param p
     * @return
     */
    public static boolean isVIntersect(Point s, Point e, Point p) {
        double d1 = getDistance(s, p);
        double d2 = getDistance(p, e);
        double d3 = getDistance(s, e);

        double alpha = Math.acos((d1 * d1 + d3 * d3 - d2 * d2) / (2 * d1 * d3));
        double beta = Math.acos((d2 * d2 + d3 * d3 - d1 * d1) / (2 * d2 * d3));

        if (alpha <= Math.PI / 2 && beta <= Math.PI / 2) {
            return true;
        }
        return false;
    }

    static double TO_RAD = Math.PI / 180;

    private static double toRad(double v) {
        return v * TO_RAD;
    }

    private static double toFixed(double v, int precision) {
        return new BigDecimal(v).setScale(precision, RoundingMode.HALF_EVEN).doubleValue();
    }

    /**
     * 点是否在线上
     *
     * @param p
     * @param s
     * @param e
     * @return
     */
    public static boolean isPointInLine(Point p, Point s, Point e) {
        double d1 = getDistance(s, p);
        double d2 = getDistance(e, p);

        if (d1 < 2 || d2 < 2)
            return true;
        else
            return Math.abs(d1 + d2 - getDistance(s, e)) < 5;
    }

    static Logger logger = LoggerFactory.getLogger(GeoUtils.class);

    /**
     * 计算2条直线的最短距离
     *
     * @param p1
     * @param p2
     * @param p3
     * @param p4
     * @return
     */
    public static double getDistanceLineToLine(Point p1, Point p2, Point p3, Point p4) {
        double distance;
        double x1 = p1.getLat(); //A点坐标（x1,y1,z1）
        double y1 = p1.getLon();
        double z1 = 0;
        double x2 = p2.getLat(); //B点坐标（x2,y2,z2）
        double y2 = p2.getLon();
        double z2 = 0;
        double x3 = p3.getLat(); //C点坐标（x3,y3,z3）
        double y3 = p3.getLon();
        double z3 = 0;
        double x4 = p4.getLat(); //D点坐标（x4,y4,z4）
        double y4 = p4.getLon();
        double z4 = 0;

        double a = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1) + (z2 - z1) * (z2 - z1);
        double b = -((x2 - x1) * (x4 - x3) + (y2 - y1) * (y4 - y3) + (z2 - z1) * (z4 - z3));
        double c = -((x1 - x2) * (x1 - x3) + (y1 - y2) * (y1 - y3) + (z1 - z2) * (z1 - z3));

        double d = -((x2 - x1) * (x4 - x3) + (y2 - y1) * (y4 - y3) + (z2 - z1) * (z4 - z3));
        double e = (x4 - x3) * (x4 - x3) + (y4 - y3) * (y4 - y3) + (z4 - z3) * (z4 - z3);
        double f = -((x1 - x3) * (x4 - x3) + (y1 - y3) * (y4 - y3) + (z1 - z3) * (z4 - z3));

        //平行
        if ((a * e - b * d) == 0 && (b * d - a * e) == 0) {
            double d1 = getDistance(p1, p3);
            double d2 = getDistance(p1, p4);
            distance = (d1 < d2) ? d1 : d2;
            return distance;
        }

        double s = (b * f - e * c) / (a * e - b * d);
        double t = (a * f - d * c) / (b * d - a * e);

        //说明P点落在线段AB上,Q点落在线段CD上
        if (0 <= s && s <= 1 && 0 <= t && t <= 1) {
            //2条线段的公垂线段PQ;
            //P点坐标
            double X = x1 + s * (x2 - x1);
            double Y = y1 + s * (y2 - y1);
            double Z = z1 + s * (z2 - z1);
            //Q点坐标
            double U = x3 + t * (x4 - x3);
            double V = y3 + t * (y4 - y3);
            double W = z3 + t * (z4 - z3);

            Point p = new Point(Y, X);
            Point q = new Point(V, U);
            distance = getDistance(p, q);
        } else {
            double d1 = getDistanceFromLine(p3, p4, p1);
            double d2 = getDistanceFromLine(p3, p4, p2);
            double d3 = getDistanceFromLine(p1, p2, p3);
            double d4 = getDistanceFromLine(p1, p2, p4);
            distance = (d1 < d2) ? d1 : d2;
            distance = (distance < d3) ? distance : d3;
            distance = (distance < d4) ? distance : d4;
        }

        return distance;
    }

    /**
     * 计算2条直线的交叉点
     *
     * @param p1
     * @param p2
     * @param p3
     * @param p4
     * @return
     */
    public static Point getLineToLineIntersection(Point p1, Point p2, Point p3, Point p4, double dt) {
        double x1 = p1.getLat(); //A点坐标（x1,y1,z1）
        double y1 = p1.getLon();
        double z1 = 0;
        double x2 = p2.getLat(); //B点坐标（x2,y2,z2）
        double y2 = p2.getLon();
        double z2 = 0;
        double x3 = p3.getLat(); //C点坐标（x3,y3,z3）
        double y3 = p3.getLon();
        double z3 = 0;
        double x4 = p4.getLat(); //D点坐标（x4,y4,z4）
        double y4 = p4.getLon();
        double z4 = 0;

        double a = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1) + (z2 - z1) * (z2 - z1);
        double b = -((x2 - x1) * (x4 - x3) + (y2 - y1) * (y4 - y3) + (z2 - z1) * (z4 - z3));
        double c = -((x1 - x2) * (x1 - x3) + (y1 - y2) * (y1 - y3) + (z1 - z2) * (z1 - z3));

        double d = -((x2 - x1) * (x4 - x3) + (y2 - y1) * (y4 - y3) + (z2 - z1) * (z4 - z3));
        double e = (x4 - x3) * (x4 - x3) + (y4 - y3) * (y4 - y3) + (z4 - z3) * (z4 - z3);
        double f = -((x1 - x3) * (x4 - x3) + (y1 - y3) * (y4 - y3) + (z1 - z3) * (z4 - z3));

        //平行
        if ((a * e - b * d) == 0 && (b * d - a * e) == 0) {
            return null;
        }

        double s = (b * f - e * c) / (a * e - b * d);
        double t = (a * f - d * c) / (b * d - a * e);

        //说明P点落在线段AB上,Q点落在线段CD上
        if (0 <= s && s <= 1 && 0 <= t && t <= 1) {
            //2条线段的公垂线段PQ;
            //P点坐标
            double X = x1 + s * (x2 - x1);
            double Y = y1 + s * (y2 - y1);
            double Z = z1 + s * (z2 - z1);
            //Q点坐标
            double U = x3 + t * (x4 - x3);
            double V = y3 + t * (y4 - y3);
            double W = z3 + t * (z4 - z3);

            if (Y == V && X == U) {
                return new Point(Y, X);
            }
        } else {
            double distance;
            double d1 = getDistanceFromLine(p3, p4, p1);
            double d2 = getDistanceFromLine(p3, p4, p2);
            double d3 = getDistanceFromLine(p1, p2, p3);
            double d4 = getDistanceFromLine(p1, p2, p4);

            distance = (d1 < d2) ? d1 : d2;
            if (distance < dt)
                return p1;

            if (d3 < distance && d3 < dt)
                return p3;

            if (d4 < distance && d4 < dt)
                return p4;
        }

        return null;
    }


    /**
     * 计算点 到 线的垂直交点
     *
     * @param lp1
     * @param lp2
     * @param p
     * @return
     */
    public static Point perpendularPoint(Point lp1, Point lp2, Point p) {
        double a = lp1.getLat() - lp2.getLat(), b = lp2.getLon() - lp1.getLon(), c = lp1.getLon() * lp2.getLat() - lp2.getLon() * lp1.getLat();
        double lon = (Math.pow(b, 2) * p.getLon() - a * b * p.getLat() - a * c) / (Math.pow(a, 2) + Math.pow(b, 2));
        double lat = (Math.pow(a, 2) * p.getLat() - a * b * p.getLon() - b * c) / (Math.pow(a, 2) + Math.pow(b, 2));

        return new Point(lon, lat);
    }

    public static boolean isPointInRect(Point point, Bounds bounds) {
        Point sw = bounds.getSouthWest(); // 西南脚点
        Point ne = bounds.getNorthEast(); // 东北脚点
        return (point.getLon() >= sw.getLon() && point.getLon() <= ne.getLon()
                && point.getLat() >= sw.getLat() && point.getLat() <= ne
                .getLat());
    }

    public static boolean isPointInPolygon(Point point, Polygon polygon) {
        Bounds polygonBounds = polygon.getBounds();
        if (!isPointInRect(point, polygonBounds)) {
            return false;
        }

        List<Point> pts = polygon.getPoints();// 获取多边形点

        // 下述代码来源：http://paulbourke.net/geometry/insidepoly/，进行了部分修改
        // 基本思想是利用射线法，计算射线与多边形各边的交点，如果是偶数，则点在多边形外，否则
        // 在多边形内。还会考虑一些特殊情况，如点在多边形顶点上，点在多边形边上等特殊情况。

        int N = pts.size();
        boolean boundOrVertex = true; // 如果点位于多边形的顶点或边上，也算做点在多边形内，直接返回true
        int intersectCount = 0;// cross points count of x
        double precision = 2e-10; // 浮点类型计算时候与0比较时候的容差
        Point p1, p2;// neighbour bound vertices
        Point p = point; // 测试点

        p1 = pts.get(0);// left vertex
        for (int i = 1; i <= N; ++i) {// check all rays
            if (p.equals(p1)) {
                return boundOrVertex;// p is an vertex
            }

            p2 = pts.get(i % N);// right vertex
            if (p.getLat() < Math.min(p1.getLat(), p2.getLat())
                    || p.getLat() > Math.max(p1.getLat(), p2.getLat())) {// ray
                // is
                // outside
                // of
                // our
                // interests
                p1 = p2;
                continue;// next ray left point
            }

            if (p.getLat() > Math.min(p1.getLat(), p2.getLat())
                    && p.getLat() < Math.max(p1.getLat(), p2.getLat())) {// ray
                // is
                // crossing
                // over
                // by
                // the
                // algorithm
                // (common
                // part
                // of)
                if (p.getLon() <= Math.max(p1.getLon(), p2.getLon())) {// x is
                    // before
                    // of
                    // ray
                    if (p1.getLat() == p2.getLat()
                            && p.getLon() >= Math.min(p1.getLon(), p2.getLon())) {// overlies
                        // on
                        // a
                        // horizontal
                        // ray
                        return boundOrVertex;
                    }

                    if (p1.getLon() == p2.getLon()) {// ray is vertical
                        if (p1.getLon() == p.getLon()) {// overlies on a
                            // vertical ray
                            return boundOrVertex;
                        } else {// before ray
                            ++intersectCount;
                        }
                    } else {// cross point on the left side
                        double xinters = (p.getLat() - p1.getLat())
                                * (p2.getLon() - p1.getLon())
                                / (p2.getLat() - p1.getLat()) + p1.getLon();// cross
                        // point
                        // of
                        // lng
                        if (Math.abs(p.getLon() - xinters) < precision) {// overlies
                            // on
                            // a
                            // ray
                            return boundOrVertex;
                        }

                        if (p.getLon() < xinters) {// before ray
                            ++intersectCount;
                        }
                    }
                }
            } else {// special case when ray is crossing through the vertex
                if (p.getLat() == p2.getLat() && p.getLon() <= p2.getLon()) {// p
                    // crossing
                    // over
                    // p2
                    Point p3 = pts.get((i + 1) % N); // next vertex
                    if (p.getLat() >= Math.min(p1.getLat(), p3.getLat())
                            && p.getLat() <= Math.max(p1.getLat(), p3.getLat())) {// p.lat
                        // lies
                        // between
                        // p1.lat
                        // &
                        // p3.lat
                        ++intersectCount;
                    } else {
                        intersectCount += 2;
                    }
                }
            }
            p1 = p2;// next ray left point
        }

        if (intersectCount % 2 == 0) {// 偶数在多边形外
            return false;
        } else { // 奇数在多边形内
            return true;
        }
    }

    /**
     * gps是否在路由上的某个站内
     *
     * @param gps
     * @param srs
     * @return
     */
    public static ArriveStationDetail gpsInStation(GpsEntity gps, List<StationRoute> srs) {
        List<ArriveStationDetail> rs = new ArrayList();

        Point point = gps.getPoint();
        double distance;
        StationRoute sr;
        for (int i = 0, len = srs.size(); i < len; i++) {
            sr = srs.get(i);

            if (sr.getPolygon() == null) {
                //圆形
                distance = getDistance(sr.getPoint(), point);

                if (distance > sr.getRadius())
                    continue;

                rs.add(new ArriveStationDetail(sr, distance, i + 1));
            } else if (isPointInPolygon(point, sr.getPolygon())) {
                distance = getDistance(sr.getPoint(), point);
                rs.add(new ArriveStationDetail(sr, distance, i + 1));
            }
        }

        if (rs.size() == 0)
            return null;
        else
            return findNearStation(rs, gps);
    }

    private static ArriveStationDetail findNearStation(List<ArriveStationDetail> rs, GpsEntity gps) {
        if (rs.size() > 1) {
            //按路由顺序排序
            Collections.sort(rs, new Comparator<ArriveStationDetail>() {
                @Override
                public int compare(ArriveStationDetail s1, ArriveStationDetail s2) {
                    return s1.getI() - s2.getI();
                }
            });

            //上一个进的站点
            StationRoute ps = GpsCacheData.prevInStation(gps);
            if (null != ps) {
                for (ArriveStationDetail s : rs) {
                    if (s.getSr().getSerialNo() >= ps.getSerialNo())
                        return s;
                }
            }
        }
        return rs.get(0);
    }

    public static Polygon parsePolygon(String polygonStr) {
        String[] arr = polygonStr.substring(9, polygonStr.length() - 2).split(","), temps;

        List<Point> cds = new ArrayList<>(arr.length);
        int len = arr.length;
        for (int i = 0; i < len; i++) {
            temps = arr[i].split(" ");
            cds.add(new Point(Float.parseFloat(temps[0]), Float.parseFloat(temps[1])));
        }
        return new Polygon(cds);
    }

    /**
     * gps 是否在某个停车场内
     *
     * @param gps
     * @return
     */
    public static String gpsInCarpark(GpsEntity gps) {
        Point point = gps.getPoint();
        Collection<CarPark> cps = GeoCacheData.carPacks();

        for (CarPark cp : cps) {
            if (isPointInPolygon(point, cp.getPolygon()))
                return cp.getCode();
        }
        return null;
    }

    /**
     * 点 和 线的最近相交点 和 索引
     *
     * @param p
     * @param coords
     * @return
     */
    public static IntersePoint pointToLineNearPoint(Point p, List<Point> coords) {
        IntersePoint sri = new IntersePoint();

        Point s, e, intersection;
        double d1, d2, d3, alpha, beta;

        double distance;
        int index;
        for (int i = 1, len = coords.size(); i < len; i++) {
            s = coords.get(i - 1);
            e = coords.get(i);

            d1 = getDistance(s, p);
            d2 = getDistance(p, e);
            d3 = getDistance(s, e);

            alpha = Math.acos((d1 * d1 + d3 * d3 - d2 * d2) / (2 * d1 * d3));
            beta = Math.acos((d2 * d2 + d3 * d3 - d1 * d1) / (2 * d2 * d3));


            if (d3 < 0.5 || Double.isNaN(alpha) || Double.isNaN(beta))
                continue;

            index = i;
            if (alpha > Math.PI / 2) {
                distance = d1;
                intersection = s;
            } else if (beta > Math.PI / 2) {
                distance = d2;
                intersection = e;
            } else {
                distance = Math.sin(alpha) * d1;
                intersection = perpendularPoint(s, e, p);
            }

            if (null == sri.getPoint() || sri.getDistance() > distance) {

                sri.setIndex(index);
                sri.setDistance(distance);
                sri.setPoint(intersection);
            }
        }
        return sri;
    }

    public static double getDistance(List<Point> ps) {
        double sum = 0;

        for (int i = 0, len = ps.size() - 1; i < len; i++) {
            sum += GeoUtils.getDistance(ps.get(i), ps.get(i + 1));
        }
        return Double.isNaN(sum) ? 0 : Double.parseDouble(df.format(sum));
    }

    /**
     * 计算2个gps的距离（线路上站点间的信号）
     *
     * @param firts
     * @param gps
     * @return
     */
    public static double getDistance(GpsEntity firts, GpsEntity gps) {
        List<Point> ps = GeoCacheData.getFullSection(gps.getLineId(), gps.getUpDown());

        IntersePoint ip1 = pointToLineNearPoint(firts.getPoint(), ps), ip2 = pointToLineNearPoint(gps.getPoint(), ps);

        int s = ip1.getIndex(), e = ip2.getIndex();
        if (e < s)
            return Double.NaN;

        double sum = 0, distance;
        for (; s < e; s++) {
            distance = getDistance(ps.get(s), ps.get(s + 1));

            if (!Double.isNaN(distance))
                sum += distance;
        }

        if (!Double.isNaN(sum))
            sum = Double.parseDouble(df.format(sum));
        return sum;
    }
}