/// <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)); }