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