public TreeEdge AddNeighbour(TreeNode neighbour)
{
foreach (TreeEdge node in adjacentEdges)
{
if (node.Destination.GetID == this.index)
{
return(null);
}
}
TreeEdge edge = new TreeEdge(this, neighbour, RRT.DistanceOf2PointsSquared(this.position, neighbour.position));
adjacentEdges.AddFirst(edge);
return(edge);
}
public void AddLeaf(TreeNode node, TreeNode leaf, float stepSize)
{
float dx = leaf.Position.X - node.Position.X;
float dy = leaf.Position.Y - node.Position.Y;
float incline = Math.Abs(dy / dx);
float Xstep = stepSize * Math.Sign(dx);
float Ystep = stepSize * incline * Math.Sign(dy);
int steps = (int)(Math.Abs(dx / stepSize));
PointF dummyPoint = new PointF(node.Position.X, node.Position.Y);
PointF lastDummyPoint = new PointF(-1, -1);
//bool found_a_valid_point = false;
for (int i = 0; i < steps; i++)
{
dummyPoint.X += Xstep;
dummyPoint.Y += Ystep;
if (PointIsValid(dummyPoint, 4))
{
//found_a_valid_point = true;
lastDummyPoint = dummyPoint;
}
else
{
break;
}
}
if (lastDummyPoint.X < 0)
{
return;
}
TreeNode newNode = new TreeNode(lastDummyPoint);
// create bidirectional connection:
TreeEdge edge1 = node.AddNeighbour(newNode);
TreeEdge edge2 = newNode.AddNeighbour(node);
treeEdges.AddFirst(edge1);
treeEdges.AddFirst(edge2);
// Add new node to tree:
treeNodes.AddFirst(newNode);
debugOutput = string.Format("{0} \n {1} \n", debugOutput, new PointF(newNode.Position.X - root.Position.X, newNode.Position.Y - root.Position.Y).ToString());
}
public TreeNode ExtendTreeRand(TreeNode extNode, int StepsNum)
{
TreeNode nearest = null;
float minDist = float.PositiveInfinity;
foreach (TreeNode tn in treeNodes)
{
float tmp = DistanceOf2PointsSquared(tn.Position, extNode.Position);
if (tmp <= minDist)
{
minDist = tmp;
nearest = tn;
}
}
///PointF randPoint = TreeNode.RandomConfigPiont(100, 100, 0.5f);
float dx = extNode.Position.X - nearest.Position.X;
float dy = extNode.Position.Y - nearest.Position.Y;
if (dx * dx + dy * dy > 10000)
{
float dist = (float)Math.Sqrt(dx * dx + dy * dy);
dx = (dx / dist) * 100;
dy = (dy / dist) * 100;
}
if (Math.Abs(dx) < 10 && Math.Abs(dy) < 10)
{
StepsNum = 1;
}
float Xstep = dx / StepsNum;
float Ystep = dy / StepsNum;
PointF dummyPoint = new PointF(nearest.Position.X, nearest.Position.Y);
PointF lastDummyPoint = new PointF(-1, -1);
//bool found_a_valid_point = false;
for (int i = 0; i < StepsNum; i++)
{
dummyPoint.X += Xstep;
dummyPoint.Y += Ystep;
if (PointIsValid(dummyPoint, 4))
{
//found_a_valid_point = true;
lastDummyPoint = dummyPoint;
}
else
{
break;
}
if (DistanceOf2PointsSquared(lastDummyPoint, nearest.Position) > 2500)
{
break;
}
}
if (lastDummyPoint.X < 0)
{
return(null);
}
TreeNode newNode = new TreeNode(lastDummyPoint);
// create bidirectional connection:
TreeEdge edge1 = nearest.AddNeighbour(newNode);
TreeEdge edge2 = newNode.AddNeighbour(nearest);
treeEdges.AddFirst(edge1);
treeEdges.AddFirst(edge2);
// Add new node to tree:
treeNodes.AddFirst(newNode);
return(newNode);
}
public static LinkedList <TreeEdge> LinkedListDijkestra(TreeNode start, TreeNode dest, LinkedList <TreeEdge> graphEdges, LinkedList <TreeNode> graphNodes) //LinkedList<Edge>
{
// TreeNode vnear = start;
TreeNode[] nodes = graphNodes.ToArray <TreeNode>();
float[] length = new float[graphNodes.Count];
TreeNode [] touch = new TreeNode[graphNodes.Count];
int[] touchIndices = new int[graphNodes.Count];
TreeEdge[] edges = graphEdges.ToArray <TreeEdge>();
int i, vnear = 0;
int startIndex = 0;
int destIndex = 0;
for (i = 0; i < nodes.Length; i++)
{
touch[i] = start;
length[i] = edges[i].Cost;
if (nodes[i].GetID == start.GetID)
{
startIndex = i;
}
if (nodes[i].GetID == dest.GetID)
{
destIndex = i;
}
}
for (i = 0; i < nodes.Length; i++)
{
touchIndices[i] = startIndex;
}
for (int j = 0; j < nodes.Length; j++)
{
float min = float.PositiveInfinity;
for (i = 0; i < nodes.Length; i++)
{
if (length[i] >= 0 && length[i] < min)
{
min = length[i];
vnear = i;
}
}
// e = new Edge(touch[vnear], vnear);
// edgesOfGraph.AddLast(e);
for (i = 0; i < nodes.Length; i++)
{
float weight = 0;
foreach (TreeEdge e in nodes[i].NeighbourEdges)
{
if (e.Destination.GetID == nodes[vnear].GetID)
{
weight = e.Cost;
break;
}
}
if (length[vnear] + weight < length[i])
{
length[i] = length[vnear] + weight;
touch[i] = nodes[vnear];
touchIndices[i] = vnear;
}
}
length[vnear] = -1;
}
touch[startIndex] = start;
touchIndices[startIndex] = startIndex;
// Create route comrised of edges:
int index = destIndex;
LinkedList <TreeEdge> Path = new LinkedList <TreeEdge>();
while (true)
{
//Console.Write(index + " ")
TreeEdge edge = null;
foreach (TreeEdge e in nodes[touchIndices[index]].NeighbourEdges)
{
if (e.Destination.GetID == nodes[index].GetID)
{
edge = e;
break;
}
}
Path.AddFirst(edge);
index = touchIndices[index];
if (index == startIndex)
{
// Console.Write((graphVertex)index + " ");
break;
}
}
TreeEdge edgeTmp = null;
foreach (TreeEdge e in nodes[index].NeighbourEdges)
{
if (e.Destination.GetID == nodes[touchIndices[index]].GetID)
{
edgeTmp = e;
break;
}
}
Path.AddFirst(edgeTmp);
return(Path);
/*
* int i, vnear = start;
* // Edge e;
* int[] touch = new int[n];
* float[] length = new float[n];
*
* // LinkedList<Edge> edgesOfGraph = new LinkedList<Edge>();
*
* for (i = 0; i <= n - 1; i++)
* {
* touch[i] = start;
* length[i] = weights[start, i];
* }
*
* for (int j = 0; j < n; j++)
* {
* float min = float.PositiveInfinity;
* for (i = 0; i <= n - 1; i++)
* if (length[i] >= 0 && length[i] < min)
* {
* min = length[i];
* vnear = i;
* }
* // e = new Edge(touch[vnear], vnear);
*
* // edgesOfGraph.AddLast(e);
*
*
* for (i = 0; i <= n - 1; i++)
* {
* if (length[vnear] + weights[vnear, i] < length[i])
* {
* length[i] = length[vnear] + weights[vnear, i];
* touch[i] = vnear;
* }
* }
*
*
*
* length[vnear] = -1;
* }
* Console.WriteLine("this: {0}", length[1]);
*
* //return edgesOfGraph;
* touch[start] = start;
* return touch;
*/
}
