public static double DistanceBetween(Node n1, Node n2)
{
var x1 = n1.X;
var y1 = n1.Y;
var x2 = n2.X;
var y2 = n2.Y;
return Math.Sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2));
}
public Node(int x, int y, double distanceFromRoot, double distanceFromGoal, Node parent)
{
X = x;
Y = y;
DistanceFromGoal = distanceFromGoal;
DistanceFromRoot = distanceFromRoot;
Parent = parent;
Children = new List<Node>();
}
public void RemoveChild(Node child)
{
Children.Remove(child);
}
public void AddChild(Node child)
{
Children.Add(child);
}
private static Node MoveTowardRandNode(Node nearestNode, Node randomNode, int edgeLength)
{
var x1 = nearestNode.X;
var y1 = nearestNode.Y;
var x2 = randomNode.X;
var y2 = randomNode.Y;
var newNode = new Node(x2, y2);
var d = DistanceBetween(nearestNode, randomNode);
if (d <= edgeLength)
{
newNode.DistanceFromRoot = nearestNode.DistanceFromRoot + d;
return newNode;
}
newNode.DistanceFromRoot = nearestNode.DistanceFromRoot + edgeLength;
double distFromLine;
double distFromNearestNode;
int num;
double den;
if (x1 > x2 && y1 > y2)
{
for (var x = x2; x <= x1; x++)
{
for (var y = y2; y <= y1; y++)
{
num = Math.Abs((x - x1)*(y1 - y2) - (y - y1)*(x1 - x2));
den = Math.Sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2));
distFromLine = num/den;
distFromNearestNode = Math.Sqrt((x - x1)*(x - x1) + (y - y1)*(y - y1));
if (distFromLine <= 2 && distFromNearestNode <= edgeLength &&
distFromNearestNode > edgeLength - 2)
{
newNode.SetCoordinate(x, y);
return newNode;
}
}
}
}
else if (x1 > x2 && y1 < y2)
{
for (var x = x2; x <= x1; x++)
{
for (var y = y2; y >= y1; y--)
{
num = Math.Abs((x - x1)*(y1 - y2) - (y - y1)*(x1 - x2));
den = Math.Sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2));
distFromLine = num/den;
distFromNearestNode = Math.Sqrt((x - x1)*(x - x1) + (y - y1)*(y - y1));
if (distFromLine <= 2 && distFromNearestNode <= edgeLength &&
distFromNearestNode > edgeLength - 2)
{
newNode.SetCoordinate(x, y);
return newNode;
}
}
}
}
else if (x1 < x2 && y1 > y2)
{
for (var x = x2; x >= x1; x--)
{
for (var y = y2; y <= y1; y++)
{
num = Math.Abs((x - x1)*(y1 - y2) - (y - y1)*(x1 - x2));
den = Math.Sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2));
distFromLine = num/den;
distFromNearestNode = Math.Sqrt((x - x1)*(x - x1) + (y - y1)*(y - y1));
if (distFromLine <= 2 && distFromNearestNode <= edgeLength &&
distFromNearestNode > edgeLength - 2)
{
newNode.SetCoordinate(x, y);
return newNode;
}
}
}
}
else if (x1 < x2 && y1 < y2)
{
for (var x = x2; x >= x1; x--)
{
for (var y = y2; y >= y1; y--)
{
num = Math.Abs((x - x1)*(y1 - y2) - (y - y1)*(x1 - x2));
den = Math.Sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2));
distFromLine = num/den;
distFromNearestNode = Math.Sqrt((x - x1)*(x - x1) + (y - y1)*(y - y1));
if (distFromLine <= 2 && distFromNearestNode <= edgeLength &&
distFromNearestNode > edgeLength - 2)
{
newNode.SetCoordinate(x, y);
return newNode;
}
}
}
}
else if (x1 == x2 && y1 > y2)
{
for (var y = y2; y <= y1; y++)
{
distFromNearestNode = Math.Sqrt((y - y1)*(y - y1));
if (distFromNearestNode <= edgeLength && distFromNearestNode > edgeLength - 2)
{
newNode.SetCoordinate(x1, y);
return newNode;
}
}
}
else if (x1 == x2 && y1 < y2)
{
for (var y = y2; y >= y1; y--)
{
distFromNearestNode = Math.Sqrt((y - y1)*(y - y1));
if (distFromNearestNode <= edgeLength && distFromNearestNode > edgeLength - 2)
{
newNode.SetCoordinate(x1, y);
return newNode;
}
}
}
else if (x1 > x2 && y1 == y2)
{
for (var x = x2; x <= x1; x++)
{
distFromNearestNode = Math.Sqrt((x - x1)*(x - x1));
if (distFromNearestNode <= edgeLength && distFromNearestNode > edgeLength - 2)
{
newNode.SetCoordinate(x, y1);
return newNode;
}
}
}
else if (x1 < x2 && y1 == y2)
{
for (var x = x2; x >= x1; x--)
{
distFromNearestNode = Math.Sqrt((x - x1)*(x - x1));
if (distFromNearestNode <= edgeLength && distFromNearestNode > edgeLength - 2)
{
newNode.SetCoordinate(x, y1);
return newNode;
}
}
}
return null;
}
private bool IsValidNode(Node randomNode, Node nearestNode)
{
var x1 = nearestNode.X;
var y1 = nearestNode.Y;
var x2 = randomNode.X;
var y2 = randomNode.Y;
try
{
if (x1 == x2 && y1 == y2)
{
return false;
}
if (x1 > x2 && y1 > y2)
{
for (var x = x2; x <= x1; x++)
{
for (var y = y2; y <= y1; y++)
{
if (((x - x1)/(x1 - x2)) == ((y - y1)/(y1 - y2)) && _obstaclePoints[x][y])
{
return false;
}
}
}
}
else if (x1 > x2 && y1 < y2)
{
for (var x = x2; x <= x1; x++)
{
for (var y = y2; y >= y1; y--)
{
if (((x - x1)/(x1 - x2)) == ((y - y1)/(y1 - y2)) && _obstaclePoints[x][y])
{
return false;
}
}
}
}
else if (x1 < x2 && y1 > y2)
{
for (var x = x2; x >= x1; x--)
{
for (var y = y2; y <= y1; y++)
{
if (((x - x1)/(x1 - x2)) == ((y - y1)/(y1 - y2)) && _obstaclePoints[x][y])
{
return false;
}
}
}
}
else if (x1 < x2 && y1 < y2)
{
for (var x = x2; x >= x1; x--)
{
for (var y = y2; y >= y1; y--)
{
if (((x - x1)/(x1 - x2)) == ((y - y1)/(y1 - y2)) && _obstaclePoints[x][y])
{
return false;
}
}
}
}
else if (x1 == x2 && y1 > y2)
{
for (var y = y2; y <= y1; y++)
{
if (_obstaclePoints[x1][y])
{
return false;
}
}
}
else if (x1 == x2 && y1 < y2)
{
for (var y = y2; y >= y1; y--)
{
if (_obstaclePoints[x1][y])
{
return false;
}
}
}
else if (x1 > x2 && y1 == y2)
{
for (var x = x2; x <= x1; x++)
{
if (_obstaclePoints[x][y1])
{
return false;
}
}
}
else if (x1 < x2 && y1 == y2)
{
for (var x = x2; x >= x1; x--)
{
if (_obstaclePoints[x][y1])
{
return false;
}
}
}
}
catch (Exception ex)
{
_controller.PrintErrorMessage(ex);
return false;
}
return true;
}
public List<Node> GetNearestNodes(Node node)
{
var nearestNodes = new Dictionary<Node, Double>();
for (var i = _tree.NodeList.Count - 1; i >= 0; i--)
{
var treeNode = _tree.NodeList[i];
var distance = DistanceBetween(treeNode, node);
var pathLength = treeNode.DistanceFromRoot;
if (distance < _neighbourRadius)
nearestNodes.Add(treeNode, pathLength + distance);
}
var sortedList = new List<Node>();
nearestNodes.OrderBy(pair => pair.Value).ToList().ForEach(sortedPair => sortedList.Add(sortedPair.Key));
return sortedList;
}
public void SetConditions(Node startNode, Node goalNode, IEnumerable<Node> obstacles)
{
_tree = new Tree(startNode.X, startNode.Y);
_goalNode = goalNode;
for (var i = 0; i <= _fieldWidth; i++)
{
_goalPoints[i] = new bool[_fieldHeight + 1];
_obstaclePoints[i] = new bool[_fieldHeight + 1];
}
foreach (var obstacle in obstacles)
{
for (var x = obstacle.X - _radius; x < obstacle.X + _radius; x++)
{
for (var y = obstacle.Y - _radius; y < obstacle.Y + _radius; y++)
{
if ((x - obstacle.X)*(x - obstacle.X) + (y - obstacle.Y)*(y - obstacle.Y) <= _radius*_radius &&
x >= 0 && y >= 0 && x <= _fieldWidth && y <= _fieldHeight)
{
_obstaclePoints[x][y] = true;
}
}
}
}
for (var x = goalNode.X - _radius; x <= goalNode.X + _radius; x++)
{
for (var y = goalNode.Y - _radius; y <= goalNode.Y + _radius; y++)
{
if ((x - goalNode.X)*(x - goalNode.X) + (y - goalNode.Y)*(y - goalNode.Y) <= _radius*_radius &&
x >= 0 && y >= 0 && x <= _fieldWidth && y <= _fieldHeight)
{
_goalPoints[x][y] = true;
}
}
}
}
public List<Node> FindPath(Node end)
{
var path = new List<Node> {end};
var node = end;
while (true)
{
try
{
node = node.Parent;
path.Add(node);
if (node.Parent == _tree.Root)
{
return path;
}
}
catch (Exception ex)
{
_controller.PrintErrorMessage(ex);
}
}
}
public Tree(int x, int y)
{
Root = new Node(x, y, 0, Double.MaxValue, null);
NodeList = new List<Node> {Root};
}
public void Remove(Node node)
{
node.Parent.RemoveChild(node);
NodeList.Remove(node);
}
public bool Contains(Node nodeReq)
{
return NodeList.Contains(nodeReq);
}
public void Add(Node parent, Node child)
{
parent.AddChild(child);
NodeList.Add(child);
child.Parent = parent;
}