判断一辆车是否在规定的路径上行驶

//所有源代码，查看这http://www.docin.com/p1-342123847.html
package com.wynlink.util;

import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

import com.wynlink.bean.GPSLine;
import com.wynlink.bean.PlanPath;

public class GPSRoudUtils {

	public static final double wd = 110.9463; //纬度1=110.9463km
	public static final double jd = 95.24952; //经度1=95.24952km
	public static final double roudWidth = 0.02;	//这个值除以1.414得到半边路宽km，这个值比整个路宽稍小些
	
	
	public static void main(String[] args) {
		
		PlanPath p1 = new PlanPath();
		p1.setLat("31.281903");
		p1.setLng("120.735247");
		p1.setOrdernum(1);
		p1.setOrderbefore(11);
		p1.setOrderafter(4);
		
		PlanPath p4 = new PlanPath();
		p4.setLat("31.281961");
		p4.setLng("120.736811");
		p4.setOrdernum(4);
		p4.setOrderbefore(1);
		p4.setOrderafter(7);
		
		PlanPath p7 = new PlanPath();
		p7.setLat("31.282028");
		p7.setLng("120.738622");
		p7.setOrdernum(7);
		p7.setOrderbefore(4);
		p7.setOrderafter(10);
		
		PlanPath p10 = new PlanPath();
		p10.setLat("31.280511");
		p10.setLng("120.738878");
		p10.setOrdernum(10);
		p10.setOrderbefore(7);
		p10.setOrderafter(11);
		
		PlanPath p11 = new PlanPath();
		p11.setLat("31.279225");
		p11.setLng("120.736078");
		p11.setOrdernum(11);
		p11.setOrderbefore(10);
		p11.setOrderafter(1);
		
		List<PlanPath> list = new ArrayList<PlanPath>();
		list.add(p1);
		list.add(p4);
		list.add(p7);
		list.add(p10);
		list.add(p11);
		
		//double cx = 31.282056;
		//double cy = 120.736442;
		
		//double cx = 31.281961;
		//double cy = 120.736811;
		
		double cx = 31.283944;
		double cy = 120.735447;
		
		System.out.println(judgeCar(cx,cy,list));
	}
	
	/**
	 * 判断车，是否在规划路径上
	 */
	@SuppressWarnings("unchecked")
	public static boolean judgeCar(double cx,double cy,List<PlanPath> list){
		//得到多个值得数组
		List listd = new ArrayList();
		Map map = new HashMap();
		for(PlanPath pp : list){
			double d = getLength(cx,cy,Double.parseDouble(pp.getLat()),Double.parseDouble(pp.getLng()));
			listd.add(d);
			map.put(d, pp);
		}
		
		//对数组进行排序
		Collections.sort(listd);
		
		//得到最小距离的值
		//boolean bpPath = (Boolean)listd.get(0);
		PlanPath pPath = (PlanPath)map.get(listd.get(0));
		
		//获取当前坐标点前一个点和后一个点
		PlanPath pPathBefore = new PlanPath();	//前一个点
		PlanPath pPathAfter = new PlanPath();	//后一个点
		
		//pPath.getOrderbefore()
		
		for(PlanPath pp : list){
			if(pPath.getOrderbefore() == pp.getOrdernum()){
				pPathBefore = pp;
			}
			if(pPath.getOrderafter() == pp.getOrdernum()){
				pPathAfter = pp;
			}
		}
		
		boolean bb1 = getInOrOut(Double.parseDouble(pPathBefore.getLat()),Double.parseDouble(pPathBefore.getLng()),
				Double.parseDouble(pPath.getLat()),Double.parseDouble(pPath.getLng()),cx,cy);
		if(bb1){	//等于true则表示在第一个矩形中，如果等于false则表示不在第一个矩形中
			return true;
		}
		boolean bb2 = getInOrOut(Double.parseDouble(pPath.getLat()),Double.parseDouble(pPath.getLng()),
				Double.parseDouble(pPathAfter.getLat()),Double.parseDouble(pPathAfter.getLng()),cx,cy);
		if(bb2){	//等于true则表示在第二个矩形中，如果等于false则表示不在第二个矩形中
			return true;
		} 
		
		return false;
	}
	
	/**
	 * 算出两个点之间的距离（车辆的坐标，相对于所有定义的每个点的坐标距离）
	 * 	实际距离如果要在地图上显示，则要开根号
	 */
	public static double getLength(double a1,double a2,double b1,double b2){
		double c1 = (a1-b1)*(a1-b1)*wd*wd;	//纬度1=110.9463km
		double c2 = (a2-b2)*(a2-b2)*jd*jd;	//经度1=95.24952km
		double c3 = c1+c2;
		return c3;
	}
	
	/**
	 * 判断此点是否在六边形内，在则返回true，不在则返回false
	 * @param a1x	前一个X轴的坐标点			离车最短距离的X轴坐标点
	 * @param a1y	前一个Y轴的坐标点			离车最短距离的Y轴坐标点
	 * @param a4x	离车最短距离的X轴坐标点		后一个X轴的坐标点
	 * @param a4y	离车最短距离的Y轴坐标点		后一个Y轴的坐标点
	 * @param cx	汽车的X轴坐标点
	 * @param cy	汽车的Y轴坐标点
	 * @return
	 */
	public static boolean getInOrOut(double a1x,double a1y,double a4x,double a4y,double cx,double cy){
		double a2x = a1x+roudWidth/wd;
		double a2y = a1y+roudWidth/jd;
		
		double a3x = a1x-roudWidth/wd;
		double a3y = a1y-roudWidth/jd;
		
		double a5x = a4x+roudWidth/wd;
		double a5y = a4y+roudWidth/jd;
		
		double a6x = a4x-roudWidth/wd;
		double a6y = a4y-roudWidth/jd;
		
		//左右两条平行线
		GPSLine l1z = getLine(a2x,a2y,a3x,a3y);
		GPSLine l1y = getLine(a5x,a5y,a6x,a6y);
		
		double bzy = cy -( l1z.getK() * cx);	//车辆的竖线
		
		//上下两条平行线
		GPSLine l2s = getLine(a2x,a2y,a5x,a5y);
		GPSLine l2x = getLine(a3x,a3y,a6x,a6y);
		
		double bsx = cy - (l2s.getK() * cx);	//车辆的横线
		
		double czy = (bzy-l1z.getB())* (bzy-l1y.getB());
		double csx = (bsx-l2s.getB())* (bsx-l2x.getB());
		
		//同时小于零时，表示在两对平行线中间，表示在这个区域范围内
		if(czy<=0 && csx<=0){
			return true;	//表示在此平行四边形中
		}
		return false;	//表示不在此平行四边形中
	}

	/**
	 * 根据两个坐标点得到一条线
	 */
	public static GPSLine getLine(double x1,double y1,double x2,double y2){
		double k = (y2-y1)/(x2-x1);
		double b = y1-k*x1;
		GPSLine gps = new GPSLine();
		gps.setK(k);
		gps.setB(b);
		return gps;
	}
	
}
//所有源代码，查看这http://www.docin.com/p1-342123847.html