C# Coordinates.DistanceTo方法代码示例

        private void GetObstacleHitAngle(Coordinates startPt, Coordinates center, IEnumerable<Polygon> obstacles, ref double minAngle)
        {
            // build the circle
            Circle circle = new Circle(startPt.DistanceTo(center), center);
            Coordinates[] hits = null;
            foreach (Polygon poly in obstacles) {
                if (poly.Intersect(circle, out hits)) {
                    double startAngle = (startPt - center).ArcTan;

                    foreach (Coordinates hit in hits) {
                        double angle = (hit - center).ArcTan;

                        if (angle <= startAngle)
                            angle += 2*Math.PI;

                        angle -= startAngle;

                        if (angle < minAngle)
                            minAngle = angle;

                    }
                }
            }
        }

        public HitTestResult HitTest(Coordinates loc, float tol, WorldTransform wt, DisplayObjectFilter filter)
        {
            // check filter
            if (filter.Target == null || filter.Target is ArbiterUserWaypoint)
            {
                // get bounding box dependent on tolerance
                RectangleF bounding = this.GetBoundingBox(wt);
                bounding.Inflate(tol, tol);

                // check if contains point
                if (bounding.Contains(DrawingUtility.ToPointF(loc)))
                {
                    return new HitTestResult(this, true, (float)loc.DistanceTo(this.Position));
                }
            }

            return new HitTestResult(this, false, float.MaxValue);
        }

 public double DistanceOffPath(Coordinates pt)
 {
     return Math.Abs(pt.DistanceTo(center) - radius);
 }

        /// <summary>
        /// Gets exits downstream, or the goal we are currently reaching
        /// </summary>
        /// <param name="currentPosition"></param>
        /// <param name="ignorable"></param>
        /// <param name="goal"></param>
        /// <returns></returns>
        public List<DownstreamPointOfInterest> Downstream(Coordinates currentPosition, List<ArbiterWaypoint> ignorable, INavigableNode goal)
        {
            // downstream final
            List<DownstreamPointOfInterest> waypoints = new List<DownstreamPointOfInterest>();

            foreach (ArbiterLane al in Way.Lanes.Values)
            {
                if (al.Equals(this) || (al.GetClosestPartition(currentPosition).Type != PartitionType.Startup &&
                    ((this.LaneOnLeft != null && this.LaneOnLeft.Equals(al) && this.BoundaryLeft != ArbiterLaneBoundary.SolidWhite) ||
                    (this.LaneOnRight != null && this.LaneOnRight.Equals(al) && this.BoundaryRight != ArbiterLaneBoundary.SolidWhite))))
                {
                    // get starting waypoint
                    ArbiterWaypoint waypoint = null;

                    // get closest partition
                    ArbiterLanePartition alp = al.GetClosestPartition(currentPosition);
                    if (alp.Initial.Position.DistanceTo(currentPosition) < TahoeParams.VL - 2)
                        waypoint = alp.Initial;
                    else if (alp.IsInside(currentPosition) || alp.Final.Position.DistanceTo(currentPosition) < TahoeParams.VL)
                        waypoint = alp.Final;
                    else if (alp.Initial.Position.DistanceTo(currentPosition) < alp.Final.Position.DistanceTo(currentPosition))
                        waypoint = alp.Initial;
                    else if (alp.Initial.Position.DistanceTo(currentPosition) < alp.Final.Position.DistanceTo(currentPosition) && alp.Final.NextPartition == null)
                        waypoint = null;
                    else
                        waypoint = null;

                    // check waypoint exists
                    if (waypoint != null)
                    {
                        // save start
                        ArbiterWaypoint initial = waypoint;

                        // initial cost
                        double initialCost = 0.0;

                        if (al.Equals(this))
                            initialCost = currentPosition.DistanceTo(waypoint.Position) / waypoint.Lane.Way.Segment.SpeedLimits.MaximumSpeed;
                        else if (waypoint.WaypointId.Number != 1)
                        {
                            // get closest partition
                            ArbiterWaypoint tmpI = this.GetClosestWaypoint(currentPosition, Double.MaxValue);
                            initialCost = NavigationPenalties.ChangeLanes * Math.Abs(this.LaneId.Number - al.LaneId.Number);
                            initialCost += currentPosition.DistanceTo(tmpI.Position) / tmpI.Lane.Way.Segment.SpeedLimits.MaximumSpeed;
                        }
                        else
                        {
                            // get closest partition
                            ArbiterWaypoint tmpI = this.GetClosestWaypoint(currentPosition, Double.MaxValue);
                            ArbiterWaypoint tmpF = this.GetClosestWaypoint(waypoint.Position, Double.MaxValue);
                            initialCost = NavigationPenalties.ChangeLanes * Math.Abs(this.LaneId.Number - al.LaneId.Number);
                            initialCost += currentPosition.DistanceTo(tmpI.Position) / tmpI.Lane.Way.Segment.SpeedLimits.MaximumSpeed;
                            initialCost += this.TimeCostInLane(tmpI, tmpF, new List<ArbiterWaypoint>());
                        }

                        // loop while waypoint not null
                        while (waypoint != null)
                        {
                            if (waypoint.IsCheckpoint && (goal is ArbiterWaypoint) && ((ArbiterWaypoint)goal).WaypointId.Equals(waypoint.WaypointId))
                            {
                                double timeCost = initialCost + this.TimeCostInLane(initial, waypoint, ignorable);
                                DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
                                dpoi.DistanceToPoint = al.DistanceBetween(currentPosition, waypoint.Position);
                                dpoi.IsExit = false;
                                dpoi.IsGoal = true;
                                dpoi.PointOfInterest = waypoint;
                                dpoi.TimeCostToPoint = timeCost;
                                waypoints.Add(dpoi);
                            }
                            else if (waypoint.IsExit && !ignorable.Contains(waypoint))
                            {
                                double timeCost = initialCost + this.TimeCostInLane(initial, waypoint, ignorable);
                                DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
                                dpoi.DistanceToPoint = al.DistanceBetween(currentPosition, waypoint.Position);
                                dpoi.IsExit = true;
                                dpoi.IsGoal = false;
                                dpoi.PointOfInterest = waypoint;
                                dpoi.TimeCostToPoint = timeCost;
                                waypoints.Add(dpoi);
                            }
                            else if (waypoint.NextPartition == null && !ignorable.Contains(waypoint))
                            {
                                DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
                                dpoi.DistanceToPoint = al.DistanceBetween(currentPosition, waypoint.Position);
                                dpoi.IsExit = false;
                                dpoi.IsGoal = false;
                                dpoi.PointOfInterest = waypoint;
                                dpoi.TimeCostToPoint = Double.MaxValue;
                                waypoints.Add(dpoi);
                            }

                            waypoint = waypoint.NextPartition != null ? waypoint.NextPartition.Final : null;
                        }
//.........这里部分代码省略.........

 /// <summary>
 /// Returns a new coordinate closer in distance to the center than point by epsilon
 /// </summary>
 /// <param name="center">of the polygon</param>
 /// <param name="point">on the boundary of the polygon</param>
 /// <param name="epsilon">how much the polygon is being shrunk by</param>
 /// <returns>The a new coordinate closer in distance to the center than point by epsilon</returns>
 public static Coordinates resizeCoordinate(Coordinates center, Coordinates point, double epsilon)
 {
     double newDist = center.DistanceTo(point) - epsilon;
     if (newDist < 0) return new Coordinates(0f, 0f);
     else return new Coordinates(center.X + Math.Cos(((Coordinates)(point - center)).ToDegrees()) * newDist,
                                 center.Y + Math.Sin(((Coordinates)(point - center)).ToDegrees()) * newDist);
 }

//.........这里部分代码省略.........
                INavigableNode nn;
                this.NavigationHitTest(c, out nn, out ne);

                if (nn != null)
                {
                    // create new node
                    INavigableNode aznn = nn;

                    if (this.PreviousNode != null)
                    {
                        // create new edges
                        NavigableEdge newE1 = new NavigableEdge(true, this.zt.current, false, null, new List<IConnectAreaWaypoints>(), this.PreviousNode, aznn);
                        this.PreviousNode.OutgoingConnections.Add(newE1);
                        this.zt.current.NavigableEdges.Add(newE1);
                    }

                    this.PreviousNode = aznn;
                }
                else if (ne != null)
                {
                    // remove old
                    this.zt.current.NavigableEdges.Remove(ne);

                    // remove all references
                    ne.Start.OutgoingConnections.Remove(ne);

                    // create new node
                    ArbiterZoneNavigableNode aznn = new ArbiterZoneNavigableNode(c);

                    // create new edges
                    NavigableEdge newE1 = new NavigableEdge(true, this.zt.current, false, null, new List<IConnectAreaWaypoints>(), ne.Start, aznn);
                    NavigableEdge newE2 = new NavigableEdge(true, this.zt.current, false, null, new List<IConnectAreaWaypoints>(), aznn, ne.End);

                    // add edges
                    ne.Start.OutgoingConnections.Add(newE1);
                    aznn.OutgoingConnections.Add(newE2);

                    // add all to lists
                    this.zt.current.NavigableEdges.Add(newE1);
                    this.zt.current.NavigableEdges.Add(newE2);
                    this.zt.current.NavigationNodes.Add(aznn);

                    if (this.PreviousNode != null)
                    {
                        NavigableEdge newE3 = new NavigableEdge(true, this.zt.current, false, null, new List<IConnectAreaWaypoints>(), this.PreviousNode, aznn);
                        this.PreviousNode.OutgoingConnections.Add(newE3);
                        this.zt.current.NavigableEdges.Add(newE3);
                    }

                    this.PreviousNode = aznn;
                }
                else
                {
                    // create new node
                    ArbiterZoneNavigableNode aznn = new ArbiterZoneNavigableNode(c);

                    if (this.PreviousNode != null)
                    {
                        // create new edges
                        NavigableEdge newE1 = new NavigableEdge(true, this.zt.current, false, null, new List<IConnectAreaWaypoints>(), this.PreviousNode, aznn);
                        this.PreviousNode.OutgoingConnections.Add(newE1);
                        this.zt.current.NavigableEdges.Add(newE1);
                    }

                    this.PreviousNode = aznn;
                    this.zt.current.NavigationNodes.Add(aznn);
                }
            }
            else if (this.zt.Mode == ZoneToolboxMode.StayOut && this.zt.current != null)
            {
                if (this.WrappingHelpers.Count == 0)
                {
                    this.WrappingHelpers.Add(c);
                }
                else
                {
                    // Determine size of bounding box
                    float scaled_offset = 1 / this.rd.WorldTransform.Scale;

                    // invert the scale
                    float scaled_size = DrawingUtility.cp_large_size;

                    // assume that the world transform is currently applied correctly to the graphics
                    RectangleF rect = new RectangleF((float)c.X - scaled_size / 2, (float)c.Y - scaled_size / 2, scaled_size, scaled_size);

                    if (rect.Contains(DrawingUtility.ToPointF(this.WrappingHelpers[0])) &&
                        c.DistanceTo(this.WrappingHelpers[0]) < 1)
                    {
                        ed.SaveUndoPoint();
                        Polygon p = new Polygon(this.WrappingHelpers);
                        this.zt.current.StayOutAreas.Add(p);
                        this.WrappingHelpers = new List<Coordinates>();
                    }
                    else
                        this.WrappingHelpers.Add(c);
                }

                this.rd.Invalidate();
            }
        }

        /// <summary>
        /// Gets all exits downstream from a point
        /// </summary>
        /// <param name="position"></param>
        /// <param name="way"></param>
        /// <param name="exits">We are looking for exits</param>
        /// <returns></returns>
        private List<DownstreamPointOfInterest> Downstream(Coordinates position, ArbiterWay way, bool exits, List<ArbiterWaypoint> ignorable)
        {
            List<DownstreamPointOfInterest> waypoints = new List<DownstreamPointOfInterest>();

            foreach (ArbiterLane al in way.Lanes.Values)
            {
                LinePath.PointOnPath pop = al.GetClosestPoint(position);

                if (al.IsInside(position) || position.DistanceTo(al.LanePath().StartPoint.Location) < 1.0)
                {
                    ArbiterLanePartition currentPartition = al.GetClosestPartition(position);
                    ArbiterWaypoint initial = currentPartition.Final;
                    double initialCost = position.DistanceTo(currentPartition.Final.Position) / way.Segment.SpeedLimits.MaximumSpeed;

                    do
                    {
                        if(((exits && currentPartition.Final.IsExit) || (!exits && currentPartition.Final.IsEntry)) && !ignorable.Contains(currentPartition.Final))
                        {
                            double timeCost = initialCost + this.TimeCostInLane(initial, currentPartition.Final);
                            DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
                            dpoi.DistanceToPoint = al.LanePath().DistanceBetween(pop, al.GetClosestPoint(currentPartition.Final.Position));
                            dpoi.IsExit = true;
                            dpoi.IsGoal = false;
                            dpoi.PointOfInterest = currentPartition.Final;
                            dpoi.TimeCostToPoint = timeCost;
                            waypoints.Add(dpoi);
                        }

                        currentPartition = currentPartition.Final.NextPartition;
                    }
                    while(currentPartition != null);
                }
            }

            return waypoints;
        }

        /// <summary>
        /// Checks if a goal is downstream on the current road
        /// </summary>
        /// <param name="way"></param>
        /// <param name="currentPosition"></param>
        /// <param name="goal"></param>
        /// <returns></returns>
        private DownstreamPointOfInterest IsGoalDownStream(ArbiterWay way, Coordinates currentPosition, INavigableNode goal)
        {
            if (goal is ArbiterWaypoint)
            {
                ArbiterWaypoint goalWaypoint = (ArbiterWaypoint)goal;

                if (goalWaypoint.Lane.Way.Equals(way))
                {
                    foreach (ArbiterLane lane in way.Lanes.Values)
                    {
                        if (lane.Equals(goalWaypoint.Lane))
                        {
                            Coordinates current = lane.GetClosest(currentPosition);
                            Coordinates goalPoint = lane.GetClosest(goal.Position);

                            if (lane.IsInside(current) || currentPosition.DistanceTo(lane.LanePath().StartPoint.Location) < 1.0)
                            {
                                double distToGoal = lane.DistanceBetween(current, goalPoint);

                                if (distToGoal > 0)
                                {
                                    DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
                                    dpoi.DistanceToPoint = distToGoal;
                                    dpoi.IsGoal = true;
                                    dpoi.IsExit = false;
                                    dpoi.PointOfInterest = goalWaypoint;
                                    dpoi.TimeCostToPoint = this.TimeCostInLane(lane.GetClosestPartition(currentPosition).Initial, goalWaypoint);
                                    dpoi.RouteTime = 0.0;
                                    return dpoi;
                                }
                            }
                        }
                        else
                        {
                            Coordinates current = lane.GetClosest(currentPosition);
                            Coordinates goalPoint = lane.GetClosest(goal.Position);

                            if ((lane.IsInside(current) || currentPosition.DistanceTo(lane.LanePath().StartPoint.Location) < 1.0) && lane.IsInside(goalPoint))
                            {
                                double distToGoal = lane.DistanceBetween(current, goalPoint);

                                if (distToGoal > 0)
                                {
                                    DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
                                    dpoi.DistanceToPoint = distToGoal;
                                    dpoi.IsGoal = true;
                                    dpoi.IsExit = false;
                                    dpoi.PointOfInterest = goalWaypoint;
                                    dpoi.TimeCostToPoint = this.TimeCostInLane(lane.GetClosestPartition(currentPosition).Initial, goalWaypoint);
                                    dpoi.RouteTime = 0.0;
                                    //return dpoi;
                                }
                            }
                        }
                    }
                }
            }

            return null;
        }

 /// <summary>
 /// Get distance from partition to a location
 /// </summary>
 /// <param name="loc"></param>
 /// <returns></returns>
 public double DistanceTo(Coordinates loc)
 {
     return(loc.DistanceTo(this.PartitionPath.GetPoint(this.PartitionPath.GetClosestPoint(loc))));
 }