/// <summary>
/// Updates the time-step.
/// </summary>
/// <exception cref="System.ArgumentNullException">
/// Collision Analyzer failed!
/// or
/// Collision Analyzer failed!
/// </exception>
/// <exception cref="System.ArgumentException">
/// Collision not found
/// or
/// </exception>
public void TimeStepUpdate()
{
try
{
#region update destination
bool decisionPeriodUpdate = this.WalkTime > this.DecisionMakingPeriod;
bool desiredStateIsNull = this.Destination == null;
bool distanceToDestination = true;
if (this.Destination != null)
{
distanceToDestination = UV.GetLengthSquared(this.CurrentState.Location, this.Destination) < .01d;
}
if (decisionPeriodUpdate || desiredStateIsNull || distanceToDestination || this.EdgeCollisionState.DistanceToBarrier < this.BodySize)
{
this.updateDestination();
}
#endregion
// make a copy of current state
var newState = this.CurrentState.Copy();
var previousState = this.CurrentState.Copy();
// direction update
var direction = this.Destination - newState.Location;
direction.Unitize();
#region update barrier relation factors
UV repulsionForce = new UV(0.0, 0.0);
// this condition is for the first time only when walking begins
if (this.EdgeCollisionState == null)
{
this.EdgeCollisionState = CollisionAnalyzer.GetCollidingEdge(newState.Location, this._cellularFloor, BarrierType.Field);
}
if (this.EdgeCollisionState != null)
{
//only if the barrier is visible to the agent the repulstion force will be applied
if (this.EdgeCollisionState.NormalizedRepulsion.DotProduct(newState.Direction) >= 0.0d)//this.VisibilityCosineFactor)
{
}
else
{
//calculate repulsion force
repulsionForce = this.EdgeCollisionState.NormalizedRepulsion * this.LoadRepulsionMagnetude();
}
}
else
{
throw new ArgumentNullException("Collision Analyzer failed!");
}
#endregion
// find Acceleration force
UV acceleration = this.AccelerationMagnitude * direction + repulsionForce;
//update velocity
newState.Velocity += acceleration * this.TimeStep;
//check velocity magnetude against the cap
if (newState.Velocity.GetLengthSquared() > this.VelocityMagnitude * this.VelocityMagnitude)
{
newState.Velocity.Unitize();
newState.Velocity *= this.VelocityMagnitude;
}
//update location
var location = newState.Location + this.TimeStep * newState.Velocity;
newState.Location = location;
//update the direction of the agent in the transformation matrix
double deltaAngle = this.AngularVelocity * this.TimeStep;
Axis axis = new Axis(this.CurrentState.Direction, direction, deltaAngle);
newState.Direction = axis.V_Axis;
this.CurrentState = newState;
//checking for collisions
var newEdgeCollisionState = CollisionAnalyzer.GetCollidingEdge(newState.Location, this._cellularFloor, BarrierType.Field);
if (newEdgeCollisionState == null)
{
throw new ArgumentNullException("Collision Analyzer failed!");
}
else
{
if (newEdgeCollisionState.DistanceToBarrier <= this.BodySize / 2)
{
var collision = CollisionAnalyzer.GetCollision(this.EdgeCollisionState, newEdgeCollisionState, this.BodySize / 2, OSMDocument.AbsoluteTolerance);
if (collision == null)
{
throw new ArgumentException("Collision not found");
}
else
{
double newTimeStep = 0.0;
if (collision.TimeStepRemainderProportion <= 0)
{
newTimeStep = this.TimeStep;
this.TimeStep = 0.0;
}
else
{
if (collision.TimeStepRemainderProportion > 1.0)
{
newTimeStep = 0;
this.TimeStep = TimeStep;
}
else
{
newTimeStep = this.TimeStep * (1.0 - collision.TimeStepRemainderProportion);
this.TimeStep = this.TimeStep - newTimeStep;
}
}
// update velocity
var velocity = this.CurrentState.Velocity + newTimeStep * acceleration;
if (velocity.GetLengthSquared() > this.VelocityMagnitude * this.VelocityMagnitude)
{
velocity.Unitize();
velocity *= this.VelocityMagnitude;
}
//decompose velocity
UV normal = collision.GetCollisionNormal();
UV velocityVerticalComponent = normal * (normal.DotProduct(velocity));
double velocityVerticalComponentLength = velocityVerticalComponent.GetLength();
UV velocityHorizontalComponent = velocity - velocityVerticalComponent;
double velocityHorizontalComponentLength = velocityHorizontalComponent.GetLength();
//decompose acceleration
UV accelerationVerticalComponent = normal * (normal.DotProduct(acceleration));
// elasticity
UV ve = this.BodyElasticity * velocityVerticalComponent;
//friction
double f1 = this.BarrierFriction * velocityVerticalComponentLength;
double f2 = velocityHorizontalComponentLength;
UV vf = velocityHorizontalComponent - Math.Min(f1, f2) * velocityHorizontalComponent / velocityHorizontalComponentLength;
//update velocity
var adjustedVelocity = vf - ve - newTimeStep * accelerationVerticalComponent;
if (adjustedVelocity.GetLengthSquared() > this.VelocityMagnitude * this.VelocityMagnitude)
{
adjustedVelocity.Unitize();
adjustedVelocity *= this.VelocityMagnitude;
}
newState.Velocity = adjustedVelocity;
// update location
if (newTimeStep == 0)
{
newState.Location = CurrentState.Location;
}
else
{
newState.Location = collision.CollisionPoint;
}
newState.Location += OSMDocument.PenaltyCollisionReaction * normal;
//update direction
deltaAngle = this.AngularVelocity * newTimeStep;
axis = new Axis(this.CurrentState.Direction, direction, deltaAngle);
newState.Direction = axis.V_Axis;
//update state
newEdgeCollisionState.DistanceToBarrier = this.BodySize / 2;
newEdgeCollisionState.Location = newState.Location;
this.EdgeCollisionState = newEdgeCollisionState;
this.CurrentState = newState;
}
}
else
{
//update state for the full timestep length
this.EdgeCollisionState = newEdgeCollisionState;
this.CurrentState = newState;
this.TimeStep = 0.0d;
}
}
}
catch (Exception error)
{
throw new ArgumentException(error.Report());
}
}
/// <summary>
/// Updates the time-step.
/// </summary>
/// <exception cref="System.ArgumentException">
/// Collision not found
/// or
/// The proportion of timestep before collision is out of range: " + collision.TimeStepRemainderProportion.ToString()
/// </exception>
/// <exception cref="System.ArgumentNullException">
/// Collision Analyzer failed!
/// or
/// Collision Analyzer failed!
/// </exception>
public void TimeStepUpdate()
{
// make a copy of current state
var newState = this.CurrentState.Copy();
var previousState = this.CurrentState.Copy();
// direction update
UV direction = this.CurrentState.Direction;
try
{
var filed_direction = this.CurrentActivity.Differentiate(this.CurrentState.Location);
if (filed_direction != null)
{
direction = filed_direction;
}
}
catch (Exception error)
{
throw new ArgumentException(error.Report());
}
direction.Unitize();
#region update barrier relation factors
UV repulsionForce = new UV(0.0, 0.0);
// this condition is for the first time only when walking begins
if (this.EdgeCollisionState == null)
{
this.EdgeCollisionState = CollisionAnalyzer.GetCollidingEdge(newState.Location, this._cellularFloor, BarrierType.Field);
}
if (this.EdgeCollisionState != null)
{
//only if the barrier is visible to the agent the repulsion force will be applied
if (this.EdgeCollisionState.NormalizedRepulsion.DotProduct(newState.Direction) >= 0.0d)//this.VisibilityCosineFactor)
{
////render agent color
//this.Fill = VisualAgentMandatoryScenario.OutsideRange;
//this.Stroke = VisualAgentMandatoryScenario.OutsideRange;
}
else
{
double repulsionMagnitude = this.LoadRepulsionMagnitude();
if (repulsionMagnitude != 0.0d)
{
////render agent color
//this.Fill = VisualAgentMandatoryScenario.InsideRange;
//this.Stroke = VisualAgentMandatoryScenario.InsideRange;
//calculate repulsion force
repulsionForce = this.EdgeCollisionState.NormalizedRepulsion * repulsionMagnitude;
}
else
{
////render agent color
//this.Fill = VisualAgentMandatoryScenario.OutsideRange;
//this.Stroke = VisualAgentMandatoryScenario.OutsideRange;
}
}
}
else
{
throw new ArgumentNullException("Collision Analyzer failed!");
}
#endregion
// find Acceleration force and update velocity
UV acceleration = new UV(0, 0);
acceleration += this.AccelerationMagnitude * direction + repulsionForce;
newState.Velocity += acceleration * this.TimeStep;
//check velocity magnitude against the cap
if (newState.Velocity.GetLengthSquared() > this.VelocityMagnitude * this.VelocityMagnitude)
{
newState.Velocity.Unitize();
newState.Velocity *= this.VelocityMagnitude;
}
//update location
var location = newState.Location + this.TimeStep * newState.Velocity;
newState.Location = location;
//update the location of the agent in the transformation matrix
//this._matrix.OffsetX = newState.Location.U;
//this._matrix.OffsetY = newState.Location.V;
// update direction
double deltaAngle = this.AngularVelocity * this.TimeStep;
Axis axis = new Axis(this.CurrentState.Direction, direction, deltaAngle);
newState.Direction = axis.V_Axis;
//update state
newState.Direction.Unitize();
//checking for collisions
var newEdgeCollisionState = CollisionAnalyzer.GetCollidingEdge(newState.Location, this._cellularFloor, BarrierType.Field);
if (newEdgeCollisionState == null)
{
this.EdgeCollisionState = CollisionAnalyzer.GetCollidingEdge(newState.Location, this._cellularFloor, BarrierType.Field);
throw new ArgumentNullException("Collision Analyzer failed!");
}
else
{
if (this.EdgeCollisionState.DistanceToBarrier > this.BodySize / 2 && newEdgeCollisionState.DistanceToBarrier < this.BodySize / 2)
{
var collision = CollisionAnalyzer.GetCollision(this.EdgeCollisionState, newEdgeCollisionState, this.BodySize / 2, OSMDocument.AbsoluteTolerance);
if (collision == null)
{
throw new ArgumentException("Collision not found");
}
else
{
if (collision.TimeStepRemainderProportion > 1.0 || collision.TimeStepRemainderProportion < 0.0d)
{
throw new ArgumentException("The proportion of timestep before collision is out of range: " + collision.TimeStepRemainderProportion.ToString());
}
else
{
//update for partial timestep
//update timestep
double newTimeStep = this.TimeStep * collision.TimeStepRemainderProportion;
this.TimeStep -= newTimeStep;
// update location
newState.Location = collision.CollisionPoint;
//this._matrix.OffsetX = newState.Location.U;
//this._matrix.OffsetY = newState.Location.V;
// update velocity
var velocity = this.CurrentState.Velocity + newTimeStep * acceleration;
if (velocity.GetLengthSquared() > this.VelocityMagnitude * this.VelocityMagnitude)
{
velocity.Unitize();
velocity *= this.VelocityMagnitude;
}
//decompose velocity
UV velocityVerticalComponent = newEdgeCollisionState.NormalizedRepulsion * (newEdgeCollisionState.NormalizedRepulsion.DotProduct(velocity));
double velocityVerticalComponentLength = velocityVerticalComponent.GetLength();
UV velocityHorizontalComponent = velocity - velocityVerticalComponent;
double velocityHorizontalComponentLength = velocityHorizontalComponent.GetLength();
//decompose acceleration
UV accelerationVerticalComponent = newEdgeCollisionState.NormalizedRepulsion * (newEdgeCollisionState.NormalizedRepulsion.DotProduct(acceleration));
// elasticity
UV ve = this.BodyElasticity * velocityVerticalComponent;
//friction
double f1 = this.BarrierFriction * velocityVerticalComponentLength;
double f2 = velocityHorizontalComponentLength;
UV vf = velocityHorizontalComponent - Math.Min(f1, f2) * velocityHorizontalComponent / velocityHorizontalComponentLength;
//update velocity
var adjustedVelocity = vf - ve - newTimeStep * accelerationVerticalComponent;
if (adjustedVelocity.GetLengthSquared() > this.VelocityMagnitude * this.VelocityMagnitude)
{
adjustedVelocity.Unitize();
adjustedVelocity *= this.VelocityMagnitude;
}
newState.Velocity = adjustedVelocity;
//update direction
deltaAngle = this.AngularVelocity * newTimeStep;
axis = new Axis(this.CurrentState.Direction, direction, deltaAngle);
newState.Direction = axis.V_Axis;
//update state
newEdgeCollisionState.DistanceToBarrier = this.BodySize / 2;
newEdgeCollisionState.Location = newState.Location;
this.EdgeCollisionState = newEdgeCollisionState;
this.CurrentState = newState;
}
}
}
else
{
//update state for the full timestep length
this.EdgeCollisionState = newEdgeCollisionState;
this.CurrentState = newState;
this.TimeStep = 0.0d;
}
}
}
