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; // First waypoint is a user waypoint GetReals((int)current.xy.X, (int)current.xy.Y, out x, out y); waypoint = new Waypoint(x + resX / 2, y + resY / 2, true, og.GetCellByIdx((int)current.xy.X, (int)current.xy.Y)); path.Add(waypoint); current = current.dad; // Middle waypoints are path waypoints while (current != root && current != null) { GetReals((int) current.xy.X, (int) current.xy.Y, out x, out y); waypoint = new Waypoint(x + resX / 2, y + resY / 2, false, og.GetCellByIdx((int)current.xy.X, (int)current.xy.Y)); path.Add(waypoint); current = current.dad; } // Last waypoint is a user waypoint if (current != null) { GetReals((int)current.xy.X, (int)current.xy.Y, out x, out y); waypoint = new Waypoint(x + resX / 2, y + resY / 2, true, og.GetCellByIdx((int)current.xy.X, (int)current.xy.Y)); path.Add(waypoint); } return path; }
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 static void DrawEllipse(GLPen p, Vector2 firstPoint, Vector2 secondPoint, float height) { Vector2 deltaPoint = new Vector2(firstPoint.X - secondPoint.X, firstPoint.Y - secondPoint.Y); double angle = deltaPoint.ToRadians(); deltaPoint = deltaPoint.Rotate90(); //Vector2 thirdPoint = deltaPoint.Normalize() * height; float length = (float)firstPoint.DistanceTo(secondPoint); Vector2 PreRotDrawpoint = new Vector2(0, 0); Vector2 PostRotDrawPoint = new Vector2(0, 0); p.GLApplyPen(); Gl.glPushMatrix(); Gl.glTranslatef((float)firstPoint.X, (float)firstPoint.Y, 0); Gl.glBegin(Gl.GL_LINE_LOOP); for (double i = 0; i < Math.PI * 2; i += 0.05) { PreRotDrawpoint = new Vector2((float)(Math.Cos(i) * length), (float)(Math.Sin(i) * height)); PostRotDrawPoint = PreRotDrawpoint.Rotate(angle); Gl.glVertex2f((float)PostRotDrawPoint.X, (float)PostRotDrawPoint.Y); } Gl.glEnd(); Gl.glPopMatrix(); }
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; } closestSearchTimer.Start(); // 2) select the closest node to the sampled point in the existing tree based on xy distance //List<RRTNode> closestNodes = root.FindNClosestNodeInTree(samplePoint, actualNumNodesToExtend); List<RRTNode> closestNodes = root.FindNClosestNodeInTreeKDTREE(3, samplePoint, kdTree); closestSearchTime.Add(closestSearchTimer.ElapsedMilliseconds); closestSearchTimer.Reset(); extendingTimer.Start(); foreach (RRTNode closeNode in closestNodes) { RRTNode newNode = ExtendNode(ref goalPoint, obstacles, ref goal, ref foundPath, ref samplePoint, closeNode, rand); if (newNode != null) kdTree.insert(RRTNode.ToKey(newNode.State.Pose.x, newNode.State.Pose.y), newNode); } extendingTime.Add(extendingTimer.ElapsedMilliseconds); extendingTimer.Reset(); } 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 true; }