`
yaodaqing
  • 浏览: 349500 次
  • 性别: Icon_minigender_1
  • 来自: 苏州
社区版块
存档分类
最新评论

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

阅读更多
//所有源代码,查看这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

  • 大小: 246.2 KB
分享到:
评论

相关推荐

    matlab求解模拟退火求解带有时间窗的多车辆路径优化问题

    `deal_vehicles_customer.m`则负责处理车辆与客户的分配,确保每个客户被且仅被一辆车服务。 在带有时间窗的VRPTW中,时间窗是指每个客户接收服务的允许时间段。规划的路线必须保证车辆在这个时间段内到达,否则会...

    基于单片机的智能小车.docx

    在循迹方案上,采用了红外反射对管作为传感器,通过检测跑道上的反射信号,确保小车保持在预设路径上行驶。两辆小车通过红外对管互相感知对方的位置,判断是否可以安全超车或转弯。 2. 系统硬件设计 电源部分设计...

    2019华为软成渝赛区初赛参赛源码+学习说明(第4,复赛19).zip

    通过dijkstra算法求出所有车辆的最短路径,根据每辆车走过的道路之间的关系(上下左右的数量),判断具体为哪一组 4. 调整两组车辆的发车间隔 ``` * 车辆实际出发时间 ```C++ 1. 将每组车辆按车辆行驶速度...

    悬挂运动控制系统(E题).doc

    【电动车跷跷板】则是一项关于动态平衡和自动化控制的挑战,要求参赛队伍设计一辆能够在不平衡状态下行驶的电动车。 1. **任务描述**:电动车需要在没有配重的情况下,从起点A行驶到中心C,再行驶到终点B,然后返回...

    第十三届Scratch编程比赛试卷 (2).docx

    - **运动控制**:第四题要求制作一辆能在赛道上行驶的小车,需要掌握Scratch中的运动指令和路径规划。 2. **高级功能**: - **路径记录与回放**:第五题中,小猫应能跟随鼠标移动,并记录和回放路径,这涉及到...

    专题资料(2021-2022年)GPS培训内容.doc

    对于需要重点监控的车辆,只需鼠标点击该车辆并进入相应界面,即可进行多辆车的同时监控,确保对关键车辆的实时跟踪。 4. **获取GPS故障不在线数据**: 在GPS管理分析系统的“报表分析1”中,选择相应的选项,...

    最全铁路司机考试试题题库汇总含答案.docx

    6. 信号表示器种类:包括道岔、脱轨、进路、发车、发车线路、调车及车挡表示器,辅助驾驶员判断行车安全。 7. 信号机位置:信号机通常设置在列车运行方向的左侧或所属线路的中心线上空,便于驾驶员观察。 8. 预告...

    2018-2019学年高一物理10月月考试题.pdf

    题目中提到,若两车均向东行驶,以其中一辆车为参考系,另一辆车是静止的;如果以第三辆车为参考系,两车可能同时静止或运动;在车内移动的人为参考系时,乙车状态可能变化;甲车刹车时,以乙车为参考系,甲车向西...

    fun1_退火算法_

    物流VRP(Vehicle Routing Problem)是经典的运营研究问题,旨在规划一辆或多辆车辆的路线,以满足客户的需求,同时最小化总行驶距离、时间或其他成本。这个问题通常具有大量的约束,包括车辆的容量限制、每个客户的...

    【优选】第十三届Scratch编程比赛试卷(最新整理).pdf

    - **运动控制**:制作一辆能在赛道上行驶的小车。这需要掌握Scratch中的移动和转向指令。 2. **创意设计与逻辑实现**: - **路径记录与回放**:小猫跟随鼠标移动,记录并播放运动轨迹。这涉及到Scratch中的坐标...

    大数据在物流行业的应用.pdf

    此外,大数据技术还可以应用于设备修理预测,例如美国 UPS 公司使用预测性分析来检测自己全美 60 000 辆车规模的车队,这样就能及时地进行防御性的修理;供应链协同管理,例如使用大数据技术可以迅速高效地发挥数据...

    电子设计大赛论文

    - **主要功能**:自动寻找行驶路线,确保小车按规定的路线行驶。 #### 三、关键技术点 ##### 1. 单片机控制 - **型号**:STC89C52 - **应用**:作为核心控制器,负责处理各种传感器数据并控制小车的速度和方向。 ...

    2019高中物理第一章运动的描述课时训练6用图象描述直线运动粤教版必修1

    虽然在t1时刻两车位移相同,但它们的速度和路程却不同,因为甲车经历了方向的改变,而乙车一直朝同一方向行驶。此外,平均速度是位移与时间的比值,所以在0到t1时间内,尽管它们的位移相同,但平均速度也相同。 在...

Global site tag (gtag.js) - Google Analytics