[31] | 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 | } |
---|