public void Test_New()
{
var ca = new CollisionAnalyzer ();
Assert.IsNotNull (ca.CollisionWorld);
Assert.AreEqual (0, ca.CollisionObjectCount);
Assert.AreEqual (0, ca.CollisionObjects.Count());
Assert.AreEqual (0, ca.OverlapppingPairCount);
Assert.AreEqual (0, ca.OverlappingPairs.Count ());
}
public void Test_AddCollisionObject()
{
var ca = new CollisionAnalyzer ();
var node = new Node ();
var col = new CollisionObject ();
col.Shape = new SphereShape (1);
node.Attach (col);
ca.AddGhostObject (node);
Assert.AreEqual (1, ca.CollisionObjectCount);
Assert.AreEqual (1, ca.CollisionObjects.Count ());
Assert.AreEqual (true, ca.IsRegistered (node));
}
static void Main(string[] args)
{
var AirspaceConfiguration = XMLAirspaceConfiguration.LoadAirspace(@"../../../AirspaceConfiguration.xml");
Airspace airspace = new Airspace();
airspace.HighestAltitude = AirspaceConfiguration.HighestAltitude;
airspace.LowestAltitude = AirspaceConfiguration.LowestAltitude;
airspace.NorthEastXCoordinate = AirspaceConfiguration.NorthEastXCoordinate;
airspace.NorthEastYCoordinate = AirspaceConfiguration.NorthEastYCoordinate;
airspace.SouthWestXCoordinate = AirspaceConfiguration.SouthWestXCoordinate;
airspace.SouthWestYCoordinate = AirspaceConfiguration.SouthWestYCoordinate;
ITransponderReceiver transponderReceiver = TransponderReceiverFactory.CreateTransponderDataReceiver();
ITrackFactory trackFactory = new TrackFactory();
ITransponderDataReciever transponderDataReciever = new TransponderDataReceiver(transponderReceiver, trackFactory);
IAirspaceTrackChecker airspaceTrackChecker = new AirspaceTrackChecker(airspace);
IAirspaceController airspaceController = new AirspaceController.AirspaceController(transponderDataReciever, airspaceTrackChecker);
ITrackRemover trackRemover = new TrackRemover();
ITrackManagement trackManagement = new TrackManagement();
IFlightManagement flightManagement = new FlightManagement.FlightManagement(airspaceController, trackRemover, trackManagement);
ICompassCalculator compassCalculator = new CompassCalculator();
ICourseAnalyzer courseAnalyzer = new CourseAnalyzer(compassCalculator);
IDistanceCalculator distanceCalculator = new DistanceCalculator();
ITimeSpanCalculator timeSpanCalculator = new TimeSpanCalculator();
IVelocityCalculator velocityCalculator = new VelocityCalculator(timeSpanCalculator, distanceCalculator);
IVelocityAnalyzer velocityAnalyzer = new VelocityAnalyzer(velocityCalculator);
IFlightAnalyzer flightAnalyzer = new FlightAnalyzer.FlightAnalyzer(flightManagement, courseAnalyzer, velocityAnalyzer);
IAltitudeDistanceCalculator altitudeDistanceCalculator = new AltitudeDistanceCalculator();
ICollisionAnalyzer collisionAnalyzer = new CollisionAnalyzer(distanceCalculator, altitudeDistanceCalculator);
ISeparationStringBuilder separationStringBuilder = new SeparationStringBuilder();
ICollisionController collisionController = new CollisionController.CollisionController(flightManagement, collisionAnalyzer, separationStringBuilder);
IDisplay display = new ConsoleDisplay(flightAnalyzer, collisionController);
IFileWriter fileWriter = new FileWriter();
var currentDirectory = Directory.GetCurrentDirectory();
var path = Path.Combine(currentDirectory, "SeparationLog.txt");
ISeparationEventLogger logger = new CollisionController.Logger(collisionController, path, fileWriter);
Console.ReadLine();
}
public void Test_RemoveCollisionObject()
{
var ca = new CollisionAnalyzer ();
var node1 = new Node ();
var col1 = new CollisionObject ();
col1.Shape = new SphereShape (1);
node1.Attach (col1);
var node2 = new Node ();
var col2 = new CollisionObject ();
col2.Shape = new SphereShape (1);
node2.Attach (col2);
ca.AddGhostObject (node1);
ca.AddGhostObject (node2);
ca.RemoveGhostObject (node1);
Assert.AreEqual (1, ca.CollisionObjectCount);
Assert.AreEqual (1, ca.CollisionObjects.Count ());
Assert.AreEqual (true, ca.IsRegistered (node2));
}
/// <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());
}
}
public void SetUp()
{
_fakeDistanceCalculator = Substitute.For <IDistanceCalculator>();
_fakeAltitudeDistanceCalculator = Substitute.For <IAltitudeDistanceCalculator>();
_uut = new CollisionAnalyzer(_fakeDistanceCalculator, _fakeAltitudeDistanceCalculator);
}
/// <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;
}
}
}
