C# Vector2.DistanceTo方法代码示例

        public override void OnMouseOver( Vector2 mouseWorldPos )
        {
            _pointUnderMouse = -1 ;

            IMainForm mainForm = summonMainForm( ) ;

            for( int i = 0; i < _properties.WorldPoints.Count; i++ )
            {
                if( mouseWorldPos.DistanceTo( _properties.WorldPoints[ i ] ) <= 5 )
                {
                    _pointUnderMouse = i ;
                    mainForm.SetCursorForCanvas( Cursors.Hand ) ;
                    mainForm.SetToolStripStatusLabel1(
                        "{0} (Point {1}: {2})".FormatWith(_properties.Name, i.ToString( ), _properties.WorldPoints[ i ].ToString( )) ) ;
                }
            }

            if( _pointUnderMouse == -1 )
            {
                mainForm.SetCursorForCanvas( Cursors.Default ) ;
            }

            base.OnMouseOver( mouseWorldPos ) ;
        }

 public override void onMouseOver(Vector2 mouseworldpos)
 {
     pointundermouse = -1;
     for (int i = 0; i < WorldPoints.Length; i++)
     {
         if (mouseworldpos.DistanceTo(WorldPoints[i]) <= 5)
         {
             pointundermouse = i;
             MainForm.Instance.pictureBox1.Cursor = Cursors.Hand;
             MainForm.Instance.toolStripStatusLabel1.Text = Name + " (Point " + i.ToString() + ": " + WorldPoints[i].ToString() + ")";
         }
     }
     if (pointundermouse == -1) MainForm.Instance.pictureBox1.Cursor = Cursors.Default;
     base.onMouseOver(mouseworldpos);
 }

        /// <summary>
        /// Update rover position and remove nodes from waypoints and planned path if the rover approaches them
        /// </summary>
        /// <param name="pose"></param>
        public void UpdatePose(RobotPose pose)
        {
            lock (plannerLock)
            {
                List<SimpleTreeNode<Vector2>> nodesToRemove = new List<SimpleTreeNode<Vector2>>();
                currentLocation = pose.ToVector2();
                //Remove waypoints from the list as the rover nears them
                nodesToRemove.Clear();
                foreach (SimpleTreeNode<Vector2> node in userWaypoints)
                {
                    if (currentLocation.DistanceTo(node.Value) < wayPointRadius)
                    {
                        nodesToRemove.Add(node);
                    }
                    else break;
                }
                foreach (SimpleTreeNode<Vector2> node in nodesToRemove)
                {
                    userWaypoints.Remove(node);
                }

                //Remove nodes from the planned path as the rover approaches them
                lock (pathLock)
                {
                    nodesToRemove.Clear();
                    foreach (SimpleTreeNode<Vector2> node in outputNodeList)
                    {
                        if (currentLocation.DistanceTo(node.Value) < pathPointRadius)
                        {
                            nodesToRemove.Add(node);
                        }
                        else break;
                    }
                    foreach (SimpleTreeNode<Vector2> node in nodesToRemove)
                    {
                        outputNodeList.Remove(node);
                    }
                }
            }
        }

 private static Vector2 FindNeighborToCompare(List<ILidar2DPoint> filteredScan, SensorPose lidarPose, int initialIdx, double threshold)
 {
     double currentX = -filteredScan[initialIdx].RThetaPoint.R * Math.Sin(filteredScan[initialIdx].RThetaPoint.theta) - lidarPose.y;
     double currentY = filteredScan[initialIdx].RThetaPoint.R * Math.Cos(filteredScan[initialIdx].RThetaPoint.theta) + lidarPose.x;
     double neighborX = -filteredScan[filteredScan.Count - 1].RThetaPoint.R * Math.Sin(filteredScan[filteredScan.Count - 1].RThetaPoint.theta) - lidarPose.y;
     double neighborY = filteredScan[filteredScan.Count - 1].RThetaPoint.R * Math.Cos(filteredScan[filteredScan.Count - 1].RThetaPoint.theta) + lidarPose.x;
     Vector2 currentPt = new Vector2(currentX, currentY);
     for (int k = initialIdx; k < filteredScan.Count; k++)
     {
         neighborX = -filteredScan[k].RThetaPoint.R * Math.Sin(filteredScan[k].RThetaPoint.theta) - lidarPose.y;
         neighborY = filteredScan[k].RThetaPoint.R * Math.Cos(filteredScan[k].RThetaPoint.theta) + lidarPose.x;
         Vector2 neighborPt = new Vector2(neighborX, neighborY);
         if (currentPt.DistanceTo(neighborPt) > threshold)
             return neighborPt;
     }
     return new Vector2(neighborX, neighborY);
 }

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

 /// <summary>
 /// Gets a point on another subField that is closest to input location.
 /// Distance must be less than range to be considered. Snaps to corner by 1.5 
 /// times more than 
 /// </summary>
 /// <param name="location">Closest point from this location</param>
 /// <param name="range">Maximum distance looking for</param>
 /// <returns>CLosest point</returns>
 private Vector2 getNearest(Vector2 location, double range, List<Polygon> subFields)
 {
     Vector2 minVP = location;
     Vector2 minVL = location;
     double minDP = Double.MaxValue;
     double minDL = Double.MaxValue;
     double dis;
     foreach (Polygon p in subFields)
     {
         foreach (Vector2 v in p)
         {
             dis = location.DistanceTo(v);
             if (dis < range && dis < minDP)
             {
                 minDP = dis;
                 minVP = v;
             }
         }
         DEASL.Core.Mathematics.Shapes.LineSegment l =
             p.ShortestLineToOther(new Polygon(new Vector2[] { location }));
         dis = l.Length;
         if (dis < range && dis < minDL)
         {
             minDL = dis;
             minVL = l.P0;
         }
     }
     return (minDP < minDL * 1.5)? minVP : minVL;
 }

        public List<Waypoint> FindPath(Waypoint start, Waypoint goal, OccupancyGrid2D og, out bool success)
        {
            List<Waypoint> path = new List<Waypoint>();

            //added by aaron (sort of a hack)
            if (og == null || goal.Coordinate.DistanceTo(start.Coordinate) == 0)
            {
                path.Add(new Waypoint(start.Coordinate, true, 0));
                success = true;
                return path;
            }

            int xIdx, yIdx;
            success = true;
            Vector2[] NESWVector = new Vector2[4];
            Vector2[] diagVector = new Vector2[4];
            bool[] NESW = new bool[4];
            Vector2 startV = start.Coordinate; // Start Vector2
            Vector2 goalV = goal.Coordinate; // Goal Vector2

            PriorityQueue open = new PriorityQueue();
            closed = new OccupancyGrid2D(resX, resY, extentX, extentY);
            opened = new OccupancyGrid2D(resX, resY, extentX, extentY);

            GetIndicies(startV.X, startV.Y, out xIdx, out yIdx);
            startV = new Vector2(xIdx, yIdx);
            GetIndicies(goalV.X, goalV.Y, out xIdx, out yIdx);
            goalV = new Vector2(xIdx, yIdx);

            Node root = new Node(goalV, goalV.DistanceTo(startV), 0, null);

            Node current = root;
            open.Push(current);

            // Do the spreading/discovering stuff until we discover a path.
            while (current.xy != startV)
            {
                if (open.q.Count == 0 || open.q.Count > MAX_OPEN)
                {
                    Console.WriteLine("Failure in DSstar. Open count is: " + open.q.Count);
                    success = false;
                    break;
                }
                current = open.Pop();

                NESWVector[0] = new Vector2(current.xy.X, current.xy.Y - 1);
                NESWVector[1] = new Vector2(current.xy.X + 1, current.xy.Y);
                NESWVector[2] = new Vector2(current.xy.X, current.xy.Y + 1);
                NESWVector[3] = new Vector2(current.xy.X - 1, current.xy.Y);

                diagVector[0] = new Vector2(current.xy.X + 1, current.xy.Y - 1);
                diagVector[1] = new Vector2(current.xy.X + 1, current.xy.Y + 1);
                diagVector[2] = new Vector2(current.xy.X - 1, current.xy.Y + 1);
                diagVector[3] = new Vector2(current.xy.X - 1, current.xy.Y - 1);

                for (int i = 0; i < 4; i++)
                {
                    if ((int)og.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) < 255)
                    {
                        if (closed.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) == 0)
                        {
                            NESW[i] = true;
                            if (opened.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) == 0)
                            {
                                open.Push(new Node(NESWVector[i], NESWVector[i].DistanceTo(startV), current.h + 1
                                    + og.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) / blurWeight, current));
                                opened.SetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y, 1);
                            }
                        }
                    }
                }

                for (int i = 0; i < 4; i++)
                {
                    if (NESW[i % 4] && NESW[(i + 1) % 4])
                    {
                        if (og.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) < 255)
                        {
                            if (closed.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) == 0)
                            {
                                if (opened.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) == 0)
                                {
                                    open.Push(new Node(diagVector[i], diagVector[i].DistanceTo(startV), current.h + 1.4
                                        + og.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) / blurWeight, current));
                                    opened.SetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y, 1);
                                }
                            }
                        }
                    }
                }

                for (int i = 0; i < 4; i++) NESW[i] = false;

                closed.SetCellByIdx((int) current.xy.X, (int) current.xy.Y, 1);
            }

            // Build a path using the discovered path.
            double x, y;
            Waypoint waypoint;

