Context Navigation

← Previous Revision
Latest Revision
Next Revision →
Blame
Revision Log

CarInstance.java @ 31

Last change on this file since 31 was 31, checked in by (none), 14 years ago

File size: 52.4 KB

Line
1	/************************************************************************************
2	* Copyright (C) 2008 by Politehnica University of Bucharest and Rutgers University
3	* All rights reserved.
4	* Refer to LICENSE for terms and conditions of use.
5	***********************************************************************************/
6	package vnsim.vehicular.simulator;
7
8	import java.io.File;
9	import java.io.FileOutputStream;
10	import java.io.PrintWriter;
11	import java.util.ArrayList;
12
13	import vnsim.applications.adaptiveTL.WirelessTrafficLight;
14	import vnsim.applications.trafficview.SimulatedCarInfo;
15	import vnsim.core.CarInfo;
16	import vnsim.core.Engine;
17	import vnsim.map.object.Globals;
18	import vnsim.map.object.PeanoKey;
19	import vnsim.map.object.Point;
20	import vnsim.map.object.Road;
21	import vnsim.vehicular.emissions.EmissionsResults;
22	import vnsim.vehicular.emissions.EmissionsUtil;
23	import vnsim.vehicular.routePlan.RouteComputingUtils;
24	import vnsim.vehicular.routePlan.RoutingConstants;
25	import vnsim.vehicular.simulator.intersections.DirectedRoadSegment;
26	import vnsim.vehicular.simulator.intersections.Intersection;
27	import vnsim.vehicular.simulator.intersections.IntersectionWithTrafficLights;
28	import vnsim.vehicular.simulator.intersections.IntersectionWithoutTrafficLights;
29
30	public class CarInstance extends CarInfo implements Comparable<CarInstance> {
31
32	protected SimulatedCarInfo simulatedCarInfo;
33
34	// Routing
35	public static PrintWriter logWriter;
36
37	public static RouteComputingUtils routeComputer = new RouteComputingUtils();
38
39	public long timeOnSegment = 0L;
40
41	public double timeOnSegment1 = 0;
42	public double distanceOnSegment = 0;
43
44	private double avgSpeed = -1.0;
45	private byte avgSpeedStatus = 0;
46	private static byte SPEED_RESET = 0;
47	private static byte SPEED_COMPUTED = 1;
48	private static byte SPEED_DELAY_RESET = 2;
49	// nb of cycles that this car has to wait at a traffic light
50	private int nTLCycles = 0; // traffic lights need to broadcast
51	// PROT_TL_FEEDBACK messages
52
53	public Location source, destination;
54
55	public int currentRoadState = 0;
56
57	public long startTime, stopTime;
58
59	public int toLateFailCount = 0, noRouteFailCount = 0, sentMsgCount = 0, receivedMsgCount = 0, emptyMsgCount = 0,
60	inccorectReplyCount = 0, noChangeNeededCount = 0, routeComputingFailCount = 0, routeChangesCount = 0;
61
62	public int routingType; // 0 _ShortestPath without dynamic search
63
64	// 1 _ShortestPath with dynamic search
65	// 2 _BestPath without dynamic search
66	// 3 _BestPath with dynamic search
67	public int queryTime;
68
69	static {
70	File log = new File(Globals.routePlanConstants.fisOutCar);
71	try {
72	log.delete();
73	log.createNewFile();
74	logWriter = new PrintWriter(new FileOutputStream(log));
75	} catch (Exception ex) {
76	ex.printStackTrace();
77	}
78	}
79
80	public int ID; // unique id;
81
82	public int index; // the index of "this" object in the "carsOnThisRoad"
83	// vector from the corresponding "Road" object;
84
85	// stare
86	// public int roadIndex; //the road it is on
87	// public int pointIndex;
88	// public double offset; //offset to the point, along the road (km!)
89	// //offset>0.0
90	// public int direction; //1=along the road; 0=reverse
91	// public int lane; //the lane
92	//
93	public boolean finished = false;
94	//
95	// public double speed; //km/h
96
97	// personality
98	public Personality p;
99
100	// route
101	public RouteSegment[] route = null; // the route segments MUST NOT include
102	// intersections; if they do, they should first be divided until
103	// they include no intersections
104	public int routeIndex = -1;
105
106	public void setRoute(java.util.ArrayList<RouteSegment> route) {
107
108	if (route == null \|\| route.size() == 0) {
109	System.out.println("Error... route is null");
110	return;
111	}
112	// to be called at the beginning
113	this.route = new RouteSegment[route.size()];
114	for (int i = 0; i < route.size(); i++) {
115	this.route[i] = route.get(i);
116	}
117	routeIndex = 0;
118
119	// also adjust the direction
120	RouteSegment first = route.get(0);
121	if (first.roadIndex != roadIdx) {
122	// System.out.println("ERROR! First route segment is not on current
123	// road!");
124	// the vehicle was probably placed in an intersection and the first
125	// route segment is not on its current road; adjust the current road
126	this.roadIdx = first.roadIndex;
127	this.pointIdx = first.pt1;
128	if (first.pt2 < pointIdx)
129	direction = 0;
130	else
131	direction = 1;
132	return;
133	}
134	if (first.pt2 < pointIdx)
135	direction = 0;
136	else
137	direction = 1;
138	}
139
140	// public void setRoute(RouteSegment[] route) {
141	// this.route=route;
142	// routeIndex=0;
143	// }
144
145	// values obtained from the "Personality" object
146	private double accelFreeDriving;
147	private double wantedSpeed;
148
149	private double maxWantedSpeed;
150
151	private boolean havePriority = false;
152	public Intersection intersection; // the following intersection
153
154	private double inflDist;
155	private double inflDistLaneChange;
156
157	private double maxSpeed;
158	private int nextIntersectionPointIndex; // the point index (on "roadIndex"
159	// road)
160	// where the next intersection is
161
162	private boolean wasApproaching = false;
163	private int nIterationsApproaching = 0;
164
165	private boolean influenced = false;
166	public boolean influencedLaneChange = false;
167
168	private ArrayList<Byte> desiredLanes; // which are the lanes on which
169	// the vehicle must be before the end of its current route segment?
170	public boolean wellPositioned = false; // am I well positioned for the
171	// turn?
172
173	// only valid if the intersection ahead has no traffic lights
174	private ArrayList<DirectedRoadSegment> yieldFor;
175
176	public boolean influencedByTrafficLight = false;
177	private long timeStartInfluenced = 0;
178	private long timeExpectedInfluenced = 0;
179	private long timeRealInfluenced = 0;
180	public int controlDelay;
181	public int totalControlDelay = 0;
182	public boolean startMonitor = false;
183	public boolean endMonitor = false;
184	public double startMonitorDistance;
185	public Intersection lastIntersection;
186	public int lastIntersectionPointIndex;
187	double previousSpeed = 0.0;
188
189	public EmissionsResults em = new EmissionsResults();
190	public boolean turningLeft = false;
191
192	public boolean demanded = false;
193	public boolean queued = false;
194
195	// get the average speed on the segment
196	// return -1 if avgSpeed is not computed or if this function is
197	// called more than kMaxAttempts after the avgSpeed is computed
198	public double getAvgSpeed() {
199	if (avgSpeedStatus != SPEED_DELAY_RESET) {
200	return -1.0;
201	}
202	avgSpeedStatus = SPEED_RESET;
203	return avgSpeed;
204	}
205
206	// get the nb of red Cycles this car has waited at a trafficLight
207	public int getTLCycles() {
208	int nTL = nTLCycles;
209	nTLCycles = 0;
210	return nTL;
211	}
212
213	// get the segmentIndex on which avgSpeed was computed
214	public int getSegmentIndex()
215	{
216	Intersection it;
217	RouteSegment rs;
218	if (avgSpeedStatus == SPEED_DELAY_RESET) {
219	// car has already changed the segment
220	it = lastIntersection;
221	rs = route[routeIndex - 1];
222	} else {
223	it = intersection;
224	rs = route[routeIndex];
225	}
226	int index = -1;
227	DirectedRoadSegment drs;
228	for (int i = 0; i < it.segments.size(); i++) {
229	drs = it.segments.get(i);
230	if (drs.roadIndex == rs.roadIndex) {
231	if (drs.direction && drs.pointIndex == rs.pt1) {
232	index = i;
233	break;
234	}
235	if (!drs.direction && drs.pointIndex == rs.pt2) {
236	index = i;
237	break;
238	}
239	}
240	}
241	return index;
242	}
243
244	public void startOnRoad() {
245	// method to be called once, just after the vehicle gets on a new road
246
247	canSignal = false;
248	turningLeft = false;
249	demanded = false;
250	queued = false;
251
252	if (Engine.simulationType == RoutingConstants.DYNAMIC_CITY_ROUTE) {
253	if (avgSpeedStatus == SPEED_RESET && timeOnSegment1 != 0) {
254	avgSpeed = distanceOnSegment * 3600 / timeOnSegment1;
255	avgSpeedStatus = SPEED_DELAY_RESET;
256	}
257	}
258	distanceOnSegment = 0;
259	timeOnSegment1 = 0;
260
261	if (intersection != null) {
262	lastIntersection = intersection;
263	lastIntersectionPointIndex = pointIdx;
264	if (Globals.monitorWTL != null && intersection.equals(Globals.monitorWTL)) {
265	Globals.volume++;
266	}
267
268	if (Globals.engine.startedWithGUI && Globals.demo.mv.currentCar != null
269	&& simulatedCarInfo.equals(Globals.demo.mv.currentCar)) {
270	Globals.demo.st.setSemTime(-1, -1);
271	}
272	}
273	if (influencedByTrafficLight) {
274	influencedByTrafficLight = false;
275	timeRealInfluenced = (Globals.engine.crtTime - timeStartInfluenced) / Globals.executionFPS;
276	controlDelay = (int) (timeRealInfluenced - timeExpectedInfluenced);
277	totalControlDelay += controlDelay;
278	// System.out.println("ACTUAL TIME:"+timeRealInfluenced+" [secs];
279	// EXPECTED="+timeExpectedInfluenced+" [secs];
280	// DELAY="+(timeRealInfluenced-timeExpectedInfluenced));
281	}
282
283	accelFreeDriving = p.getAccelerationFreeDriving();
284	maxWantedSpeed = wantedSpeed = p.getWantedSpeed(roadIdx);
285	influenced = false;
286	influencedLaneChange = false;
287	wellPositioned = false;
288
289	// obtain the intersection object corresponding to the next intersection
290	// it has to pass through; also set "nextIntersectionPointIndex"
291	// System.out.println("Setting intersection!Pt="+pointIdx);
292
293	if (route != null && routeIndex == route.length - 1) {
294	// I am entering the last route segment
295	// Simulate a special type of intersection, where I must stop
296	// (it's the final destination of this vehicle)
297
298	intersection = new IntersectionWithoutTrafficLights();
299	DirectedRoadSegment drs = null;
300	if (route[routeIndex].pt1 > route[routeIndex].pt2) {
301	drs = new DirectedRoadSegment(roadIdx, route[routeIndex].pt2, true);
302	} else {
303	drs = new DirectedRoadSegment(roadIdx, route[routeIndex].pt2, false);
304	}
305	intersection.addCrossingSegment(drs);
306	intersection.addCrossingSegment(drs); // no matter
307	((IntersectionWithoutTrafficLights) intersection).mustStop = new java.util.ArrayList<Boolean>();
308	// ((IntersectionWithoutTrafficLights)intersection).mustStop.add(new
309	// Boolean(true));
310	// ((IntersectionWithoutTrafficLights)intersection).mustStop.add(new
311	// Boolean(true));
312	((IntersectionWithoutTrafficLights) intersection).mustStop.add(new Boolean(false));
313	((IntersectionWithoutTrafficLights) intersection).mustStop.add(new Boolean(false));
314
315	nextIntersectionPointIndex = route[routeIndex].pt2;
316	maxSpeed = 50.0;
317	inflDist = 0.100;
318	inflDistLaneChange = 0.200;
319	yieldFor = new ArrayList<DirectedRoadSegment>();
320
321	desiredLanes = new ArrayList<Byte>();
322	for (int i = 0; i < Globals.map.roads.get(route[routeIndex].roadIndex).laneNo; i++) {
323	desiredLanes.add((byte) (i + 1));
324	}
325	if (lane > Globals.map.roads.get(roadIdx).laneNo) {
326	lane = (byte) Globals.map.roads.get(roadIdx).laneNo;
327	}
328	return;
329	}
330
331	Road r = Globals.map.roads.get(roadIdx);
332
333	if (direction == 1) {
334	for (int i = 0; i < r.crosses.size(); i++) {
335	if (r.crosses.get(i).getPointIndex() > pointIdx) {
336	// that's the next cross
337	intersection = Globals.map.allIntersections.get(r.crosses.get(i).intersectionIndex);
338	nextIntersectionPointIndex = r.crosses.get(i).getPointIndex();
339	break;
340	}
341	}
342	} else {
343	for (int i = r.crosses.size() - 1; i >= 0; i--) {
344	if (r.crosses.get(i).getPointIndex() < pointIdx) {
345	// that's the next cross
346	intersection = Globals.map.allIntersections.get(r.crosses.get(i).intersectionIndex);
347	nextIntersectionPointIndex = r.crosses.get(i).getPointIndex();
348	// System.out.println("FOUND!");
349	break;
350	}
351	}
352	}
353	inflDist = intersection.getInfluenceDistance(route[routeIndex], route[routeIndex + 1]);
354	inflDistLaneChange = intersection.getInfluenceDistanceLaneChange(route[routeIndex], route[routeIndex + 1]);
355	maxSpeed = intersection.getMaximumSpeed(route[routeIndex], route[routeIndex + 1]);
356	havePriority = intersection.havePriority(route[routeIndex], route[routeIndex + 1]);
357
358	desiredLanes = intersection.getDesiredLanes(route[routeIndex], route[routeIndex + 1]);
359
360	if (!intersection.hasTrafficLights()) {
361	yieldFor = intersection.yieldFor(route[routeIndex], route[routeIndex + 1]);
362	}
363	if (lane > Globals.map.roads.get(roadIdx).laneNo) {
364	lane = (byte) Globals.map.roads.get(roadIdx).laneNo;
365	}
366	if (intersection.hasTrafficLights()) {
367	IntersectionWithTrafficLights iwtl = (IntersectionWithTrafficLights) intersection;
368	turningLeft = iwtl.isLeftTurn(route[routeIndex], route[routeIndex + 1]);
369	}
370
371	}
372
373	public void setIntersection() {
374	if (routeIndex == route.length - 1) {
375	return;
376
377	}
378	Road r = Globals.map.roads.get(roadIdx);
379
380	if (direction == 1) {
381	for (int i = 0; i < r.crosses.size(); i++) {
382	if (r.crosses.get(i).getPointIndex() > pointIdx) {
383	// that's the next cross
384	intersection = Globals.map.allIntersections.get(r.crosses.get(i).intersectionIndex);
385	nextIntersectionPointIndex = r.crosses.get(i).getPointIndex();
386	break;
387	}
388	}
389	} else {
390	for (int i = r.crosses.size() - 1; i >= 0; i--) {
391	if (r.crosses.get(i).getPointIndex() < pointIdx) {
392	// that's the next cross
393	intersection = Globals.map.allIntersections.get(r.crosses.get(i).intersectionIndex);
394	nextIntersectionPointIndex = r.crosses.get(i).getPointIndex();
395	// System.out.println("FOUND!");
396	break;
397	}
398	}
399	}
400
401	// System.out.println("carReceivingMsg "+this.ID+" got route ");
402	// for(int i=0;i<route.length;i++){
403	// System.out.print(" "+route[i]);
404	// }
405
406	inflDist = intersection.getInfluenceDistance(route[routeIndex], route[routeIndex + 1]);
407	inflDistLaneChange = intersection.getInfluenceDistanceLaneChange(route[routeIndex], route[routeIndex + 1]);
408	maxSpeed = intersection.getMaximumSpeed(route[routeIndex], route[routeIndex + 1]);
409	havePriority = intersection.havePriority(route[routeIndex], route[routeIndex + 1]);
410
411	desiredLanes = intersection.getDesiredLanes(route[routeIndex], route[routeIndex + 1]);
412
413	if (!intersection.hasTrafficLights())
414	yieldFor = intersection.yieldFor(route[routeIndex], route[routeIndex + 1]);
415	if (lane > Globals.map.roads.get(roadIdx).laneNo) {
416	lane = (byte) Globals.map.roads.get(roadIdx).laneNo;
417	}
418
419	// System.out.println("carReceivingMsg "+this.ID+" after staff ");
420	// for(int i=0;i<route.length;i++){
421	// System.out.print(" "+route[i]);
422	// }
423
424	}
425
426	public double accel = 0;
427	public boolean canSignal = false;
428	// public double speed1s = 0;
429
430	int lastInfoTime = -1;
431	public int travelTime = 0;
432
433	public void move() {
434	if (finished) {
435	// System.out.println("Car finished!");
436	return;
437	}
438
439	if (!demanded && intersection != null && Globals.monitorWTL != null && intersection.equals(Globals.monitorWTL)
440	&& (influenced \|\| influencedByTrafficLight)) {
441	Globals.demand++;
442	demanded = true;
443	}
444	// compute emissions!
445	vnsim.vehicular.emissions.EmissionsResults emAux = null;
446	double time22 = 0;
447	accel = 0;
448
449	// if (Globals.engine.crtTime % Globals.SECOND == 0){
450	// if (Globals.engine.crtTime < Globals.SECOND )
451	// speed1s = speed;
452	// accel1s = (speed - speed1s) * (3600.0/time22);
453	// speed1s = speed;
454	// }
455
456	time22 = (1.0 / Globals.FPS);
457	accel = (speed - previousSpeed) * (3600.0 / time22);
458	emAux = EmissionsUtil.estimateFuelConsumption((speed + previousSpeed) / 2, accel, time22);
459
460	if (startMonitor) {
461	em.addEmissionsResults(emAux, 1);
462	}
463	travelTime += Globals.FPS;
464	timeOnSegment1 += time22;
465
466	if (Globals.engine.startedWithGUI && Globals.demo.mv.currentCar != null
467	&& simulatedCarInfo.equals(Globals.demo.mv.currentCar) && Globals.engine.crtTime % Globals.SECOND == 0) {
468	Globals.demo.st.setDemand("ne", (int) this.speed, 0, false);
469	// System.out.println("move t:" + (Globals.engine.crtTime /
470	// Globals.SECOND) +" speed: " + (speed+previousSpeed)/2 +" acc: " +
471	// accel / (3.6 * 3600) +" fuel: " + (emAux.fc * (3600.0/time22) /
472	// 1000 ) );
473	// System.out.println((Globals.engine.crtTime / Globals.SECOND) +" "
474	// + (speed+previousSpeed)/2 +" " + accel / (3.6 * 3600) +" " +
475	// (emAux.fc * (3600.0/time22) / 1000 ) );
476	}
477
478	if (lastIntersection != null && Globals.monitorWTL != null && lastIntersection.equals(Globals.monitorWTL)) {
479	double d1 = Globals.map.roads.get(roadIdx).points.get(lastIntersectionPointIndex).getDistance();
480	double d2 = Globals.map.roads.get(roadIdx).points.get(pointIdx).getDistance() + offset;
481	double dif = d2 - d1;
482	if (d1 > d2)
483	dif = d1 - d2;
484	if (dif > 0.5 && !endMonitor && startMonitor) {
485	endMonitor = true;
486	}
487	}
488
489	// am I influenced by the next intersection?
490	if (intersection != null) {
491	double d1 = Globals.map.roads.get(roadIdx).points.get(nextIntersectionPointIndex).getDistance();
492	double d2 = Globals.map.roads.get(roadIdx).points.get(pointIdx).getDistance() + offset;
493	double dif;
494	if (d1 > d2)
495	dif = d1 - d2;
496	else
497	dif = d2 - d1;
498
499	if (dif < 0.2) {
500	canSignal = true;
501	}
502	if (!startMonitor && Globals.monitorWTL != null && intersection.equals(Globals.monitorWTL) && dif < 0.5) {
503	startMonitor = true;
504	startMonitorDistance = totalDistance;
505	}
506
507	if (!queued && intersection != null && Globals.monitorWTL != null
508	&& intersection.equals(Globals.monitorWTL) && (influenced \|\| influencedByTrafficLight)
509	&& speed < WirelessTrafficLight.LOW_SPEED) {
510
511	queued = true;
512	}
513
514	// int toEndNextGreen=-1;
515	// if(simulatedCarInfo.currentPhase==Globals.GREEN) {
516	// toEndNextGreen=simulatedCarInfo.phaseRemainingTime;
517	// } else {
518	// toEndNextGreen=simulatedCarInfo.phaseRemainingTime+
519	//simulatedCarInfo.cycleLength-simulatedCarInfo.interGreenPhaseTime;
520	// }
521
522	if (simulatedCarInfo.infoTime != lastInfoTime && simulatedCarInfo.currentPhase != -1
523	&& simulatedCarInfo.intersection != null && intersection.equals(simulatedCarInfo.intersection)
524	&& intersection.hasTrafficLights()) {
525	lastInfoTime = simulatedCarInfo.infoTime;
526	int dischargeTime = (int) (simulatedCarInfo.queueSize * (Globals.CARSPERKM) / (Globals.QUEUEDISCHARGEFLOW));
527
528	int greenTime = simulatedCarInfo.cycleLength - simulatedCarInfo.interGreenPhaseTime;
529
530	if (Globals.engine.startedWithGUI && Globals.demo.mv.currentCar != null
531	&& simulatedCarInfo.equals(Globals.demo.mv.currentCar)) {
532	// System.out.println();
533	}
534	if (simulatedCarInfo.currentPhase == Globals.GREEN) {
535	int nCars = (int) (simulatedCarInfo.phaseRemainingTime * Globals.QUEUEDISCHARGEFLOW);
536	if ((double) nCars / Globals.CARSPERKM > simulatedCarInfo.queueSize) {
537	// pot trece pe verdele curent
538
539	} else {
540	dischargeTime -= simulatedCarInfo.phaseRemainingTime;
541	if (dischargeTime < 0)
542	dischargeTime = 1;
543	}
544	}
545
546	if (dischargeTime < 0)
547	dischargeTime = 0;
548	double nCycles = (double) dischargeTime / greenTime;
549
550	if (Globals.engine.startedWithGUI && Globals.demo.mv.currentCar != null
551	&& simulatedCarInfo.equals(Globals.demo.mv.currentCar)) {
552	// System.out.println("NCYCLES=" + nCycles);
553	// System.out.println("DT =" + dischargeTime);
554	}
555
556	int nIntCycles = (int) Math.ceil(nCycles);
557	// set the trafficlight cycles this car has to wait
558	if (avgSpeedStatus == SPEED_COMPUTED && nTLCycles < nIntCycles) {
559	nTLCycles = nIntCycles;
560	}
561	int toEndNextGreen = -1;
562	if (simulatedCarInfo.currentPhase == Globals.RED) {
563	toEndNextGreen = nIntCycles * simulatedCarInfo.cycleLength + simulatedCarInfo.phaseRemainingTime
564	+ greenTime;
565	} else {
566	toEndNextGreen = simulatedCarInfo.phaseRemainingTime + nIntCycles * simulatedCarInfo.cycleLength;
567	}
568
569	// int secsToIntersection=(int) (((double)dif /
570	// (double)p.getWantedSpeed(roadIdx)) * 3600);
571
572	double fracGreen = (nIntCycles - nCycles) * greenTime;
573
574	int timeToArrive = (int) (toEndNextGreen - fracGreen);
575
576	double suggestedSpeed = dif * 3600 / (double) timeToArrive;
577
578	if (suggestedSpeed < 10) {
579	suggestedSpeed = 10;
580	}
581	if (Globals.engine.startedWithGUI && Globals.demo.mv.currentCar != null
582	&& simulatedCarInfo.equals(Globals.demo.mv.currentCar)) {
583	// System.out.println("MAXSSUGGESTED = " + suggestedSpeed+"
584	// ; MAX = "+p.getWantedSpeed(roadIdx));
585	Globals.demo.st.setDemand("nw", (int) suggestedSpeed, 0, false);
586	}
587
588	// if(suggestedSpeed < p.getWantedSpeed(roadIdx)) {
589	// wantedSpeed=suggestedSpeed;
590	// System.out.println("SET TO:"+maxSpeed);
591	// }
592
593	}
594	// if(secsToIntersection > toEndNextGreen) {
595	// int
596	// toEndAfterNextGreen=toEndNextGreen+simulatedCarInfo.cycleLength;
597	// if(secsToIntersection > toEndAfterNextGreen) {
598	// //compute speed
599	//
600	// //foarte mica
601	//
602	// } else {
603	//
604	//
605	// }
606	// } else {
607	// maxSpeed=p.getWantedSpeed(roadIdx);
608	// }
609
610	if (dif < inflDist) {
611
612	wantedSpeed = p.getWantedSpeed(roadIdx);
613
614	// if I dont't have priority, check if I will be able to pass
615	influenced = true;
616	if (!intersection.hasTrafficLights()) {
617	boolean canPass = true;
618	if (routeIndex == route.length - 1) {
619	// I am on the last segment
620	canPass = true;
621	} else {
622	if (havePriority) {
623	canPass = true;
624	} else {
625	if (yieldFor != null) {
626	for (int i = 0; i < yieldFor.size(); i++) {
627	// look on that segment if there are close
628	// vehicles
629	CarInstance c2 = getFirstApproachingVehicle(yieldFor.get(i));
630
631	try {
632	if (intersection.isScissors(route[routeIndex], route[routeIndex + 1],
633	c2.route[c2.routeIndex], c2.route[c2.routeIndex + 1]))
634	continue;
635	} catch (Exception ex) {
636	// ex.printStackTrace();
637	}
638	// how far is it?
639	if (c2 == null) {
640	continue;
641	}
642	double do1 = Globals.map.roads.get(c2.roadIdx).points.get(c2.pointIdx)
643	.getDistance()
644	+ offset;
645	double do2 = Globals.map.roads.get(yieldFor.get(i).roadIndex).points.get(
646	yieldFor.get(i).pointIndex).getDistance();
647	double dist = 0.0;
648	if (do1 - do2 > 0)
649	dist = do1 - do2;
650	else
651	dist = do2 - do1;
652
653	// dist=distance of other vehicle (c2) from
654	// intersection
655	// dif=my distance from intersection
656
657	// dif, dist = [km]
658	if (!p.estimateRoadChangePossible(speed, c2.speed, dif, dist))
659	canPass = false;
660	}
661	}
662	}
663	}
664	if (!canPass) {
665	// try to get to a stop 10 meters before
666	wantedSpeed = p.getWantedSpeedInfluenced(dif - 0.01, 0.0);
667	// the intersection
668
669	// if(ID==12)
670	// System.out.println("I CANNOT PASS! MAX="+maxSpeed+" ;
671	// DIF="+dif+" ; WS="+wantedSpeed);
672	} else {
673	if (maxSpeed != -1.0) {
674	// -1.0 means there is no speed reducing necessary
675	wantedSpeed = p.getWantedSpeedInfluenced(dif, maxSpeed);
676	// if(ID==12)
677	// System.out.println("I CAN PASS! MAX="+maxSpeed+"
678	// ; DIF="+dif+" ; WS="+wantedSpeed);
679	}
680	}
681	if (wantedSpeed > maxWantedSpeed)
682	wantedSpeed = maxWantedSpeed;
683	// System.out.println("NO LIGHTS : MODIFY WS="+wantedSpeed);
684	} else {
685	// there are traffic lights
686	IntersectionWithTrafficLights iwtl = (IntersectionWithTrafficLights) intersection;
687	if (iwtl.isGreen(route[routeIndex])) {
688
689	boolean canPass = true;
690	ArrayList<DirectedRoadSegment> yieldFor = iwtl.yieldFor(route[routeIndex],
691	route[routeIndex + 1]);
692	for (int i = 0; i < yieldFor.size(); i++) {
693	// look on that segment if there are close vehicles
694	CarInstance c2 = getFirstApproachingVehicle(yieldFor.get(i));
695
696	try {
697	if (intersection.isScissors(route[routeIndex], route[routeIndex + 1],
698	c2.route[c2.routeIndex], c2.route[c2.routeIndex + 1]))
699	continue;
700	} catch (Exception ex) {
701	// ex.printStackTrace();
702	}
703
704	// how far is it?
705	if (c2 == null) {
706	canPass = true;
707	continue;
708	}
709	double do1 = Globals.map.roads.get(c2.roadIdx).points.get(c2.pointIdx).getDistance()
710	+ offset;
711	double do2 = Globals.map.roads.get(yieldFor.get(i).roadIndex).points.get(
712	yieldFor.get(i).pointIndex).getDistance();
713	double dist = 0.0;
714	if (do1 - do2 > 0)
715	dist = do1 - do2;
716	else
717	dist = do2 - do1;
718
719	// dif, dist = [km]
720	if (!p.estimateRoadChangePossible(speed, c2.speed, dif, dist))
721	canPass = false;
722	}
723
724	if (!canPass) {
725	wantedSpeed = p.getWantedSpeedInfluenced(dif - 0.003, 0.0); // try
726	// to
727	// get
728	// to
729	// a
730	// stop
731	// 3 meters before the
732	// intersection
733	} else {
734	if (maxSpeed != -1.0) {
735	// -1.0 means there is no speed reducing
736	// necessary
737	wantedSpeed = p.getWantedSpeedInfluenced(dif, maxSpeed);
738	}
739	}
740	} else {
741	if (dif < 0.010 && iwtl.isYellow(route[routeIndex])) { // closer
742	// than
743	// 10
744	// meters;
745	// go
746	// ahead
747	// if(ID==57)
748	// System.out.println("ELSE - IF1");
749	wantedSpeed = p.getWantedSpeedInfluenced(dif, maxSpeed);
750	} else {
751
752	if (dif < 0.006) {
753	// go ahead! no matter the color
754	wantedSpeed = p.getWantedSpeedInfluenced(dif, maxSpeed);
755	} else {
756	if (!influencedByTrafficLight) {
757	influencedByTrafficLight = true;
758	timeStartInfluenced = Globals.engine.crtTime;
759	// also compute expected time!
760	double time = (dif / wantedSpeed) * 3600; // secs
761	timeExpectedInfluenced = (long) time;
762	}
763	wantedSpeed = p.getWantedSpeedInfluenced(dif - 0.008, 0.0); // try
764	// to
765	// get
766	// to
767	// a
768	// stop 5 meters
769	// before the
770	// intersection
771	}
772	}
773	}
774	if (wantedSpeed > maxWantedSpeed)
775	wantedSpeed = maxWantedSpeed;
776
777	// if(ID==57)
778	// System.out.println("DIF= "+dif+" THIS="+this+"WITH LIGHTS
779	// : MODIFY WS="+wantedSpeed);
780	}
781	}
782
783	if (dif < inflDistLaneChange \|\| influencedByTrafficLight) {
784	influencedLaneChange = true;
785	// try to change lane;
786
787	if (!wellPositioned && dif < 0.005) {
788	desiredLanes.add((byte) lane);
789	wellPositioned = true;
790	}
791
792	if (!wellPositioned) {
793	if (desiredLanes.contains(lane)) {
794	wellPositioned = true;
795	} else {
796	int first = desiredLanes.get(0);
797	if (lane < first) {
798	// should go to upper
799	if (canSafelyChangeUpperLane()) {
800	lane++;
801	} else {
802	// should brake (try to get to a stop 20 meters
803	// before the intersection)
804	wantedSpeed = p.getWantedSpeedInfluenced(dif - 0.020, maxSpeed);
805	}
806	} else {
807	// should go to lower
808	if (canSafelyChangeLowerLane()) {
809	lane--;
810	} else {
811	// should brake (try to get to a stop 20 meters
812	// before the intersection)
813	wantedSpeed = p.getWantedSpeedInfluenced(dif - 0.020, maxSpeed);
814	}
815	}
816	if (wantedSpeed > maxWantedSpeed)
817	wantedSpeed = maxWantedSpeed;
818	}
819	}
820	}
821	}
822
823	// System.out.println("Calling getDistance");
824	CarInstance c = getPrecedingCarOnLane(this.lane);
825
826	// System.out.println("Returned:"+c);
827	double distance = 0.0;
828	if (c != null) {
829	distance = getDistance(c);
830	// System.out.println("DISTANCE="+distance);
831	}
832
833	// if(ID==12) {
834	// System.out.println("THIS="+this+" ; PREC="+c+" DISTANCE="+distance);
835	// }
836
837	if (c == null \|\| distance >= p.getInfluenceDistance(speed)) {
838	// free driving;
839	// can change lane to a lower lane and still be in free driving?
840
841	// System.out.println("I AM:"+this+" ; FREE DRIVING; CAN I CHANGE
842	// LANE LOWER?");
843	// System.out.println("I AM "+ID+" DISTANCE="+distance);
844	// System.out.println("I AM "+ID+"; FREE DRIVING ;
845	// PT="+pointIdx+"LANE="+lane);
846
847	// if(!influencedLaneChange && checkFreeDrivingLowerLane()) {
848	if (((desiredLanes.contains((byte) (lane - 1))) \|\| (!influencedLaneChange)) && checkFreeDrivingLowerLane()) {
849	lane--;
850	}
851	previousSpeed = speed;
852	computeSpeedFreeDriving();
853	changePosition();
854	return;
855	}
856	if (distance < p.getInfluenceDistance(speed) && distance >= p.getSafetyDistance(speed)) {
857	// depends on the speed
858	if (c.speed > this.speed) {
859	if (wasApproaching) {
860	nIterationsApproaching++;
861	if (nIterationsApproaching < p.getReactionTimeFreeDriving() * Globals.FPS) {
862	previousSpeed = speed;
863	changePosition(); // no speed change
864	return;
865	}
866	wasApproaching = false;
867	nIterationsApproaching = 0;
868	}
869
870	// System.out.println("I AM "+ID+"; FREE DRIVING2 ;
871	// LANE="+lane);
872	// System.out.println("I AM:"+this+" ; FREE DRIVING2; CAN I
873	// CHANGE LANE LOWER?");
874
875	// free driving
876	if ((!influencedLaneChange \|\| (desiredLanes.contains((byte) (lane - 1))))
877	&& checkFreeDrivingLowerLane()) {
878	lane--;
879	}
880	previousSpeed = speed;
881	computeSpeedFreeDriving();
882	changePosition();
883	return;
884	}
885	if (c.speed == this.speed) {
886	// System.out.println("I AM "+ID+"; FOLLOWING ;
887	// PT="+pointIdx+"LANE="+lane);
888	if (c.speed == 0.0) {
889	// practically stopped behind another stopped vehicle;
890	// it's safe to get a little closer
891	previousSpeed = speed;
892	computeSpeedFreeDriving();
893	changePosition();
894	return;
895	}
896	// following; no change of speed
897	if ((!influencedLaneChange \|\| (desiredLanes.contains((byte) (lane + 1))))
898	&& checkFreeDrivingUpperLane()) {
899	lane++;
900	previousSpeed = speed;
901	computeSpeedFreeDriving();
902	} else {
903	previousSpeed = speed;
904	}
905	changePosition();
906	return;
907	}
908	if (c.speed < this.speed) {
909	// System.out.println("I AM "+ID+"; INFLUENCED ;
910	// PT="+pointIdx+"LANE="+lane);
911	// should brake; first check if it can change lane
912
913	// System.out.println("I AM:"+this+" ; INFLUENCED; SHOULD I GO
914	// TO UPPER?");
915
916	if ((!influencedLaneChange \|\| (desiredLanes.contains((byte) (lane + 1))))
917	&& checkFreeDrivingUpperLane()) {
918	// System.out.println("YES!");
919	lane++;
920	previousSpeed = speed;
921	computeSpeedFreeDriving();
922	changePosition();
923	return;
924	}
925	// System.out.println("I AM "+ID+"; APPROACHING ;
926	// PT="+pointIdx+"; LANE="+lane);
927
928	if (!wasApproaching) {
929	wasApproaching = true;
930	nIterationsApproaching = 0;
931	}
932
933	if (!influencedByTrafficLight) {
934	if (c.influencedByTrafficLight) {
935	becomeInfluencedByTrafficLight();
936	timeStartInfluenced = Globals.engine.crtTime;
937
938	// also compute expected time!
939	if (intersection != null) {
940	double d1 = Globals.map.roads.get(roadIdx).points.get(nextIntersectionPointIndex)
941	.getDistance();
942	double d2 = Globals.map.roads.get(roadIdx).points.get(pointIdx).getDistance() + offset;
943	double dif;
944	if (d1 > d2)
945	dif = d1 - d2;
946	else
947	dif = d2 - d1;
948	double time = (dif / wantedSpeed) * 3600; // secs
949	timeExpectedInfluenced = (long) time;
950	}
951	}
952	}
953
954	previousSpeed = speed;
955	computeSpeedApproaching(c.speed, distance);
956	changePosition();
957	return;
958	}
959	}
960
961	if (distance < p.getSafetyDistance(speed)) {
962	if (c.speed > speed) {
963	// no need to brake, but do not accelerate either
964	previousSpeed = speed;
965	changePosition();
966	return;
967	}
968
969	// System.out.println("I AM "+ID+" MAXIMUM BRAKE !");
970	// first check if it can change lane
971	if ((!influencedLaneChange \|\| (desiredLanes.contains((byte) (lane + 1)))) && checkFreeDrivingUpperLane()) {
972	// System.out.println("YES!");
973	lane++;
974	previousSpeed = speed;
975	computeSpeedFreeDriving();
976	changePosition();
977	return;
978	}
979	if ((!influencedLaneChange \|\| (desiredLanes.contains((byte) (lane + 1)))) && checkApproachingUpperLane()) {
980	lane++;
981	CarInstance c2 = getPrecedingCarOnLane(this.lane);
982	// System.out.println("Returned:"+c);
983	double distance2 = 0.0;
984	if (c2 != null) {
985	// System.out.println("Calling getDistance");
986	distance2 = getDistance(c2);
987	// System.out.println("DISTANCE="+distance);
988
989	if (!influencedByTrafficLight) {
990	if (c2.influencedByTrafficLight) {
991	becomeInfluencedByTrafficLight();
992	timeStartInfluenced = Globals.engine.crtTime;
993
994	// also compute expected time!
995	if (intersection != null) {
996	double d1 = Globals.map.roads.get(roadIdx).points.get(nextIntersectionPointIndex)
997	.getDistance();
998	double d2 = Globals.map.roads.get(roadIdx).points.get(pointIdx).getDistance() + offset;
999	double dif;
1000	if (d1 > d2)
1001	dif = d1 - d2;
1002	else
1003	dif = d2 - d1;
1004	double time = (dif / wantedSpeed) * 3600; // secs
1005	timeExpectedInfluenced = (long) time;
1006	}
1007	}
1008	}
1009
1010	previousSpeed = speed;
1011	computeSpeedApproaching(c2.speed, distance2);
1012	changePosition();
1013	return;
1014	}
1015	previousSpeed = speed;
1016	computeSpeedFreeDriving();
1017	changePosition();
1018	return;
1019	}
1020	// maximum brake
1021	// System.out.println("I AM "+ID+"; MAXIMUM BRAKE ; LANE="+lane);
1022	previousSpeed = speed;
1023	computeSpeedMaximumBrake();
1024	changePosition();
1025	}
1026	}
1027
1028	public boolean canSafelyChangeLowerLane() {
1029	if (lane == 1)
1030	return false;
1031
1032	CarInstance c = getFollowingCarOnLane(lane - 1);
1033	CarInstance c2 = getPrecedingCarOnLane(lane - 1);
1034
1035	double distance1, otherSpeed;
1036	if (c == null) {
1037	distance1 = Double.MAX_VALUE;
1038	otherSpeed = 0.0;
1039	} else {
1040	distance1 = getDistance(c);
1041	otherSpeed = c.speed;
1042	}
1043
1044	double distance2, otherSpeed2;
1045	if (c2 == null) {
1046	distance2 = Double.MAX_VALUE;
1047	otherSpeed2 = 0.0;
1048	} else {
1049	distance2 = getDistance(c2);
1050	otherSpeed2 = c2.speed;
1051	}
1052
1053	if (distance1 > p.getLaneChangeSafetyDistance(speed, otherSpeed)
1054	&& distance2 > p.getLaneChangeSafetyDistanceInFront(speed, otherSpeed2))
1055	return true;
1056	return false;
1057	}
1058
1059	public boolean canSafelyChangeUpperLane() {
1060	if (lane >= Globals.map.roads.get(roadIdx).laneNo)
1061	return false;
1062	CarInstance c = getFollowingCarOnLane(lane + 1);
1063	CarInstance c2 = getPrecedingCarOnLane(lane + 1);
1064
1065	double distance1, otherSpeed;
1066	if (c == null) {
1067	distance1 = Double.MAX_VALUE;
1068	otherSpeed = 0.0;
1069	} else {
1070	distance1 = getDistance(c);
1071	otherSpeed = c.speed;
1072	}
1073
1074	double distance2, otherSpeed2;
1075	if (c2 == null) {
1076	distance2 = Double.MAX_VALUE;
1077	otherSpeed2 = 0.0;
1078	} else {
1079	distance2 = getDistance(c2);
1080	otherSpeed2 = c2.speed;
1081	}
1082	if (distance1 > p.getLaneChangeSafetyDistance(speed, otherSpeed)
1083	&& distance2 > p.getLaneChangeSafetyDistanceInFront(speed, otherSpeed2)) {
1084	return true;
1085	}
1086	return false;
1087	}
1088
1089	public boolean checkFreeDrivingLowerLane() {
1090	if (!canSafelyChangeLowerLane())
1091	return false;
1092	CarInstance c = getPrecedingCarOnLane(this.lane - 1);
1093	double distance = 0.0;
1094	if (c != null) {
1095	distance = getDistance(c);
1096	}
1097	if (c == null \|\| distance > p.getInfluenceDistance(speed)) {
1098	// can change lane and still be in free driving
1099	// System.out.println("Returning true, because
1100	// c="+c+"distance="+distance+">"+p.getInfluenceDistance(speed));
1101
1102	return true;
1103	}
1104	if (distance < p.getInfluenceDistance(speed) && distance >= p.getSafetyDistance(speed)) {
1105	if (c.speed > this.speed) {
1106	// can change lane and still be in free driving
1107	return true;
1108	}
1109	}
1110	return false;
1111	}
1112
1113	public boolean checkFreeDrivingUpperLane() {
1114	if (!canSafelyChangeUpperLane())
1115	return false;
1116	CarInstance c = getPrecedingCarOnLane(this.lane + 1);
1117	double distance = 0.0;
1118	if (c != null) {
1119	distance = getDistance(c);
1120	}
1121	if (c == null \|\| distance > p.getInfluenceDistance(speed)) {
1122	// can change lane and still be in free driving
1123	return true;
1124	}
1125	if (distance < p.getInfluenceDistance(speed) && distance >= p.getSafetyDistance(speed)) {
1126	if (c.speed > this.speed) {
1127	// can change lane and still be in free driving
1128	return true;
1129	}
1130	}
1131	return false;
1132	}
1133
1134	public boolean checkApproachingUpperLane() {
1135	if (!canSafelyChangeUpperLane())
1136	return false;
1137	CarInstance c = getPrecedingCarOnLane(this.lane + 1);
1138	double distance = 0.0;
1139	if (c != null) {
1140	distance = getDistance(c);
1141	}
1142	if (c == null \|\| distance > p.getInfluenceDistance(speed)) {
1143	// can change lane and be in free driving
1144	return true;
1145	}
1146	if (distance < p.getInfluenceDistance(speed) && distance >= p.getSafetyDistance(speed)) {
1147	return true; // can change lane and be approaching or in free
1148	// driving
1149	}
1150	return false;
1151	}
1152
1153	public void computeSpeedFreeDriving() {
1154	if (speed < wantedSpeed) {
1155	speed = speed + accelFreeDriving / (double) Globals.FPS;
1156	}
1157	if (speed > wantedSpeed)
1158	speed = wantedSpeed;
1159	}
1160
1161	public void computeSpeedApproaching(double otherCarSpeed, double otherCarDistance) {
1162	// brake so that "desiredDistance" is obtained when the
1163	// speed reaches the speed of the preceding vehicle
1164
1165	// System.out.println("DISTANCE="+otherCarDistance+";
1166	// DESIRED="+p.getDesiredDistance(otherCarSpeed));
1167
1168	double accel = -(this.speed - otherCarSpeed) * (this.speed - otherCarSpeed)
1169	/ (2 * (otherCarDistance - p.getDesiredDistance(otherCarSpeed)));
1170
1171	accel /= 2.0; // correction
1172
1173	if (accel < 0) {
1174	// accel should be negative
1175	// System.out.println("ACCEL="+accel);
1176	speed = speed + accel / (double) Globals.FPS;
1177	} else {
1178	// maximum brake (I am already closer than "desiredDistance"
1179	// System.out.println("Maximum brake!");
1180	computeSpeedMaximumBrake();
1181	}
1182	if (speed > wantedSpeed)
1183	speed = wantedSpeed;
1184	}
1185
1186	public void computeSpeedMaximumBrake() {
1187	double accel = p.getAccelerationMaximumBrake();
1188	speed = speed + accel / (double) Globals.FPS;
1189	}
1190
1191	public double totalDistance = 0.0; // [km]
1192
1193	public void changePosition() {
1194	// change position according to "speed"
1195
1196	if (speed < 0) {
1197	speed = 0.0;
1198	return;
1199	}
1200	if (avgSpeedStatus == SPEED_DELAY_RESET) {
1201	avgSpeedStatus = SPEED_RESET;
1202	nTLCycles = 0;
1203	}
1204
1205	double distance = speed / 3600.0 * (1.0 / Globals.FPS); // km!
1206
1207	distanceOnSegment += distance;
1208	totalDistance += distance;
1209	// System.out.println("I AM "+ID+";PT="+pointIdx+"SPEED="+speed+";
1210	// INTERS
1211	// AT="+nextIntersectionPointIndex+"DIR="+direction+";OFFSET="+offset);
1212
1213	if (direction == 1) {
1214	double newOffset = offset + distance;
1215	if (pointIdx >= Globals.map.roads.get(roadIdx).points.size() - 1) {
1216	// last point on road + offset = error
1217	System.out.println("ERROR 1 ! changePosition()");
1218	return;
1219	}
1220	double gapBetweenPoints = Globals.map.roads.get(roadIdx).points.get(pointIdx + 1).getDistance()
1221	- Globals.map.roads.get(roadIdx).points.get(pointIdx).getDistance();
1222	// [km]
1223	if (newOffset > gapBetweenPoints) {
1224	// must increase pointIdx
1225	pointIdx++;
1226	offset = newOffset - gapBetweenPoints;
1227	// after increasing pointIdx, have I reached the last point
1228	// on this road segment?
1229	// if yes, than change road segment!
1230	if (pointIdx == route[routeIndex].pt2) {
1231	if (routeIndex == route.length - 1)
1232	finished = true;
1233	else {
1234	roadIdx = route[routeIndex + 1].roadIndex;
1235	pointIdx = route[routeIndex + 1].pt1;
1236	if (route[routeIndex + 1].pt2 > pointIdx)
1237	direction = 1;
1238	else
1239	direction = 0;
1240	offset = 0;
1241	routeIndex++;
1242	startOnRoad();
1243	}
1244	}
1245	} else
1246	offset = newOffset;
1247	} else {
1248	if (pointIdx == 0) {
1249	double newOffset = offset - distance;
1250	if (newOffset < 0) {
1251	offset = 0;
1252	} else
1253	offset = newOffset;
1254	} else {
1255	double newOffset = offset - distance;
1256	double gapBetweenPoints = Globals.map.roads.get(roadIdx).points.get(pointIdx).getDistance()
1257	- Globals.map.roads.get(roadIdx).points.get(pointIdx - 1).getDistance();
1258	// [km]
1259	if (newOffset < 0) {
1260	pointIdx--;
1261	offset = gapBetweenPoints + newOffset;
1262	// after decreasing pointIdx, have I passed the last point
1263	// on this road segment?
1264	// if yes, than change road segment!
1265	if ((pointIdx + 1) == route[routeIndex].pt2) {
1266	if (routeIndex == route.length - 1)
1267	finished = true;
1268	else {
1269	roadIdx = route[routeIndex + 1].roadIndex;
1270	pointIdx = route[routeIndex + 1].pt1;
1271	if (route[routeIndex + 1].pt2 > pointIdx)
1272	direction = 1;
1273	else
1274	direction = 0;
1275	offset = 0;
1276	routeIndex++;
1277	startOnRoad();
1278	}
1279	}
1280	} else
1281	offset = newOffset;
1282	}
1283	}
1284
1285	// if it is "very close" to the intersection, change segment
1286	// then change segment
1287
1288	double dif = 1.0;
1289	if (intersection != null && influenced) {
1290	double d1 = Globals.map.roads.get(roadIdx).points.get(nextIntersectionPointIndex).getDistance();
1291	double d2 = Globals.map.roads.get(roadIdx).points.get(pointIdx).getDistance() + offset;
1292	if (d1 > d2)
1293	dif = d1 - d2;
1294	else
1295	dif = d2 - d1;
1296	}
1297	if (dif < 0.001) { // less than 1 meter
1298	if (routeIndex < route.length - 1) {
1299	roadIdx = route[routeIndex + 1].roadIndex;
1300	pointIdx = route[routeIndex + 1].pt1;
1301	if (route[routeIndex + 1].pt2 > pointIdx)
1302	direction = 1;
1303	else
1304	direction = 0;
1305	offset = 0;
1306	routeIndex++;
1307	startOnRoad();
1308	} else {
1309	finished = true;
1310	}
1311	}
1312
1313	// if the cars waits at a traffic light compute the average speed
1314	if (Engine.simulationType == RoutingConstants.DYNAMIC_CITY_ROUTE && avgSpeedStatus == SPEED_RESET && !queued
1315	&& intersection != null && speed < 0.5) {
1316	// queued = true;
1317	avgSpeed = distanceOnSegment * 3600 / timeOnSegment1;
1318	avgSpeedStatus = SPEED_COMPUTED;
1319	}
1320	}
1321
1322	public CarInstance(short roadIdx, short pointIdx, Personality p) {
1323	this.roadIdx = roadIdx;
1324	this.pointIdx = pointIdx;
1325	this.p = p;
1326	}
1327
1328	public CarInstance(CarInfo car) {
1329	super(car);
1330	}
1331
1332	// public CarInstance(int ID){
1333	// this.ID = ID;
1334	// }
1335
1336	private double getDistance(CarInstance c) {
1337	// return the absolute distance between "this" car and "c" (meters)
1338	// c can be either on the same road with "this" or on the next road
1339	// (according to this.route)
1340	// or on the previous road (according to this.route)
1341	if (c.roadIdx == roadIdx) {
1342	double d1 = Globals.map.roads.get(roadIdx).points.get(pointIdx).getDistance() + offset;
1343	double d2 = Globals.map.roads.get(c.roadIdx).points.get(c.pointIdx).getDistance() + c.offset;
1344	if (d1 > d2)
1345	return (d1 - d2) * 1000;
1346	return (d2 - d1) * 1000;
1347	} else {
1348	if (routeIndex < route.length - 1) {
1349	// can check next road
1350	if (c.roadIdx == route[routeIndex + 1].roadIndex) {
1351	double d1 = Globals.map.roads.get(roadIdx).points.get(nextIntersectionPointIndex).getDistance();
1352	double d2 = Globals.map.roads.get(roadIdx).points.get(pointIdx).getDistance() + offset;
1353	double dist1;
1354	if (d1 > d2)
1355	dist1 = d1 - d2;
1356	else
1357	dist1 = d2 - d1;
1358	d1 = Globals.map.roads.get(route[routeIndex + 1].roadIndex).points.get(route[routeIndex + 1].pt1)
1359	.getDistance();
1360	d2 = Globals.map.roads.get(route[routeIndex + 1].roadIndex).points.get(c.pointIdx).getDistance()
1361	+ c.offset;
1362	double dist2;
1363	if (d1 > d2)
1364	dist2 = d1 - d2;
1365	else
1366	dist2 = d2 - d1;
1367	return (dist1 + dist2) * 1000;
1368	}
1369	}
1370	if (routeIndex > 0) {
1371	// can check previous road
1372	if (c.roadIdx == route[routeIndex - 1].roadIndex) {
1373	double d1 = Globals.map.roads.get(roadIdx).points.get(pointIdx).getDistance() + offset;
1374	double d2 = Globals.map.roads.get(roadIdx).points.get(route[routeIndex].pt1).getDistance();
1375	double dist1;
1376	if (d1 > d2)
1377	dist1 = d1 - d2;
1378	else
1379	dist1 = d2 - d1;
1380
1381	d1 = Globals.map.roads.get(c.roadIdx).points.get(route[routeIndex - 1].pt2).getDistance();
1382	d2 = Globals.map.roads.get(c.roadIdx).points.get(c.pointIdx).getDistance() + c.offset;
1383	double dist2;
1384	if (d1 > d2)
1385	dist2 = d1 - d2;
1386	else
1387	dist2 = d2 - d1;
1388	return (dist1 + dist2) * 1000;
1389	}
1390	}
1391	// System.out.println("ERROR! getDistance called, but cars not on
1392	// the same road or on consecutive roads!");
1393	// System.out.println("THIS = "+this);
1394	// System.out.println("OTHER = "+c);
1395	return -1.0;
1396	}
1397	}
1398
1399	public double getDistanceToCarInFront() {
1400	CarInstance c = getPrecedingCarOnLane(lane);
1401	if (c == null)
1402	return 1000.0;
1403	double d = getDistance(c);
1404	if (d < 0)
1405	return 1000.0;
1406	return d;
1407	}
1408
1409	private CarInstance getPrecedingCarOnLane(int laneNo) {
1410	// returns the car in front of "this", on lane "laneNo";
1411	Road thisRoad = Globals.map.roads.get(roadIdx);
1412	if (direction == 1) {
1413	int i = index + 1;
1414	while (i < thisRoad.carsOnThisRoad.size()) {
1415	CarInstance c = thisRoad.carsOnThisRoad.get(i);
1416	if (c.lane == laneNo && c.direction == this.direction && c.pointIdx < route[routeIndex].pt2)
1417	return c;
1418	i++;
1419	}
1420	// there was no car on the current road! But is there one on the
1421	// "next" road?
1422	return getFirstCarOnNextSegment();
1423	} else {
1424	// System.out.println("else; index="+index);
1425	int i = index - 1;
1426	while (i >= 0) {
1427	// System.out.println("Trying a car...");
1428	CarInstance c = thisRoad.carsOnThisRoad.get(i);
1429	if (c.lane == laneNo && c.direction == this.direction && c.pointIdx >= route[routeIndex].pt2) {
1430	// System.out.println("IS GOOD!");
1431	return c;
1432	} else {
1433	}
1434	i--;
1435	}
1436	// there was no car in the current road! But is there one on the
1437	// "next" road?
1438	return getFirstCarOnNextSegment();
1439
1440	}
1441	}
1442
1443	private CarInstance getFirstCarOnNextSegment() {
1444	boolean considerLanes;
1445
1446	if (routeIndex < route.length - 1) {
1447	if (Globals.map.roads.get(route[routeIndex].roadIndex).laneNo <= Globals.map.roads
1448	.get(route[routeIndex + 1].roadIndex).laneNo)
1449	considerLanes = true;
1450	else
1451	considerLanes = false;
1452
1453	Road nextRoad = Globals.map.roads.get(route[routeIndex + 1].roadIndex);
1454	if (route[routeIndex + 1].pt1 > route[routeIndex + 1].pt2) {
1455	for (int i = nextRoad.carsOnThisRoad.size() - 1; i >= 0; i--) {
1456	CarInstance c = nextRoad.carsOnThisRoad.get(i);
1457	// if(ID==22) {
1458	// System.out.println("TESTING : "+c);
1459	// }
1460	if (c.pointIdx < route[routeIndex + 1].pt1 && c.pointIdx > route[routeIndex + 1].pt2
1461	&& c.direction == 0 && (!considerLanes \|\| (considerLanes && c.lane == this.lane))) {
1462	// if(ID==22) {
1463	// System.out.println("WAS GOOD : "+c);
1464	// }
1465	return c;
1466	}
1467	}
1468	return null;
1469	} else {
1470	for (int i = 0; i < nextRoad.carsOnThisRoad.size(); i++) {
1471	CarInstance c = nextRoad.carsOnThisRoad.get(i);
1472	// if(ID==22) {
1473	// System.out.println("TESTING : "+c);
1474	// }
1475	if (c.pointIdx < route[routeIndex + 1].pt2 && c.pointIdx >= route[routeIndex + 1].pt1
1476	&& c.direction == 1 && (!considerLanes \|\| (considerLanes && c.lane == this.lane))) {
1477	// if(ID==22) {
1478	// System.out.println("WAS GOOD : "+c);
1479	// }
1480	return c;
1481	}
1482	}
1483	return null;
1484	}
1485	} else
1486	return null;
1487	}
1488
1489	private CarInstance getFirstApproachingVehicle(DirectedRoadSegment drs) {
1490	SortedCarVector sv = Globals.map.roads.get(drs.roadIndex).carsOnThisRoad;
1491	if (drs.direction) {
1492	for (int i = 0; i < sv.size(); i++) {
1493	CarInstance c = sv.get(i);
1494	if (c.pointIdx < drs.pointIndex \|\| c.direction == 1)
1495	continue;
1496	// else this is the one
1497	return c;
1498	}
1499	} else {
1500	for (int i = sv.size() - 1; i >= 0; i--) {
1501	CarInstance c = sv.get(i);
1502	if (c.pointIdx >= drs.pointIndex \|\| c.direction == 0)
1503	continue;
1504	// else this is the one
1505	return c;
1506	}
1507	}
1508	return null;
1509	}
1510
1511	private CarInstance getFollowingCarOnLane(int laneNo) {
1512	// returns the car behind "this", on lane "laneNo";
1513	Road thisRoad = Globals.map.roads.get(roadIdx);
1514	if (direction == 1) {
1515	int i = index - 1;
1516	while (i >= 0) {
1517	CarInstance c = thisRoad.carsOnThisRoad.get(i);
1518	if (c.lane == laneNo && c.direction == this.direction)
1519	return c;
1520	i--;
1521	}
1522	return getFollowingCarOnPrecRoad(laneNo);
1523	} else {
1524	int i = index + 1;
1525	while (i < thisRoad.carsOnThisRoad.size()) {
1526	CarInstance c = thisRoad.carsOnThisRoad.get(i);
1527	if (c.lane == laneNo && c.direction == this.direction)
1528	return c;
1529	i++;
1530	}
1531	return getFollowingCarOnPrecRoad(laneNo);
1532	}
1533	}
1534
1535	private CarInstance getFollowingCarOnPrecRoad(int laneNo) {
1536	// returns the car just "behind" the current car, but
1537	// situated on the previous road segment (according to
1538	// the route)
1539	if (routeIndex == 0)
1540	return null;
1541	Road prevRoad = Globals.map.roads.get(route[routeIndex - 1].roadIndex);
1542	if (route[routeIndex - 1].pt1 > route[routeIndex - 1].pt2) {
1543	int i = 0;
1544	while (i < prevRoad.carsOnThisRoad.size()) {
1545	CarInstance c = prevRoad.carsOnThisRoad.get(i);
1546	if (c.lane == laneNo && c.pointIdx >= route[routeIndex - 1].pt2 && c.direction == 0)
1547	return c;
1548	i++;
1549	}
1550	return null;
1551	} else {
1552	int i = prevRoad.carsOnThisRoad.size() - 1;
1553	while (i >= 0) {
1554	CarInstance c = prevRoad.carsOnThisRoad.get(i);
1555	if (c.lane == laneNo && c.pointIdx < route[routeIndex - 1].pt2 && c.direction == 1)
1556	return c;
1557	i--;
1558	}
1559	return null;
1560	}
1561	}
1562
1563	private void becomeInfluencedByTrafficLight() {
1564	influencedByTrafficLight = true;
1565	influencedLaneChange = true;
1566
1567	// which lane is the best?
1568	double bestDist = -1.0;
1569	int lane = 1;
1570	for (int i = 0; i < Globals.map.roads.get(roadIdx).laneNo; i++) {
1571	if (!desiredLanes.contains(new Byte((byte) (i + 1))))
1572	continue;
1573
1574	CarInstance c = getPrecedingCarOnLane(i + 1);
1575	double dist = 0.0;
1576	if (c != null) {
1577	dist = getDistance(c);
1578	} else {
1579	dist = Double.MAX_VALUE;
1580	}
1581	if (dist > bestDist) {
1582	bestDist = dist;
1583	lane = i;
1584	}
1585	}
1586	if (bestDist == -1.0) {
1587	System.err.println("ERROR! becomeInfluencedByTrafficLight!");
1588	}
1589	desiredLanes = new ArrayList<Byte>();
1590	desiredLanes.add((byte) (lane + 1));
1591	}
1592
1593	public int compareTo(CarInstance c) {
1594	// if this>c ==> return 1;
1595	// if this=c ==> return 0;
1596	// if this<c ==> return -1;
1597
1598	// assert(c.roadIdx==this.roadIdx);
1599	if (c.roadIdx != this.roadIdx) {
1600	System.out.println("ERROR! Comparing cars on different roads!: " + this + " with " + c);
1601	return 0;
1602	}
1603
1604	if (this.pointIdx > c.pointIdx)
1605	return 1;
1606	if (this.pointIdx < c.pointIdx)
1607	return -1;
1608	if (this.offset == c.offset)
1609	return this.ID - c.ID;
1610	if (this.offset > c.offset)
1611	return 1;
1612	return -1;
1613	}
1614
1615	public void printRoute() {
1616	for (int i = 0; i < route.length; i++) {
1617	System.out.println(route[i] + " ");
1618	}
1619	System.out.println("**");
1620	}
1621
1622	public String toString() {
1623	String ret = "";
1624	ret = ret + "Road:" + roadIdx + ";Point=" + pointIdx + ";offSet=" + offset + ";Speed=" + speed + ";Lane="
1625	+ lane;
1626	return ret;
1627	}
1628
1629	public void deleteCarToPointMapping() {
1630	Road r = Globals.map.roads.get(this.getRoadIdx());
1631	Point p = r.points.get(this.getPointIdx());
1632	PeanoKey pk = Globals.map.peanoKeys.get(p.getPeanoKeyIdx());
1633	pk.removeCar(this.simulatedCarInfo);
1634	}
1635
1636	public void updateCarToPointMapping() {
1637	Road r = Globals.map.roads.get(this.getRoadIdx());
1638	Point p = r.points.get(this.getPointIdx());
1639	PeanoKey pk = Globals.map.peanoKeys.get(p.getPeanoKeyIdx());
1640	pk.addCar(this.simulatedCarInfo);
1641	}
1642
1643	public SimulatedCarInfo getSimulatedCarInfo() {
1644	return simulatedCarInfo;
1645	}
1646
1647	public void setSimulatedCarInfo(SimulatedCarInfo record) {
1648	this.simulatedCarInfo = record;
1649	}
1650
1651	public void finish() {
1652	// System.out.println(" printing...");
1653	double dist = 0.0;
1654	stopTime = (Globals.engine.crtTime * 1000) / Globals.engine.fps;
1655	dist = RouteComputingUtils.getDistanceForRoute(this.route);
1656	CarInstance.logWriter
1657	.println(this.ID + " " + route[0].roadIndex + " " + this.route[0].pt1 + " "
1658	+ this.route[route.length - 1].roadIndex + " " + this.route[route.length - 1].pt1 + " "
1659	+ (stopTime - startTime) + " " + this.routingType + " " + dist + " " + this.routeChangesCount
1660	+ " " + toLateFailCount + " " + noRouteFailCount + " " + sentMsgCount + " " + receivedMsgCount
1661	+ " " + emptyMsgCount + " " + inccorectReplyCount + " " + noChangeNeededCount + " "
1662	+ routeComputingFailCount);
1663	CarInstance.logWriter.flush();
1664	}
1665
1666	}

Note: See TracBrowser for help on using the repository browser.

Context Navigation

source: proiecte/ptvs/src/vnsim/vehicular/simulator/CarInstance.java @ 31

Download in other formats: