/// <summary> /// Calculates driving parameters and new acceleration of the vehicle. /// </summary> /// <param name="route">Route of the Vehicle.</param> /// <param name="arcPos">Current arc position of the vehicle on the first NodeConnection on <paramref name="route"/>.</param> /// <param name="onlySimpleCalculations">Perform only simple calculations (e.g. no free line changes).</param> /// <param name="tickLength">Length of a tick in seconds.</param> /// <returns></returns> public double Think(List<NodeConnection> route, double arcPos, bool onlySimpleCalculations, double tickLength) { if (route.Count == 0) return 0; double lookaheadDistance = Constants.lookaheadDistance; double intersectionLookaheadDistance = Constants.intersectionLookaheadDistance; double stopDistance = -1; _state._freeDrive = true; bool thinkAboutLineChange = false; double lowestAcceleration = 0; #region LineChangeVehicleInteraction // if necessary, wait for other vehicle to change line if (_state.letVehicleChangeLine) { double percentOfLCILeft = (lci == null) ? 0.2 : Math.Max(0.2, (lci.endArcPos - currentPosition - Constants.breakPointBeforeForcedLineChange) / (lci.length - Constants.breakPointBeforeForcedLineChange)); lookaheadDistance = Math.Max(3 * percentOfLCILeft * s0, _state.tailPositionOfOtherVehicle - currentPosition); thinkAboutLineChange = false; lowestAcceleration = CalculateAcceleration(physics.velocity, effectiveDesiredVelocity, lookaheadDistance, physics.velocity); _state._freeDrive = false; } #endregion #region Vehicles in front // Determine the next vehicle in front. VehicleDistance theVehicleInFrontOfMe = GetNextVehicleOnMyTrack(route[0], arcPos, lookaheadDistance); // The stored distance is to the front of the vehicle. All following calculations need the distance // to its tail. Hence, substract the vehicle length. if (theVehicleInFrontOfMe != null && theVehicleInFrontOfMe.distance < lookaheadDistance) { lookaheadDistance = theVehicleInFrontOfMe.distance; thinkAboutLineChange = true; lowestAcceleration = CalculateAcceleration(physics.velocity, effectiveDesiredVelocity, theVehicleInFrontOfMe.distance, physics.velocity - theVehicleInFrontOfMe.vehicle.physics.velocity); if (lowestAcceleration < 0.1) _state._freeDrive = false; if ( (theVehicleInFrontOfMe.vehicle.physics.velocity < 2.5) || (theVehicleInFrontOfMe.vehicle.physics.velocity < 5 && theVehicleInFrontOfMe.vehicle.physics.acceleration < 0.1)) { stopDistance = theVehicleInFrontOfMe.distance; } } else { lowestAcceleration = CalculateAcceleration(physics.velocity, effectiveDesiredVelocity, lookaheadDistance, physics.velocity); } #endregion #region Stop Signs Pair<LineNode, double> nextStopSign = GetDistanceToNextStopSignOnRoute(route, arcPos,lookaheadDistance); if (nextStopSign.Left != null && nextStopSign.Left != _stopSignToIgnore) { if (isStopped) { _stopSignToIgnore = nextStopSign.Left; } else { lookaheadDistance = nextStopSign.Right; thinkAboutLineChange = false; lowestAcceleration = CalculateAcceleration(physics.velocity, effectiveDesiredVelocity, nextStopSign.Right, physics.velocity); _state._freeDrive = false; } } #endregion #region Traffic lights // Check for red traffic lights on route double distanceToTrafficLight = GetDistanceToNextTrafficLightOnRoute(route, arcPos, Constants.lookaheadDistance, true); intersectionLookaheadDistance = distanceToTrafficLight; // If the next TrafficLight is closer than the next vehicle, no free line change shall be performed if (distanceToTrafficLight < lookaheadDistance) { lookaheadDistance = distanceToTrafficLight; thinkAboutLineChange = false; lowestAcceleration = CalculateAcceleration(physics.velocity, effectiveDesiredVelocity, lookaheadDistance, physics.velocity); _state._freeDrive = false; } #endregion #region Intersections // Registration target for intersections. // (When doing simple calculations, we do not want to unregister at all intersections. Hence, we use a temporary regsitration): LinkedList<SpecificIntersection> registrationTarget = (onlySimpleCalculations ? temporaryRegisteredIntersections : registeredIntersections); // gather all upcoming intersections (and update the ones we are already registered at) GatherNextIntersectionsOnMyTrack(route, arcPos, registrationTarget, intersectionLookaheadDistance); double distanceToIntersection = HandleIntersections(registrationTarget, stopDistance); // If there is an intersection where I should wait, I should do so... if (!Double.IsPositiveInfinity(distanceToIntersection))// && distanceToIntersection < lookaheadDistance) { lookaheadDistance = distanceToIntersection; double newAcceleration = CalculateAcceleration(physics.velocity, effectiveDesiredVelocity, distanceToIntersection, physics.velocity); lowestAcceleration = Math.Min(lowestAcceleration, newAcceleration); } // In case of simple calculations we need to unregister the vehicle from all intersections. Otherwise // some other vehicle might wait for this vehicle even if it will never pass the intersection. // (simple calculations are only hypothetical) if (onlySimpleCalculations) { foreach (SpecificIntersection si in registrationTarget) { si.intersection.UnregisterVehicle(this, si.nodeConnection); } registrationTarget.Clear(); } #endregion #region Line changes // simple calculation do not consider line changes if (!onlySimpleCalculations) { #region Forced line changes // our route forces to perform a line change if (lineChangeNeeded && !currentlyChangingLine && lci != null) { thinkAboutLineChange = false; lastLineChangeCheck.Left = GlobalTime.Instance.currentTime; lastLineChangeCheck.Right = currentPosition; // get current LineChangePoint and check, whether it's leading to our target NodeConnection.LineChangePoint lcp = route[0].GetPrevLineChangePoint(arcPos); if (lci.targetNode.prevConnections.Contains(lcp.target.nc)) { bool slowDownToBreakPoint = false; double myArcPositionOnOtherConnection = lcp.otherStart.arcPosition + (arcPos - lcp.start.arcPosition); // check if found LineChangePoint is not too far away to perform the line change if ((myArcPositionOnOtherConnection >= 0) && (Math.Abs(arcPos - lcp.start.arcPosition) < Constants.maxDistanceToLineChangePoint * 1.25)) { // Check the relation to my surrounding vehicles on the target NodeConnection Pair<VehicleDistance> otherVehicles = lcp.otherStart.nc.GetVehiclesAroundArcPosition(myArcPositionOnOtherConnection, Constants.lookaheadDistance); // the new vehicle in front wouldn't be too close if ( otherVehicles.Right == null || otherVehicles.Right.distance > otherVehicles.Right.vehicle.length + CalculateWantedDistance(physics.velocity, physics.velocity - otherVehicles.Right.vehicle.physics.velocity)/2) { // the new vehicle behind wouldn't be too close if ( otherVehicles.Left == null || otherVehicles.Left.distance > length + otherVehicles.Left.vehicle.CalculateWantedDistance(otherVehicles.Left.vehicle.physics.velocity, otherVehicles.Left.vehicle.physics.velocity - physics.velocity)/2) { // calculate my necessary acceleration in case of a line change double myAccelerationOnOtherConnection = (otherVehicles.Right != null) ? CalculateAcceleration(physics.velocity, effectiveDesiredVelocity, otherVehicles.Right.vehicle.currentPosition - myArcPositionOnOtherConnection, physics.velocity - otherVehicles.Right.vehicle.physics.velocity) : CalculateAcceleration(physics.velocity, effectiveDesiredVelocity, lookaheadDistance, physics.velocity); // calculate the necessary acceleration of the vehicle behind in case of a line change double forcedAccelerationOfVehicleBehindMeOnOtherConnection = (otherVehicles.Left != null) ? CalculateAcceleration(otherVehicles.Left.vehicle.physics.velocity, otherVehicles.Left.vehicle.effectiveDesiredVelocity, otherVehicles.Left.distance, otherVehicles.Left.vehicle.physics.velocity - physics.velocity) : 0; double currentAccelerationOfVehicleBehindMeOnOtherConnection = (otherVehicles.Left != null) ? otherVehicles.Left.vehicle.physics.acceleration : 0; // Final check: // - The new vehicle behind must not break harder than bSave // - My line change must be sufficiently necessary. The closer I come to the end of the LineChangeInterval, the more I may thwart the vehicle behind. if ( (forcedAccelerationOfVehicleBehindMeOnOtherConnection > bSave) && ((arcPos - lci.startArcPos) / lci.length >= (currentAccelerationOfVehicleBehindMeOnOtherConnection - forcedAccelerationOfVehicleBehindMeOnOtherConnection))) { // return to normal velocity _physics.multiplierTargetVelocity = 1; // initiate the line change InitiateLineChange(lcp, arcPos - lcp.start.arcPosition); lowestAcceleration = myAccelerationOnOtherConnection; } // I do not want to change line yet, but I could position myself better between the two other vehicles on the parallel line. else if (true) { // TODO: implement slowDownToBreakPoint = true; } } // the new vehicle behind would too close but I can accelerate and are at least as fast as him else if ( otherVehicles.Left.vehicle.physics.velocity / this.physics.velocity < 1.2 // I am not significantly slower than him && (otherVehicles.Right == null || otherVehicles.Right.distance > 1.5 * length) // the new vehicle in front is far enough away && lookaheadDistance > 2 * length // no vehicle/traffic light/intersection in front && lci.endArcPos - arcPos > 2 * length // enough space left in LineChangeInterval && lowestAcceleration >= -0.1) // currently not braking { // accelerate to get in front _physics.multiplierTargetVelocity = 1.75; lowestAcceleration = Math.Min(lowestAcceleration, CalculateAcceleration(physics.velocity, effectiveDesiredVelocity, lookaheadDistance, physics.velocity)); _state.SetLineChangeVehicleInteraction(this, otherVehicles.Left.vehicle, lcp.otherStart.nc, myArcPositionOnOtherConnection - _length); } // There is no way to perform a line change now => slow down else { slowDownToBreakPoint = true; } } // The new vehicle in front is too close => slow down else { slowDownToBreakPoint = true; } } if (slowDownToBreakPoint) { double percentOfLCILeft = Math.Max(0.2, (lci.endArcPos - currentPosition - Constants.breakPointBeforeForcedLineChange) / (lci.length - Constants.breakPointBeforeForcedLineChange)); // slow down a bit _physics.multiplierTargetVelocity = Math.Min(0.9, 1.5 * percentOfLCILeft); // When reaching the end of the LineChangeInterval, check whether there are other possibilities to reach the target: if (percentOfLCILeft < 0.5) { Routing newRTT = Routing.CalculateShortestConenction(route[0].endNode, _targetNodes, _vehicleType); // The alternative route does not cost too much -> choose it if (newRTT.SegmentCount() > 0 && newRTT.costs / _wayToGo.costs < Constants.maxRatioForEnforcedLineChange) { _wayToGo = newRTT; _physics.multiplierTargetVelocity = 1; lineChangeNeeded = false; lci = null; } } // Line change still necessacy => stop at break point if (lineChangeNeeded) { if (! _state.letVehicleChangeLine && percentOfLCILeft < 0.8) { VehicleDistance otherBehind = lcp.otherStart.nc.GetVehicleBeforeArcPosition(myArcPositionOnOtherConnection - ((_length + s0)), Constants.lookaheadDistance); // In rare cases deadlocks may appear when a very long and a short vehicle are driving parallel to each other (and both want to change line): // Then, none of the two finds a vehicle behind on the parallel connection. Hence, none of the two will wait for the other one // and both might drive to the end of the line change interval and there block each other. // To avoid this case, if there is no otherBehind, we also look for a parallel vehicle in front of our back (there should be one, otherwise // something went terribly wrong above). The longer one of the two will wait for the shorter one to make sure, no deadlock will occur. if (otherBehind == null) { VehicleDistance otherFront = lcp.otherStart.nc.GetVehicleBehindArcPosition(myArcPositionOnOtherConnection - ((_length + s0)), Constants.lookaheadDistance); if (otherFront.vehicle.lineChangeNeeded && otherFront.vehicle._length > _length) { otherBehind = otherFront; otherBehind.distance *= -1; } } //Pair<VehicleDistance> vd = lcp.otherStart.nc.GetVehiclesAroundArcPosition(myArcPositionOnOtherConnection - ( (_length + s0)), Constants.lookaheadDistance); if (otherBehind != null)// && otherBehind.vehicle.p >= p) { // tell the vehicle behind my back to wait for me _state.SetLineChangeVehicleInteraction(this, otherBehind.vehicle, lcp.otherStart.nc, myArcPositionOnOtherConnection - _length); // In addition, I need to get behind the vehicle in front of the vehicle which waits for me. Therefore I adapt the desired velocity if (_state.vehicleThatLetsMeChangeLine != null) { VehicleDistance otherBehindForman = _state.vehicleThatLetsMeChangeLine.currentNodeConnection.GetVehicleBehindArcPosition(_state.vehicleThatLetsMeChangeLine.currentPosition, 2 * (length + s0)); if (otherBehindForman != null) { //_physics.multiplierTargetVelocity = Math2.Clamp(Math2.Cubic((otherBehindForman.distance - otherBehind.distance - s0) / (_length + 4 * s0)), 0.3, 1); double multPerDistance = 1 - Math2.Clamp((otherBehind.distance + _length + s0 - otherBehindForman.distance + otherBehindForman.vehicle._length + s0) / (otherBehindForman.vehicle._length), 0.2, 0.75); double multPerSpeedDiff = Math2.Clamp((otherBehindForman.vehicle._physics.velocity - _physics.velocity) / 2, 0.25, 0.8); _physics.multiplierTargetVelocity = Math.Min(multPerDistance, multPerSpeedDiff); } } } } lowestAcceleration = Math.Min(lowestAcceleration, CalculateAcceleration(physics.velocity, effectiveDesiredVelocity, lci.endArcPos - Constants.breakPointBeforeForcedLineChange - arcPos, physics.velocity)); } } } } else if (_state.vehicleThatLetsMeChangeLine != null) { _state.UnsetLineChangeVehicleInteraction(); } #endregion #region freiwillig thinkAboutLineChange &= ((GlobalTime.Instance.currentTime - lastLineChangeCheck.Left > 1) || (currentPosition - lastLineChangeCheck.Right > 50)); if (thinkAboutLineChange && !currentlyChangingLine) { lastLineChangeCheck.Left = GlobalTime.Instance.currentTime; lastLineChangeCheck.Right = currentPosition; // get current LineChangePoint and check, whether it's reachable NodeConnection.LineChangePoint lcp = route[0].GetPrevLineChangePoint(arcPos); if ((lcp.target.nc != null) && (Math.Abs(arcPos - lcp.start.arcPosition) < Constants.maxDistanceToLineChangePoint * 0.67)) { // check whether there is an alternative route that is not too costly Routing alternativeRoute = Routing.CalculateShortestConenction(lcp.target.nc.endNode, targetNodes, _vehicleType); if (alternativeRoute.SegmentCount() > 0 && alternativeRoute.costs / wayToGo.costs < Constants.maxRatioForVoluntaryLineChange && !alternativeRoute.Top().lineChangeNeeded) { double myArcPositionOnOtherConnection = lcp.otherStart.arcPosition + (arcPos - lcp.start.arcPosition); if (myArcPositionOnOtherConnection >= 0) { // Check the relation to my surrounding vehicles on the target NodeConnection Pair<VehicleDistance> otherVehicles = lcp.otherStart.nc.GetVehiclesAroundArcPosition(myArcPositionOnOtherConnection, Constants.lookaheadDistance); // the new vehicle in front wouldn't be too close if ( otherVehicles.Right == null || otherVehicles.Right.distance > otherVehicles.Right.vehicle.length + 2 * lcp.length) { // the new vehicle behind wouldn't be too close if ( otherVehicles.Left == null || otherVehicles.Left.distance > length + otherVehicles.Left.vehicle.CalculateWantedDistance(otherVehicles.Left.vehicle.physics.velocity, otherVehicles.Left.vehicle.physics.velocity - physics.velocity)/2) { List<NodeConnection> l = new List<NodeConnection>(); l.Add(lcp.target.nc); foreach (Routing.RouteSegment rs in alternativeRoute) l.Add(rs.startConnection); // calculate my necessary acceleration in case of a line change double myAccelerationOnOtherConnection = (otherVehicles.Right != null) ? CalculateAcceleration(physics.velocity, effectiveDesiredVelocity, otherVehicles.Right.vehicle.currentPosition - myArcPositionOnOtherConnection, physics.velocity - otherVehicles.Right.vehicle.physics.velocity) : CalculateAcceleration(physics.velocity, effectiveDesiredVelocity, GetDistanceToNextTrafficLightOnRoute(l, myArcPositionOnOtherConnection, Constants.lookaheadDistance, true), physics.velocity); // calculate the necessary acceleration of the vehicle behind in case of a line change double forcedAccelerationOfVehicleBehindMeOnOtherConnection = (otherVehicles.Left != null) ? CalculateAcceleration(otherVehicles.Left.vehicle.physics.velocity, otherVehicles.Left.vehicle.effectiveDesiredVelocity, otherVehicles.Left.distance, otherVehicles.Left.vehicle.physics.velocity - physics.velocity) : 0; double currentAccelerationOfVehicleBehindMeOnOtherConnection = (otherVehicles.Left != null) ? otherVehicles.Left.vehicle.physics.acceleration : 0; // simplified implementation of MOBIL: http://www.vwi.tu-dresden.de/~treiber/MicroApplet/MOBIL.html if (forcedAccelerationOfVehicleBehindMeOnOtherConnection > bSave) { if (myAccelerationOnOtherConnection - lowestAcceleration > p * (currentAccelerationOfVehicleBehindMeOnOtherConnection - forcedAccelerationOfVehicleBehindMeOnOtherConnection) + lineChangeThreshold) { // initiate the line change InitiateLineChange(lcp, arcPos - lcp.start.arcPosition); lowestAcceleration = myAccelerationOnOtherConnection; } } } } } } } } #endregion } #endregion return lowestAcceleration; }
/// <summary> /// Initiiert einen Spurwechsel /// </summary> /// <param name="lcp">LCP auf dem der Spurwechsel durchgeführt werden soll</param> /// <param name="arcPositionOffset">bereits auf dem LCP zurückgelegte Strecke</param> private void InitiateLineChange(NodeConnection.LineChangePoint lcp, double arcPositionOffset) { // einem evtl. ausgebremsten Fahrzeug sagen, dass es nicht mehr extra für mich abbremsen braucht _state.UnsetLineChangeVehicleInteraction(); // unregister at all intersections foreach (SpecificIntersection si in registeredIntersections) { si.intersection.UnregisterVehicle(this, si.nodeConnection); } registeredIntersections.Clear(); // sich merken, dass das IVehicle gerade am Spurwechseln ist currentlyChangingLine = true; currentLineChangePoint = lcp; currentPositionOnLineChangePoint = arcPositionOffset; if (lcp.target.nc == lcp.otherStart.nc) { ratioProjectionOnTargetConnectionvsLCPLength = (lcp.target.arcPosition - lcp.otherStart.arcPosition) / lcp.lineSegment.length; } else { ratioProjectionOnTargetConnectionvsLCPLength = (lcp.target.arcPosition + lcp.otherStart.nc.lineSegment.length - lcp.otherStart.arcPosition) / lcp.lineSegment.length; } // Neuen State schonmal auf die ZielNodeConnection setzen (currentNodeConnection, currentPosition) RemoveFromCurrentNodeConnection(true, lcp.otherStart.nc, lcp.otherStart.arcPosition + arcPositionOffset * ratioProjectionOnTargetConnectionvsLCPLength); _wayToGo = Routing.CalculateShortestConenction(currentNodeConnection.endNode, targetNodes, _vehicleType); _statistics.numLineChanges++; lineChangeNeeded = false; lci = null; }
/// <summary> /// Bewegt das Auto /// </summary> /// <param name="tickLength">Länge eines Ticks in Sekunden (berechnet sich mit 1/#Ticks pro Sekunde)</param> public void Move(double tickLength) { if (!alreadyMoved) { _physics.velocity += physics.acceleration; // Rückwärts fahren geht nicht if (_physics.velocity < 0) _physics.velocity = 0; double arcLengthToMove = (physics.velocity * tickLength * 10); if (_physics.velocity < 0.1) { if (!isStopped) { ++_statistics.numStops; } isStopped = true; } else { isStopped = false; } // wenn ich gerade am Spurwechseln bin, sollte ich das erstmal behandeln if (currentlyChangingLine) { currentPositionOnLineChangePoint += arcLengthToMove; // ich bewege mich echt auf dem LCP _state.position += arcLengthToMove * ratioProjectionOnTargetConnectionvsLCPLength; // ich muss meine Position auf der Ziel-NodeConnection entsprechend anpassen } else { _state.position += arcLengthToMove; } // wenn meine aktuelle NodeConnection zu Ende ist, sollte ich das auch behandeln if (currentPosition > currentNodeConnection.lineSegment.length) { // gucken, ob es mit ner Connection weitergeht if ((currentNodeConnection.endNode.nextConnections.Count != 0) && (wayToGo.SegmentCount() > 0)) { _physics.multiplierTargetVelocity = 1; _state.UnsetLineChangeVehicleInteraction(); double startDistance = (currentPosition - currentNodeConnection.lineSegment.length); // falls ich mehrere Connections zur Auswahl habe, berechne die mit dem kürzesten Weg // (dieser könnte sich geändert haben, weil dort plötzlich mehr Autos fahren) if (currentNodeConnection.endNode.nextConnections.Count > 1) { _wayToGo = Routing.CalculateShortestConenction(currentNodeConnection.endNode, targetNodes, _vehicleType); if (_wayToGo.SegmentCount() == 0 || _wayToGo.Top() == null) { RemoveFromCurrentNodeConnection(true, null, 0); return; } } visitedNodeConnections.AddFirst(currentNodeConnection); // nächsten Wegpunkt extrahieren Routing.RouteSegment rs = wayToGo.Pop(); if (rs == null) { RemoveFromCurrentNodeConnection(true, null, 0); return; } else { // ist ein Spurwechsel nötig, so die entsprechenden Felder füllen if (rs.lineChangeNeeded) rs.startConnection.lineChangeIntervals.TryGetValue(rs.nextNode.hashcode, out lci); else lci = null; lineChangeNeeded = (lci != null); LinkedListNode<IVehicle> lln = rs.startConnection.GetVehicleListNodeBehindArcPosition(startDistance); if (lln == null || lln.Value.currentPosition - lln.Value.length >= startDistance) { RemoveFromCurrentNodeConnection(true, rs.startConnection, startDistance); } else { RemoveFromCurrentNodeConnection(true, null, 0); } } } else { // Ende der Fahnenstange, also selbstzerstören RemoveFromCurrentNodeConnection(true, null, 0); } } else if (Double.IsNaN(currentPosition)) { RemoveFromCurrentNodeConnection(false, null, 0); } // Der Spurwechsel ist fertig, dann sollte ich diesen auch abschließen: if (currentlyChangingLine && currentPositionOnLineChangePoint >= currentLineChangePoint.lineSegment.length) { FinishLineChange(currentPositionOnLineChangePoint - currentLineChangePoint.lineSegment.length); _statistics.startTimeOnNodeConnection = GlobalTime.Instance.currentTime; _statistics.arcPositionOfStartOnNodeConnection = _state.position; } alreadyMoved = true; } }
/// <summary> /// schließt den Spurwechsel ab /// </summary> /// <param name="arcPositionOffset">Bogenlänge über die das Fahrzeug bereits über die Länge des LCP hinaus ist</param> private void FinishLineChange(double arcPositionOffset) { _state.position = currentLineChangePoint.target.arcPosition + arcPositionOffset; lastLineChangeCheck.Left = GlobalTime.Instance.currentTime; lastLineChangeCheck.Right = currentPosition; //m_WayToGo = CalculateShortestConenction(currentNodeConnection.endNode, m_TargetNodes); currentlyChangingLine = false; lineChangeNeeded = false; lci = null; _physics.multiplierTargetVelocity = 1; // Tell other vehicle that waits for me, that I'm finished. Kinda redundant, but safe is safe. _state.UnsetLineChangeVehicleInteraction(); }