//.........这里部分代码省略.........

 public static void DrawEllipse(GLPen p, Vector2 firstPoint, Vector2 secondPoint, Vector2 thirdPoint)
 {
     float height = (float)firstPoint.DistanceTo(thirdPoint);
     DrawEllipse(p, firstPoint, secondPoint, height);
     //Vector2 delta = new Vector2(secondPDFPoint.X - firstPDFPoint.X, secondPDFPoint.Y - firstPDFPoint.Y);
     //double thirdPointAngle = delta.ToRadians();
     //thirdPDFPoint = new Vector2(mu.X + Math.Sin(thirdPointAngle) * height2, mu.Y - Math.Cos(thirdPointAngle) * height2);
 }

        public bool FindPath(ref RRTNode originalRoot, Vector2 goalPoint, List<Polygon> obstacles, out RRTNode goal)
        {
            RRTNode root = new RRTNode(originalRoot.State);
            double distance = goalPoint.DistanceTo(root.State.Pose.ToVector2());
            randomSampleRadius = distance + 15.0;
            timeStep = rand.NextDouble();
            //vSigma = rand.NextDouble() * 5.0;
            numSlice = (int)Math.Round(timeStep / 0.2);
            //if (distance < 2)
            //  timeStep = 0.3;
            //else if (distance > 4)
            //  timeStep = 1.0;
            //else
            //  timeStep = 0.5;

            //numSlice = (int)Math.Round(timeStep / 0.1);
            Stopwatch randomGenerationTimer = new Stopwatch();
            Stopwatch extendingTimer = new Stopwatch();
            Stopwatch closestSearchTimer = new Stopwatch();
            List<Double> randomTime = new List<double>();
            List<Double> extendingTime = new List<double>();
            List<Double> closestSearchTime = new List<double>();
            bool foundPath = false;
            goal = null; //not found yet!
            //RRT is divided into the following steps:
            //0) assume the root node is the first node
            //1) randomly select a sample point in space centered around our robot within some fixed distance. Every 20th can be the goal.
            //2) select the closest node to the sampled point in the existing tree based on xy distance
            //3) generate a control input that drives towards the sample point also biased with our initial control inputs
            //3a)   -Biasing Details:
            //		Select Velocity: Normal Distribution with mean = closest node velocity and sigma = SigmaVelocity
            //		Select Turn Rate:
            //			Apply the following heuristic:  mean = (atan2(yf-yi,xf-xi) - thetaInit)/(delT)
            //																			sigma = SigmaTurnRate
            //4) Divide the total RRT timestep into smaller sub-steps
            //4a) calculate the trajectory at one substep given the control inputs and closest node initial conditions
            //4b) check at each the linear path between the initial and simulation end does not intersect a polygon
            //4c) if intersects, terminate and go to 1.
            //4d) if not intersects
            //4da) if last substep, add the results of this simulation to the closest node as a child
            //4db) else simulate the next subtime step by going to 4a
            //5) Check if the new node added is within some tolerance of the goal node. If so, mark node as goal and you're done! Else, Goto 1.

            //----------------------------------------------------------------------------------------------------------------------------------//
            // Declare variables
            int sampleCount = 0; // counter for sample to be biased every 20th time
            int iterationCount = 0; // counter for termination
            kdTree = new KDTree(2);
            kdTree.insert(RRTNode.ToKey(root.State.Pose.x, root.State.Pose.y), root);
            // 0) assume root note is the first node
            while (!foundPath)
            //for (int i = 0; i < 1000; i++)
            {
                //--- Termination ---//
                iterationCount++;
                if (iterationCount > terminationCount)
                {

                    Console.WriteLine("//-----------------------------------------------------------------------//");
                    Console.WriteLine("Random generation average time: " + (randomTime.Sum() / randomTime.Count) + " ms | total time: " + randomTime.Sum() + " | iteration: " + randomTime.Count);
                    Console.WriteLine("Searching closest node average time: " + (closestSearchTime.Sum() / closestSearchTime.Count) + " ms | total time: " + closestSearchTime.Sum() + " | iteration: " + closestSearchTime.Count);
                    Console.WriteLine("Extending average time: " + (extendingTime.Sum() / extendingTime.Count) + " ms | total time: " + extendingTime.Sum() + " | iteration: " + extendingTime.Count);
                    // Benchmark output
                    Console.WriteLine("|--Simulation average time: " + (simulationTime.Sum() / simulationTime.Count) + " ms | total time: " + simulationTime.Sum() + " | iteration: " + simulationTime.Count);
                    Console.WriteLine("|--Obstacle checking average time: " + (obstacleTime.Sum() / obstacleTime.Count) + " ms | total time: " + obstacleTime.Sum() + " | iteration: " + obstacleTime.Count);
                    Console.WriteLine("//-----------------------------------------------------------------------//");
                    simulationTime.Clear();
                    obstacleTime.Clear();
                    return false;
                }
                //-------------------//

                // 1) randomly select a sample point in space centered around our robot within some fixed distance.
                int actualNumNodesToExtend = numNodesToExtend;
                Vector2 samplePoint;
                if (sampleCount < goalPointSamplingRate)
                {
                    if (rand.NextDouble() > chanceToSampleRoot)
                    {
                        randomGenerationTimer.Start();
                        //double randomX = root.State.Pose.x + (rand.NextDouble() - .5) * randomSampleRadius * 2.0;
                        //double randomY = root.State.Pose.y + (rand.NextDouble() - .5) * randomSampleRadius * 2.0;
                        double randomX = goalPoint.X + (rand.NextDouble() - .5) * randomSampleRadius * 2.0;
                        double randomY = goalPoint.Y + (rand.NextDouble() - .5) * randomSampleRadius * 2.0;
                        samplePoint = new Vector2(randomX, randomY);
                        randomTime.Add(randomGenerationTimer.ElapsedMilliseconds);
                        randomGenerationTimer.Reset();
                    }
                    else
                    {
                        samplePoint = root.Point;
                        actualNumNodesToExtend = 1;
                    }
                    sampleCount++;
                }
                else
                {
                    samplePoint = goalPoint;
                    sampleCount = 0;
                }
//.........这里部分代码省略.........