/// <summary>
/// Creates an instance of the collision
/// </summary>
/// <param name="displacementTrajectory">Displacement Trajectory</param>
/// <param name="endPoint">point at the end of an edge</param>
/// <param name="buffer">Buffer</param>
public GetCollisionWithEndPoint(Trajectory displacementTrajectory, UV endPoint, double buffer)
{
this.DisplacementTrajectory = displacementTrajectory;
this.Buffer = buffer;
//this.TrajectoryLine = trajectoryLine;
this.EndPoint = endPoint;
//create a vector that connects the start point of the line to the center of the circle
UV sO = this.EndPoint - this.DisplacementTrajectory.TrajectoryLine.Start;
double b = -2 * sO.DotProduct(this.DisplacementTrajectory.TrajectoryNormalizedVector);
double c = sO.GetLengthSquared() - this.Buffer * this.Buffer;
var results = CollisionAnalyzer.QuadraticSolver(1, b, c);
if (results == null)
{
this.LengthToCollision = double.PositiveInfinity;
}
else if (results[0] > this.DisplacementTrajectory.TrajectoryLength)
{
this.LengthToCollision = double.PositiveInfinity;
}
else
{
this.CollisionPoint = this.DisplacementTrajectory.TrajectoryLine.Start + this.DisplacementTrajectory.TrajectoryNormalizedVector * results[1];
this.TimeStepRemainderProportion = results[1] / this.DisplacementTrajectory.TrajectoryLength;
this.LengthToCollision = this.DisplacementTrajectory.TrajectoryLength - results[1];
}
}
/// <summary>
/// Return a repulsion line from the closest barrier
/// </summary>
/// <param name="point">A point in the field</param>
/// <param name="barrierType">The type of barrier</param>
/// <returns>A line</returns>
public UVLine GetRepulsionLine(UV point, BarrierType barrierType)
{
var collision = CollisionAnalyzer.GetCollidingEdge(point, this, barrierType);
if (collision == null)
{
return(null);
}
if (collision.ClosestPointOnBarrier == null)
{
return(null);
}
return(new UVLine(point, collision.ClosestPointOnBarrier));
}
/// <summary>
/// Detects the collision that occurs between two states
/// </summary>
/// <param name="previous">Previous state</param>
/// <param name="current">Current state</param>
/// <param name="buffer">Offset buffer which is the radius of the body</param>
/// <param name="tolerance">Tolerance set by default to the main document's absolute tolerance factor</param>
/// <returns>An instance of the collision which can be null</returns>
public static ICollision GetCollision(CollisionAnalyzer previous, CollisionAnalyzer current, double buffer, double tolerance = OSMDocument.AbsoluteTolerance)
{
Trajectory trajectory = new Trajectory(current.Location, previous.Location);
List <ICollision> collisions = new List <ICollision>();
UVLine edgeBuffer1 = CollisionAnalyzer.OffsetLine(previous.Barrrier, previous.Location, buffer);
ICollision collisionWithEdgeBuffer1 = new GetCollisionWithEdge(trajectory, edgeBuffer1);
collisions.Add(collisionWithEdgeBuffer1);
ICollision collisionWithPreviousBarrierEndPoint = new GetCollisionWithEndPoint(trajectory, previous.Barrrier.End, buffer);
collisions.Add(collisionWithPreviousBarrierEndPoint);
ICollision collisionWithPreviousBarrierStartPoint = new GetCollisionWithEndPoint(trajectory, previous.Barrrier.Start, buffer);
collisions.Add(collisionWithPreviousBarrierStartPoint);
if (!previous.Barrrier.Equals(current.Barrrier))
{
UVLine edgeBuffer2 = CollisionAnalyzer.OffsetLine(current.Barrrier, previous.Location, buffer);
ICollision collisionWithEdgeBuffer2 = new GetCollisionWithEdge(trajectory, edgeBuffer2);
collisions.Add(collisionWithEdgeBuffer2);
if (current.Barrrier.End != previous.Barrrier.End && current.Barrrier.End != previous.Barrrier.Start)
{
ICollision collisionWithCurrentBarrierEndPoint = new GetCollisionWithEndPoint(trajectory, current.Barrrier.End, buffer);
collisions.Add(collisionWithCurrentBarrierEndPoint);
}
if (current.Barrrier.Start != previous.Barrrier.End && current.Barrrier.Start != previous.Barrrier.Start)
{
ICollision collisionWithCurrentBarrierStartPoint = new GetCollisionWithEndPoint(trajectory, current.Barrrier.Start, buffer);
collisions.Add(collisionWithCurrentBarrierStartPoint);
}
}
if (collisions.Count != 0)
{
collisions.Sort((a, b) => a.LengthToCollision.CompareTo(b.LengthToCollision));
ICollision firstCollision = collisions[0];
if (!double.IsInfinity(firstCollision.LengthToCollision))
{
//if (double.IsNaN(firstCollision.TimeStepRemainderProportion))
//{
// System.Windows.MessageBox.Show(double.NaN.ToString() + " caught!");
// GetCollision(previous, current, buffer, tolerance);
//}
return(firstCollision);
}
}
return(null);
}
/// <summary>
/// Creats an instance of the CollisionAnalyzer.
/// </summary>
/// <param name="point">The agent or target location</param>
/// <param name="cellularFloor">The CellularFloorBaseGeometry which includes the agent or target</param>
/// <param name="barrierType">Barrier type</param>
/// <returns>An instance of the CollisionAnalyzer which can be null</returns>
public static CollisionAnalyzer GetCollidingEdge(UV point, CellularFloorBaseGeometry cellularFloor, BarrierType barrierType)
{
Cell cell = cellularFloor.FindCell(point);
if (cell == null)
{
return(null);
}
switch (barrierType)
{
case BarrierType.Visual:
if (cell.VisualOverlapState == OverlapState.Inside)
{
return(null);
}
break;
case BarrierType.Physical:
if (cell.PhysicalOverlapState == OverlapState.Inside)
{
return(null);
}
break;
case BarrierType.Field:
if (cell.FieldOverlapState == OverlapState.Outside)
{
//cell.Visualize(MainDocument.TargetVisualizer, Cell.Size, Cell.Size, 0);
return(null);
}
break;
case BarrierType.BarrierBuffer:
return(null);
}
var colision = new CollisionAnalyzer(point, cell, cellularFloor, barrierType);
if (colision.Barrrier == null)
{
return(null);
}
return(colision);
}
/// <summary>
/// return an instance of the CollisionAnalyzer to determine the collision state of a given point in relation to a barrier type
/// </summary>
/// <param name="point">A point in the field</param>
/// <param name="barrierType">The type of barrier</param>
/// <returns>An instance of CollisionAnalyzer</returns>
public CollisionAnalyzer GetCollidingEdge(UV point, BarrierType barrierType)
{
return(CollisionAnalyzer.GetCollidingEdge(point, this, barrierType));
}