diff --git a/VectoCore/Models/SimulationComponent/Impl/DistanceBasedDrivingCycle.cs b/VectoCore/Models/SimulationComponent/Impl/DistanceBasedDrivingCycle.cs
index fda1bbc67d00b4d8fb80972d2ff01b77a9c906d4..b71141f5acd6cf68468a15c40f54056f99bf116c 100644
--- a/VectoCore/Models/SimulationComponent/Impl/DistanceBasedDrivingCycle.cs
+++ b/VectoCore/Models/SimulationComponent/Impl/DistanceBasedDrivingCycle.cs
@@ -1,4 +1,6 @@
using System;
+using System.Collections;
+using System.Collections.Generic;
using System.Linq;
using System.Net.Cache;
using TUGraz.VectoCore.Exceptions;
@@ -23,11 +25,15 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
private DrivingCycleState _previousState = null;
private DrivingCycleState _currentState = new DrivingCycleState();
+ private readonly DrivingCycleEnumerator _cycleIntervalIterator;
+
private IDrivingCycleOutPort _outPort;
public DistanceBasedDrivingCycle(IVehicleContainer container, DrivingCycleData cycle) : base(container)
{
Data = cycle;
+ _cycleIntervalIterator = new DrivingCycleEnumerator(Data);
+ _cycleIntervalIterator.MoveNext();
}
#region IDrivingCycleInProvider
@@ -70,29 +76,30 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
private IResponse DoHandleRequest(TimeSpan absTime, Meter ds)
{
- var currentCycleEntry = Data.Entries[_previousState.CycleIndex];
- var nextCycleEntry = Data.Entries[_previousState.CycleIndex + 1];
+ //var currentCycleEntry = Data.Entries[_previousState.CycleIndex];
+ //var nextCycleEntry = Data.Entries[_previousState.CycleIndex + 1];
- if (currentCycleEntry.Distance.IsEqual(_previousState.Distance.Value())) {
+ if (_cycleIntervalIterator.LeftSample.Distance.IsEqual(_previousState.Distance.Value())) {
// exactly on an entry in the cycle...
- if (!currentCycleEntry.StoppingTime.IsEqual(0)
- && currentCycleEntry.StoppingTime.Value() > _previousState.WaitTime.TotalSeconds) {
+ if (!_cycleIntervalIterator.LeftSample.StoppingTime.IsEqual(0)
+ && _cycleIntervalIterator.LeftSample.StoppingTime.Value() > _previousState.WaitTime.TotalSeconds) {
// stop for certain time unless we've already waited long enough...
- if (!currentCycleEntry.VehicleTargetSpeed.IsEqual(0)) {
+ if (!_cycleIntervalIterator.LeftSample.VehicleTargetSpeed.IsEqual(0)) {
Log.WarnFormat("Stopping Time requested in cycle but target-velocity not zero. distance: {0}, target speed: {1}",
- currentCycleEntry.StoppingTime, currentCycleEntry.VehicleTargetSpeed);
+ _cycleIntervalIterator.LeftSample.StoppingTime, _cycleIntervalIterator.LeftSample.VehicleTargetSpeed);
throw new VectoSimulationException("Stopping Time only allowed when target speed is zero!");
}
- var dt = TimeSpan.FromSeconds(currentCycleEntry.StoppingTime.Value()) - _previousState.WaitTime;
+ var dt = TimeSpan.FromSeconds(_cycleIntervalIterator.LeftSample.StoppingTime.Value()) -
+ _previousState.WaitTime;
return DriveTimeInterval(absTime, dt);
}
}
- if (_previousState.Distance + ds > nextCycleEntry.Distance) {
+ if (_previousState.Distance + ds > _cycleIntervalIterator.RightSample.Distance) {
// only drive until next sample point in cycle
// only drive until next sample point in cycle
return new ResponseDrivingCycleDistanceExceeded() {
- MaxDistance = nextCycleEntry.Distance - _previousState.Distance
+ MaxDistance = _cycleIntervalIterator.RightSample.Distance - _previousState.Distance
};
}
@@ -105,35 +112,37 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
_currentState.AbsTime += dt;
_currentState.WaitTime = _previousState.WaitTime + dt;
- var currentCycleEntry = Data.Entries[_currentState.CycleIndex];
-
- _currentState.CycleIndex++;
-
return _outPort.Request(absTime, dt,
- currentCycleEntry.VehicleTargetSpeed, ComputeGradient());
+ _cycleIntervalIterator.LeftSample.VehicleTargetSpeed, ComputeGradient());
}
private IResponse DriveDistance(TimeSpan absTime, Meter ds)
{
_currentState.Distance += ds;
- while (_currentState.Distance >= Data.Entries[_currentState.CycleIndex + 1].Distance) {
- _currentState.CycleIndex++;
+ while (_currentState.Distance >= _cycleIntervalIterator.RightSample.Distance) {
+ //Data.Entries[_currentState.CycleIndex + 1].Distance) {
+ _cycleIntervalIterator.MoveNext();
}
- var leftSamplePoint = Data.Entries[_currentState.CycleIndex];
- var rightSamplePoint = Data.Entries[_currentState.CycleIndex + 1];
- _currentState.Altitude = VectoMath.Interpolate(leftSamplePoint.Distance, rightSamplePoint.Distance,
- leftSamplePoint.Altitude, rightSamplePoint.Altitude, _currentState.Distance);
-
- _currentState.VehicleTargetSpeed = Data.Entries[_currentState.CycleIndex].VehicleTargetSpeed;
+ _currentState.VehicleTargetSpeed = _cycleIntervalIterator.LeftSample.VehicleTargetSpeed;
return _outPort.Request(absTime, ds, _currentState.VehicleTargetSpeed, ComputeGradient());
}
private Radian ComputeGradient()
{
- var gradient = VectoMath.InclinationToAngle(((_currentState.Altitude - _previousState.Altitude) /
- (_currentState.Distance - _previousState.Distance)).Value());
+ var leftSamplePoint = _cycleIntervalIterator.LeftSample;
+ var rightSamplePoint = _cycleIntervalIterator.RightSample;
+
+ var gradient = leftSamplePoint.RoadGradient;
+
+ if (!leftSamplePoint.Distance.IsEqual(rightSamplePoint.Distance)) {
+ _currentState.Altitude = VectoMath.Interpolate(leftSamplePoint.Distance, rightSamplePoint.Distance,
+ leftSamplePoint.Altitude, rightSamplePoint.Altitude, _currentState.Distance);
+
+ gradient = VectoMath.InclinationToAngle(((_currentState.Altitude - _previousState.Altitude) /
+ (_currentState.Distance - _previousState.Distance)).Value());
+ }
return gradient;
}
@@ -150,7 +159,8 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
WaitTime = TimeSpan.FromSeconds(0),
Distance = first.Distance,
Altitude = first.Altitude,
- CycleIndex = 0,
+ //CycleIndex = 0,
+ //CycleInterval = new DrivingCycleEnumerator(Data)
};
_currentState = _previousState.Clone();
return new ResponseSuccess();
@@ -167,6 +177,7 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
{
_previousState = _currentState;
_currentState = _currentState.Clone();
+ _cycleIntervalIterator.MoveNext();
}
#endregion
@@ -174,6 +185,62 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
protected void LookupCycle(Meter ds) {}
+ public class DrivingCycleEnumerator : IEnumerator<DrivingCycleData.DrivingCycleEntry>
+ {
+ protected IEnumerator<DrivingCycleData.DrivingCycleEntry> LeftSampleIt;
+ protected IEnumerator<DrivingCycleData.DrivingCycleEntry> RightSampleIt;
+
+ public DrivingCycleEnumerator(DrivingCycleData data)
+ {
+ LeftSampleIt = data.Entries.GetEnumerator();
+ RightSampleIt = data.Entries.GetEnumerator();
+ RightSampleIt.MoveNext();
+ }
+
+ public DrivingCycleData.DrivingCycleEntry Current
+ {
+ get { return LeftSampleIt.Current; }
+ }
+
+ public DrivingCycleData.DrivingCycleEntry Next
+ {
+ get { return RightSampleIt.Current; }
+ }
+
+ public DrivingCycleData.DrivingCycleEntry LeftSample
+ {
+ get { return LeftSampleIt.Current; }
+ }
+
+ public DrivingCycleData.DrivingCycleEntry RightSample
+ {
+ get { return RightSampleIt.Current; }
+ }
+
+ public void Dispose()
+ {
+ LeftSampleIt.Dispose();
+ RightSampleIt.Dispose();
+ }
+
+ object System.Collections.IEnumerator.Current
+ {
+ get { return LeftSampleIt.Current; }
+ }
+
+ public bool MoveNext()
+ {
+ return LeftSampleIt.MoveNext() && RightSampleIt.MoveNext();
+ }
+
+ public void Reset()
+ {
+ LeftSampleIt.Reset();
+ RightSampleIt.Reset();
+ RightSampleIt.MoveNext();
+ }
+ }
+
public class DrivingCycleState
{
public DrivingCycleState() {}
@@ -185,7 +252,8 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
Distance = Distance,
VehicleTargetSpeed = VehicleTargetSpeed,
Altitude = Altitude,
- CycleIndex = CycleIndex,
+ //CycleIndex = CycleIndex,
+ //CycleInterval = CycleInterval,
// WaitTime is not cloned on purpose!
WaitTime = TimeSpan.FromSeconds(0)
};
@@ -201,7 +269,7 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
public Meter Altitude;
- public int CycleIndex;
+ //public int CycleIndex;
}
}
}
\ No newline at end of file