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 | } |
---|