public RobotLocator(InternalMap map, RTSWorld world) { this.world = world; this.map = map; currentDirection = map.RobotId == TwoPlayersId.Left ? Direction.Right : Direction.Left; realRobotAngle = currentDirection.ToAngle(); expectedRobotAngle = realRobotAngle; }
private static void TryAddPosition(InternalMap map, Direction direction, InternalPoint position, int xOffset, int yOffset) { var availableDirections = map.AvailableDirectionsByCoordinates[position.X, position.Y]; var point = new InternalPoint(position.X + xOffset, position.Y + yOffset, direction); if ( (availableDirections & direction)!= 0 && !handled.Contains(point)) { AddPoint(point); parents.SafeAdd(point, position); } }
public static Direction[] FindPath(InternalMap map, Point from, Point to) { var lastDirection = Bfs(map, from, to); var directions = new List<Direction>(); var currentPoint = new InternalPoint(to.X, to.Y, lastDirection); while (parents.ContainsKey(currentPoint)) { directions.Add(currentPoint.Direction); currentPoint = parents[currentPoint]; } directions.Reverse(); return directions.ToArray(); }
private static Direction Bfs(InternalMap map, Point from, Point to) { queue = new Queue<InternalPoint>(); parents = new Dictionary<InternalPoint, InternalPoint>(); handled = new HashSet<InternalPoint>(); AddPoint(new InternalPoint(from.X, from.Y, Direction.No)); while (queue.Count > 0) { var position = queue.Dequeue(); if (position.X == to.X && position.Y == to.Y) return position.Direction; TryAddPosition(map, Direction.Down, position, 0, 1); TryAddPosition(map, Direction.Up, position, 0, -1); TryAddPosition(map, Direction.Left, position, -1, 0); TryAddPosition(map, Direction.Right, position, 1, 0); } throw new Exception("Path not found."); }
/// <summary> /// Start the RRT. /// </summary> /// <param name="_Start">Start.</param> /// <param name="_StartOrientation">Start orientation.</param> /// <param name="_End">End.</param> /// <param name="_EndOrientation">End orientation.</param> public void Start(Point _Start, double _StartOrientation, Point _End, double _EndOrientation) { this.StartPoint = InternalMap.FromMapCoordinates(_Start); if (PointValid(StartPoint)) { throw new Exception("StartPoint in invalid region"); } this.StartOrientation = _StartOrientation; this.StartRRTNode = new RRTNode(StartPoint, StartOrientation, null); this.EndPoint = InternalMap.FromMapCoordinates(_End); if (PointValid(EndPoint)) { throw new Exception("EndPoint in invalid region"); } this.EndOrientation = _EndOrientation; this.EndRRTNode = new RRTNode(EndPoint, EndOrientation, null); this.AllNodes.Add(StartRRTNode); //Do n iterations of the algorithm double PreviousProgress = 0; GenerateStartLine(); Console.WriteLine(); for (UInt32 it = 0; it < Iterations; it++) { DoStep(); Progress = (int)(Math.Round(((double)it / (double)Iterations) * 100)); if (Progress != PreviousProgress) { PreviousProgress = Progress; PrintProgress(); } } if (Finished != null) { Finished(this, new EventArgs()); } }
private bool StepStraight(RRTNode node1, RRTNode node2, double Distance, double Angle) { RRTNode start = node1.Clone(); RRTNode end = node2.Clone(); RRTNode lastNode = null; bool success = true; //Connect them for (double i = 0; i <= Distance; i += StepWidthStraight) { int NewX = (int)(start.Position.X + i * Math.Cos(Angle)); int NewY = (int)(start.Position.Y + i * Math.Sin(Angle)); if (InternalMap.IsOccupied(NewX, NewY)) { success = false; break; } RRTNode newNode = null; if (lastNode == null) { newNode = new RRTNode(new System.Drawing.Point(NewX, NewY), node1.Orientation, start); newNode.Inverted = start.Inverted; start.Successors.Add(newNode); } else { newNode = new RRTNode(new System.Drawing.Point(NewX, NewY), node1.Orientation, lastNode); lastNode.Successors.Add(newNode); newNode.Inverted = lastNode.Inverted; } lastNode = newNode; } if (lastNode == null) { success = false; } //We successfully connected them if (success) { end.Predecessor = lastNode; lastNode.AddSucessor(end); if (node1.Predecessor != null) { node1.Predecessor.Successors.Clear(); node1.Predecessor.AddSucessor(start); start.Predecessor = node1.Predecessor; } else { Console.WriteLine("Node1.Predecessor was null"); } if (node2.Successors.Count > 0) { end.AddSucessor(node2.Successors[0]); node2.Successors[0].Predecessor = end; node2.Predecessor = null; node2.Successors.Clear(); } else { Console.WriteLine("Node2.Successor[0] was null"); } node1.Successors.Clear(); node2.Successors.Clear(); node2.Predecessor = null; node1.Predecessor = null; Path.CalculateLength(); return(true); } return(false); }
private bool StepCurve(RRTNode node1, RRTNode node2, double delta, System.Drawing.Point middle, double radius, double angle, double theta) { double outerlength = Math.Abs(delta * RRTHelpers.ToRadians * radius); double steps = outerlength / StepWidthCurve; double angleStep = delta / steps; RRTNode start = node1.Clone(); RRTNode end = node2.Clone(); double beforeLength = 0; RRTNode temp = node1; while (temp.Position != end.Position) { if (temp == null || temp.Predecessor == null) { break; } beforeLength += RRTHelpers.CalculateDistance(temp, temp.Predecessor); temp = temp.Predecessor; } RRTNode lastNode = null; bool success = true; for (int i = 0; i < (int)steps; i++) { int NewX = (int)(middle.X - Math.Cos(RRTHelpers.SanatizeAngle(theta * RRTHelpers.ToDegree + i * angleStep) * RRTHelpers.ToRadians) * radius); int NewY = (int)(middle.Y - Math.Sin(RRTHelpers.SanatizeAngle(theta * RRTHelpers.ToDegree + i * angleStep) * RRTHelpers.ToRadians) * radius); double NewOrientation = RRTHelpers.SanatizeAngle(node1.Orientation + i * angleStep); if (InternalMap.IsOccupied(NewX, NewY)) { success = false; break; } RRTNode newNode = null; if (lastNode == null) { newNode = new RRTNode(new System.Drawing.Point(NewX, NewY), NewOrientation, start); newNode.Inverted = start.Inverted; start.Successors.Add(newNode); } else { newNode = new RRTNode(new System.Drawing.Point(NewX, NewY), NewOrientation, lastNode); lastNode.Successors.Add(newNode); newNode.Inverted = lastNode.Inverted; //RRTHelpers.DrawImportantNode (newNode, InternalMap, 5, Color.DarkOrange); } lastNode = newNode; } if (lastNode == null) { success = false; } //We successfully connected them if (success) { double afterLength = 0; temp = lastNode; while (temp.Position != end.Position) { if (temp == null || temp.Predecessor == null) { break; } afterLength += RRTHelpers.CalculateDistance(temp, temp.Predecessor); temp = temp.Predecessor; } if (afterLength > beforeLength) { return(false); } end.Predecessor = lastNode; lastNode.AddSucessor(end); if (node1.Predecessor != null) { node1.Predecessor.Successors.Clear(); node1.Predecessor.AddSucessor(start); start.Predecessor = node1.Predecessor; } else { Console.WriteLine("Node1.Predecessor was null"); } if (node2.Successors.Count > 0) { end.AddSucessor(node2.Successors[0]); node2.Successors[0].Predecessor = end; node2.Predecessor = null; node2.Successors.Clear(); } else { Console.WriteLine("Node2.Successor[0] was null"); } node1.Successors.Clear(); node2.Successors.Clear(); node2.Predecessor = null; node1.Predecessor = null; Path.CalculateLength(); return(true); } return(false); }
/// <summary> /// Optimize our path so we hit the endpoint /// </summary> public void OptimizeForEndPoint() { //Go along from then nearest point to the endpoint RRTNode previous = Path.Start; Console.WriteLine("Path length before optimization for endpoint: " + Path.Length + " Count: " + Path.CountNodes + " Cost: " + Path.Cost()); Console.WriteLine(); while (previous != null) { if (previous == null) { break; } //Calculate angle delta (angle between orientations) (in degrees) double delta = previous.Orientation - EndPoint.Orientation; double angle = RRTHelpers.CalculateAngle(previous, EndPoint); //Check if the orientation of the selected point is nearly the same as the orientation of the endpoint //if (Math.Abs(previous.Orientation - EndPoint.Orientation) < AllowedOrientationDeviation * 5 || 180 -Math.Abs(previous.Orientation - EndPoint.Orientation) < AllowedOrientationDeviation * 5 ) if (AnglesAreClose(delta, 0, AllowedOrientationDeviation) && (AnglesAreClose(EndPoint.Orientation, angle * RRTHelpers.ToDegree, MaximumDriftAngle))) { //Okey connect them RRTNode selectedNode = previous; RRTNode lastNode = null; //Create a clone we can work on RRTNode start = selectedNode.Clone(); double Distance = RRTHelpers.CalculateDistance(selectedNode, EndPoint); if (Math.Abs(RRTHelpers.SanatizeAngle(angle * RRTHelpers.ToDegree)) > this.MaximumDriftAngle) { previous = previous.Predecessor; continue; } bool success = true; //Connect them for (double i = 0; i <= Distance; i += StepWidthEnd) { //Create new point int NewX = (int)(selectedNode.Position.X + i * Math.Cos(angle)); int NewY = (int)(selectedNode.Position.Y + i * Math.Sin(angle)); //Check if this point is occupied if (InternalMap.IsOccupied(NewX, NewY)) { success = false; break; } RRTNode newNode = null; if (lastNode == null) { newNode = new RRTNode(new System.Drawing.Point(NewX, NewY), start.Orientation, start); start.Successors.Add(newNode); } else { newNode = new RRTNode(new System.Drawing.Point(NewX, NewY), start.Orientation, lastNode); lastNode.Successors.Add(newNode); } lastNode = newNode; } if (lastNode == null) { success = false; } if (success) { Path.Start = lastNode; //Replace the selectNode with our start node. start.Predecessor = selectedNode.Predecessor; selectedNode.Predecessor.Successors.Clear(); selectedNode.Predecessor.AddSucessor(start); selectedNode.Predecessor = null; selectedNode.Successors.Clear(); previous = start; } } previous = previous.Predecessor; } Path.CalculateLength(); Console.WriteLine("Path length after optimization for endpoint: " + Path.Length + " Count: " + Path.CountNodes + " Cost: " + Path.Cost()); }
/// <summary> /// Find a path from the endpoint to the startpoint. /// This is done by looking at all points and selecting the points that are in the given target area. /// Their orientation may only vary by the given AcceptableOrientationDeviation. /// </summary> /// <returns>The path to target.</returns> public List <RRTPath> FindPathToTarget() { //Move area around the endpoint Rectangle TranslatedTargetArea = new Rectangle(EndPoint.X - TargetArea.Width / 2, EndPoint.Y + TargetArea.Height / 2, TargetArea.Width, TargetArea.Height); Graphics g = Graphics.FromImage(InternalMap.ImageMap); g.DrawRectangle(Pens.Fuchsia, new Rectangle(InternalMap.ToMapCoordinates(TranslatedTargetArea.Location), TranslatedTargetArea.Size)); List <RRTNode> NodesInTargetArea = new List <RRTNode>(); List <RRTPath> Paths = new List <RRTPath>(); //Step through all nodes foreach (var item in AllNodes) { //Check if the rectangle contains the point and check the orientation if (Math.Abs(item.Orientation - EndRRTNode.Orientation) < AcceptableOrientationDeviation) { if ((Math.Abs(EndPoint.X - item.Position.X) < TargetArea.Width / 2) && (Math.Abs(EndPoint.Y - item.Position.Y) < TargetArea.Height / 2)) { //Add to the list of found nodes. NodesInTargetArea.Add(item); Console.WriteLine("Found node in target area: " + item); } } } //In case no point was found return if (NodesInTargetArea.Count == 0) { return(Paths); } //Some helpers for creating a nice color for the paths ;) int B = 0; int R = 255; int Step = 255 / NodesInTargetArea.Count; //Step through all found nodes foreach (var item in NodesInTargetArea) { //Calculate length double length = 0; //Follow the Predecessor until their is none (this is the startpoint then) RRTNode previous = item.Predecessor; RRTNode end = null; while (previous != null) { if (previous.Predecessor != null) { //TODO replace with RRTHelpers.CalculateDistance length += RRTHelpers.CalculateDistance(previous, previous.Predecessor); //length += Math.Sqrt (Math.Pow (previous.Position.X - previous.Predecessor.Position.X, 2) + Math.Pow (previous.Position.Y - previous.Predecessor.Position.Y, 2)); } else { end = previous; } previous = previous.Predecessor; } //Create new path from start and end item RRTPath path = new RRTPath(item, end); Paths.Add(path); path.Color = Color.FromArgb(R, 50, B); path.DistanceToEnd = RRTHelpers.CalculateDistance(path.Start, EndRRTNode); path.OrientationDeviation = path.Start.Orientation - EndRRTNode.Orientation; B += Step; R -= Step; } //Sort the list by the cost function of the given paths Paths.Sort((RRTPath x, RRTPath y) => { if (x.Cost() > y.Cost()) { return(1); } else { return(-1); } }); //List<RRTPath> SortedList = Paths.AsParallel().OrderBy(o => o.Cost()).ToList(); foreach (var item in Paths) { Console.WriteLine("Length for path " + item.Color.ToString() + " : " + item.Length + " Distance to End: " + item.DistanceToEnd + " OrientationDif: " + item.OrientationDeviation); } return(Paths); }
/// <summary> /// Determins if the given point is valid /// </summary> /// <returns><c>true</c>, if valid was pointed, <c>false</c> otherwise.</returns> /// <param name="_Point">Point.</param> private bool PointValid(Point _Point) { return(InternalMap.IsOccupied(_Point.X, _Point.Y)); }
public static InternalMap BuildMap(this BotsSensorsData positionSensorsData) { var map = new InternalMap(positionSensorsData); map.AvailableDirectionsByCoordinates = GetAvailableDirections(map.Walls); return map; }