public bool ShouldChange( Car car )
{
var distance = this._laneCorner.BuildControl.Location - car.Location;
if ( distance.Length() <= 0.001f ) { return true; }
return Math.Sign( distance.X ) != Math.Sign( car.Direction.X ) && Math.Sign( distance.Y ) != Math.Sign( car.Direction.Y );
}
public CarAhedInformation GetCarAheadDistance( Car car )
{
if ( this._cars.Contains( car ) )
{
var carAhed = this._cars.GetCarAheadOf( car );
if ( carAhed != null )
{
return new CarAhedInformation
{
CarAhead = carAhed,
CarDistance = Vector2.Distance( car.Location, carAhed.Location ),
};
}
return CarAhedInformation.Empty;
}
var firstCar = this._cars.GetLastCar();
if ( firstCar != null )
{
return new CarAhedInformation
{
CarAhead = firstCar,
CarDistance = 0.0f,
};
}
return CarAhedInformation.Empty;
}
public bool ShouldChange( Car car )
{
var distance = this._lane.RoadLaneBlock.RightEdge.Location - car.Location;
if ( distance.Length() <= 0.001f ) { return true; }
return Math.Sign( distance.X ) != Math.Sign( car.Direction.X ) && Math.Sign( distance.Y ) != Math.Sign( car.Direction.Y );
}
public bool ShouldChange( Car car )
{
var next = car.Conductors.GetNext().RouteElement;
var distance = next.RoadElement.BuildControl.Location - car.Location;
if ( distance.Length() <= 0.001f ) { return true; }
return Math.Sign( distance.X ) != Math.Sign( car.Direction.X ) && Math.Sign( distance.Y ) != Math.Sign( car.Direction.Y );
}
public CarStateMachine( Car car )
{
Contract.Requires( car != null );
this._engine = new Engine();
this._car = car;
this._car.Conductors.MoveNext();
this._driverBrain = new DriverBrain( this._car, this._engine );
}
public void SetStopPoint( float distance, float requriredSpeed, Car car )
{
requriredSpeed = Math.Min( requriredSpeed, car.MaxSpeed );
var speedDelta = Math.Max( 0, car.Velocity - requriredSpeed );
var breakingForce = speedDelta / ( distance + 10 );
if ( this._brekingForce > breakingForce ) { return; }
this._brekingForce = breakingForce;
this._stopPointDistance = distance;
this._requiredSpeed = requriredSpeed;
}
public void MoveCar( Car car, int elapsedMs )
{
var distance = Math.Min( UnitConverter.FromMeter( 0.1f ), this.GetVelocity( car, elapsedMs ) );
var nextPoint = car.Direction * ( distance );
car.Location += nextPoint;
this.DestinationDistance = this._stopPointDistance;
this.DestinationSpeed = this._requiredSpeed;
this.Clear();
}
public RoadInformation Process( Car car, IRouteMark<IConductor> route )
{
var carAheadInformation = this.Information.GetCarAheadDistance( car );
return new RoadInformation
{
CarAhead = carAheadInformation.CarAhead,
CarAheadDistance = carAheadInformation.CarDistance,
PrivilagesCarInformation = null,
CanStop = this._canStopOnIt,
CanDriver = true,
};
}
private float Break( Car car, int elapsedMs )
{
var breakingDistance = GetBreakingDistance( car );
if ( breakingDistance < this._stopPointDistance - UnitConverter.FromMeter( 1.0f ) )
{
this.Accelerate( car, elapsedMs );
}
var breakingForce = Math.Pow( car.Velocity - this._requiredSpeed, 2 ) / ( 2 * this._stopPointDistance );
car.Velocity -= ( float ) breakingForce * elapsedMs;
return Math.Min( car.Velocity * elapsedMs, this._stopPointDistance );
}
private bool CheckLights( Car car )
{
if ( this._junctionEdge.Light != null )
{
// TODO Use switch
if ( this._junctionEdge.Light.LightState == LightState.Green ) { return true; }
if ( this._junctionEdge.Light.LightState == LightState.Red ) { return false; }
if ( this._junctionEdge.Light.LightState == LightState.YiellowFromRed ) { return false; }
if ( this._junctionEdge.Light.LightState == LightState.YiellowFromGreen )
{
var carDistance = Vector2.Distance( car.Location, this._junctionEdge.EdgeBuilder.Location );
return carDistance < UnitConverter.FromMeter( 5 ) && car.Velocity > UnitConverter.FromKmPerHour( 30 );
}
throw new ArgumentException( "Not supported ligth state" );
}
return true;
}
private float GetVelocity( Car car, int elapsedMs )
{
if ( this._stopPointDistance <= 0 )
{
return this._requiredSpeed;
}
if ( this._requiredSpeed > car.Velocity )
{
return this.Accelerate( car, elapsedMs );
}
if ( this._stopPointDistance < UnitConverter.FromMeter( 0.1f ) )
{
car.Velocity = this._requiredSpeed;
return this._stopPointDistance;
}
return this.Break( car, elapsedMs );
}
public Information Get( Car car )
{
var roadElements = car.RoadElements.Clone();
var carAhead = car.RoadElements.Current.RoadInformation.GetCarAheadDistance();
if ( carAhead != null )
{ return new Information( Vector2.Distance( car.Location, carAhead.Location ), carAhead.Velocity ); }
var length = car.RoadElements.Current.RoadInformation.GetCarDistanceToEnd( car );
while ( roadElements.MoveNext() )
{
carAhead = roadElements.Current.RoadInformation.GetCarAheadDistance();
if ( carAhead != null )
{
length += roadElements.Current.RoadInformation.GetCarDistanceToBegin( carAhead );
break;
}
length += roadElements.Current.RoadInformation.Lenght( roadElements.GetPrevious(), roadElements.GetNext() );
}
return new Information( length, carAhead != null ? carAhead.Velocity : float.MaxValue );
}
public void OnEnter( Car car )
{
this._cars.Add( car );
}
public virtual void OnEnter( Car car )
{
Contract.Requires( car != null );
this.Cars.Add( car );
}
public PriorityInformation( Car carWihtPriority, float carDistanceToJunction, float distanceToJunction )
{
this.CarWihtPriority = carWihtPriority;
this.CarDistanceToJunction = carDistanceToJunction;
this.DistanceToJunction = distanceToJunction;
}
public FirstCarToOutInformation( Car car, float carDistance )
{
this.Car = car;
this.CarDistance = carDistance;
}
public void Remove( Car car )
{
this._cars.Remove( car );
}
public void Take( Car car )
{
this._cars.Add( car );
// if( this._cars.Count > 1 ) { throw new InvalidOperationException();}
}
public float GetCarDistanceToEnd( Car car )
{
Debug.Assert( this.Information.ContainsCar( car ) );
return Constans.PointSize;
}
public Vector2 GetCarDirection( Car car )
{
return this._next.BuildControl.Location - car.Location;
}
public Vector2 GetCarDirection( Car car )
{
return car.Direction;
}
public void OnExit( Car car )
{
this._cars.Remove( car );
}
public DriverBrain( Car car, IEngine engine )
{
this._car = car;
this._engine = engine;
}
private void ProcessCarAheadInformation( Car carAhead, float carAheadDistance, float distance )
{
this._canDriver = false;
this._freeWay = false;
var saveArea = UnitConverter.ToKmPerHour( this._car.Velocity ) * UnitConverter.FromMeter( 0.18f );
this.SetDestinationPositionAndSpeed( distance + carAheadDistance - saveArea - carAhead.Lenght, carAhead.Velocity );
if ( carAheadDistance + carAhead.StateMachine.DestinationDistance < carAhead.Lenght + UnitConverter.FromMeter( 0.5f ) && carAhead.StateMachine.DestinationSpeed < UnitConverter.FromKmPerHour( 2 ) )
{
this._canEntry = false;
}
}
private double GetBreakingDistance( Car car )
{
var speedDifferenc = car.Velocity - this._requiredSpeed;
var breakingDistance = Math.Pow( speedDifferenc, 2 ) / ( 2 * car.BreakingForce );
return breakingDistance;
}
private float Accelerate( Car car, int elapsedMs )
{
var accelerated = car.AccelerateForce * elapsedMs;
car.Velocity = Math.Min( car.MaxSpeed, car.Velocity + accelerated );
return Math.Min( car.Velocity * elapsedMs, this._stopPointDistance );
}
public void Add( Car car )
{
this._cars.Add( car );
}
public bool ShouldChange( Car car )
{
return true;
}
public bool ContainsCar( Car car )
{
return this._cars.Contains( car );
}
public Car GetCarAheadOf( Car car )
{
// TODO Revers create new collection
return Enumerable.Reverse( this._cars ).SkipWhile( c => c != car ).Skip( 1 ).FirstOrDefault();
}
