private bool IsTrainApproaching(TrackBlock curr, TrackBlock dest)
{
if (dest == null)
{
throw new ArgumentNullException();
}
List <TrackBlock> blocks = new List <TrackBlock>(m_trackBlocks.Values);
while (blocks.Count > 0)
{
TrackBlock b = blocks.ElementAt <TrackBlock>(0);
blocks.Remove(b);
if (b == curr)
{
continue;
}
if (b.Status.TrainPresent)
{
for (TrackBlock t = b.GetNextBlock(b.Status.TrainDirection);
t != null && blocks.Contains <TrackBlock>(t) &&
t.Status.IsOpen && !t.Status.TrainPresent;
t = (t.NextBlock == t) ? t.PreviousBlock : t.NextBlock)
{
if (t == dest)
{
return(true);
}
blocks.Remove(t);
}
}
}
return(false);
}
public void GetNextBlockTest()
{
TrackBlock target = new TrackBlock(); // TODO: Initialize to an appropriate value
TrackBlock NextBlock = new TrackBlock();
target.NextBlock = NextBlock;
Direction direction = Direction.East; // TODO: Initialize to an appropriate value
TrackBlock expected = null; // TODO: Initialize to an appropriate value
TrackBlock actual;
actual = target.GetNextBlock(direction);
Assert.AreEqual(expected, actual);
}
// METHOD: DetermineSetPoint
//--------------------------------------------------------------------------------------
/// <summary>
/// Determine the set point
/// </summary>
//--------------------------------------------------------------------------------------
private void DetermineSetPoint()
{
// If a manual speed command has been given and does not exceed the speed limit, make the manual speed the setpoint
// Else, the setpoint should be set to the speed limit
if (ManualMode && ManualSpeed >= 0)
{
if (ManualSpeed < m_currentBlock.Authority.SpeedLimitKPH)
{
m_setPoint = ManualSpeed / 3.6;
}
else
{
m_setPoint = m_currentBlock.Authority.SpeedLimitKPH / 3.6;
}
}
else
{
m_setPoint = m_currentBlock.Authority.SpeedLimitKPH / 3.6;
}
// The setpoint should never be negative
if (m_setPoint < 0)
{
m_setPoint = 0;
}
// If the authority is negative, the train has exceeded its authority and must apply the emergency brake
// If the authority is zero, the train cannot pass into the next block
// So, when the train reaches its stopping distance, the setPoint must be set to zero to engage the brake.
// If the authority is zero and the train is stopped, the train should stay stopped, so the setpoint should be zero
// If the next block has a lower speed limit, the setpoint must be reduced
if (m_currentBlock.Authority.Authority < 0)
{
EmergencyBrake = true;
}
else if (m_currentBlock.Authority.Authority == 0 && (CalculateStoppingDistance(0) >= m_currentBlock.LengthMeters - m_currentState.BlockProgress || (!m_atStation && m_currentState.Speed <= 2)))
{
m_setPoint = 0;
}
else if (m_currentBlock.GetNextBlock(m_currentState.Direction) == null)
{
m_setPoint = 0;
}
else if (m_currentBlock.GetNextBlock(m_currentState.Direction).Authority.SpeedLimitKPH < m_currentBlock.Authority.SpeedLimitKPH && CalculateStoppingDistance(m_currentBlock.GetNextBlock(m_currentState.Direction).Authority.SpeedLimitKPH / 3.6) >= m_currentBlock.LengthMeters - m_currentState.BlockProgress)
{
m_setPoint = m_currentBlock.GetNextBlock(m_currentState.Direction).Authority.SpeedLimitKPH / 3.6;
}
// If the train is approaching a station and is reaches the safe stopping distance, the setpoint should be zet to zero
if (m_approachingStation)
{
if (m_currentBlock.HasTransponder && m_currentBlock.Transponder.DistanceToStation == 1 && CalculateStoppingDistance(0) >= m_currentBlock.LengthMeters + m_currentBlock.GetNextBlock(m_currentState.Direction).LengthMeters * 0.5 - m_currentState.BlockProgress)
{
m_setPoint = 0;
}
else if (m_currentBlock.HasTransponder && m_currentBlock.Transponder.DistanceToStation == 0 && CalculateStoppingDistance(0) >= m_currentBlock.LengthMeters * 0.5 - m_currentState.BlockProgress)
{
m_setPoint = 0;
}
}
}
/// <summary>
/// Updates the position of the Train.
/// </summary>
/// <param name="deltaTime">The number of seconds elapsed since last update.</param>
private void UpdatePosition(double deltaTime)
{
double timestep = deltaTime;
double distance = timestep * state.Speed;
TrackBlock block = state.CurrentBlock;
int startX = block.StartPoint.X;
int startY = block.StartPoint.Y;
int endX = block.EndPoint.X;
int endY = block.EndPoint.Y;
double length = block.LengthMeters;
switch (state.Direction)
{
case Direction.East:
state.X += distance;
state.BlockProgress = (state.X - startX);
break;
case Direction.North:
state.Y -= distance;
state.BlockProgress = (state.Y - startY);
break;
case Direction.Northeast:
distance /= Math.Sqrt(2);
state.X += distance;
state.Y -= distance;
state.BlockProgress = Math.Sqrt(Math.Pow(state.Y - startY, 2) + Math.Pow(state.X - startX, 2));
break;
case Direction.Northwest:
distance /= Math.Sqrt(2);
state.X -= distance;
state.Y -= distance;
state.BlockProgress = Math.Sqrt(Math.Pow(state.Y - endY, 2) + Math.Pow(state.X - endX, 2));
break;
case Direction.South:
state.Y += distance;
state.BlockProgress = (state.Y - endY);
break;
case Direction.Southeast:
distance /= Math.Sqrt(2);
state.X += distance;
state.Y += distance;
state.BlockProgress = Math.Sqrt(Math.Pow(state.Y - startY, 2) + Math.Pow(state.X - startX, 2));
break;
case Direction.Southwest:
distance /= Math.Sqrt(2);
state.X -= distance;
state.Y += distance;
state.BlockProgress = Math.Sqrt(Math.Pow(state.Y - endY, 2) + Math.Pow(state.X - endX, 2));
break;
case Direction.West:
state.X -= distance;
state.BlockProgress = (state.X - endX);
break;
default:
break; // Unreachable
}
state.BlockProgress = Math.Abs(state.BlockProgress);
// Move to next block
if (state.BlockProgress > length)
{
// Subtract length of previous block from progress
state.BlockProgress -= length;
// Fire an event to alert the block
if (TrainEnteredNewBlock != null)
{
TrainEnteredNewBlock(state.CurrentBlock, state.CurrentBlock.GetNextBlock(state.Direction));
}
// Move train's presence to next block
state.CurrentBlock.Status.TrainPresent = false;
state.CurrentBlock = block.GetNextBlock(state.Direction);
state.CurrentBlock.Status.TrainPresent = true;
// Set the train's direction according to new block
switch (state.CurrentBlock.Orientation)
{
case TrackOrientation.EastWest:
if (state.Direction == Direction.East || state.Direction == Direction.Northeast || state.Direction == Direction.Southeast)
{
state.Direction = Direction.East;
}
else if (state.Direction == Direction.West || state.Direction == Direction.Northwest || state.Direction == Direction.Southwest)
{
state.Direction = Direction.West;
}
else
{
Debug.Fail("Invalid transition to EastWest block!");
}
break;
case TrackOrientation.NorthSouth:
if (state.Direction == Direction.North || state.Direction == Direction.Northeast || state.Direction == Direction.Northwest)
{
state.Direction = Direction.North;
}
else if (state.Direction == Direction.South || state.Direction == Direction.Southeast || state.Direction == Direction.Southwest)
{
state.Direction = Direction.South;
}
else
{
Debug.Fail("Invalid transition to NorthSouth block!");
}
break;
case TrackOrientation.NorthWestSouthEast:
if (state.Direction == Direction.North || state.Direction == Direction.West || state.Direction == Direction.Northwest)
{
state.Direction = Direction.Northwest;
}
else if (state.Direction == Direction.South || state.Direction == Direction.East || state.Direction == Direction.Southeast)
{
state.Direction = Direction.Southeast;
}
else
{
Debug.Fail("Invalid transition to NorthwestSoutheast block!");
}
break;
case TrackOrientation.SouthWestNorthEast:
if (state.Direction == Direction.South || state.Direction == Direction.West || state.Direction == Direction.Southwest)
{
state.Direction = Direction.Southwest;
}
else if (state.Direction == Direction.North || state.Direction == Direction.East || state.Direction == Direction.Northeast)
{
state.Direction = Direction.Northeast;
}
else
{
Debug.Fail("Invalid transition to NorthSouth block!");
}
break;
default:
break; // Unreachable
}
// Set the train's direction in the TrainBlock
state.CurrentBlock.Status.TrainDirection = state.Direction;
// Update the slope according to the new block
slope = Math.Atan(block.Grade / 100.0);
}
}