public static bool Overlap(TreeNode nd1, TreeNode nd2)
{
if (RRT.DistanceOf2PointsSquared(nd1.Position, nd2.Position) <= 4)
{
return(true);
}
return(false);
}
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 bool MergeWith(RRT extTree, int numberOfAttempts)
{
RRT tree1 = this;
RRT tree2 = extTree;
TreeNode newnode1 = tree2.root;
for (int i = 0; i < numberOfAttempts; i++)
{
TreeNode rand = new TreeNode(TreeNode.RandomConfigPiont(maxHeight, maxWidth, 1));
newnode1 = tree1.ExtendTreeRand(rand, 10);
if (newnode1 != null)
{
TreeNode newnode2 = tree2.ExtendTree(newnode1, 10);
if (newnode2 != null)
{
if (TreeNode.Overlap(newnode1, newnode2))
{
foreach (TreeEdge edge in extTree.treeEdges)
{
this.treeEdges.AddFirst(edge);
}
foreach (TreeNode tn in extTree.treeNodes)
{
this.treeNodes.AddFirst(tn);
}
Form1.showMessage("Merged");
return(true);
}
}
RRT tmp = tree1;
tree1 = tree2;
tree2 = tmp;
}
}
Form1.showMessage("Failed");
return(false);
}
private void btnDrawPath_Click(object sender, EventArgs e)
{
Graphics g = this.CreateGraphics();
Pen myPen = new Pen(Brushes.Cyan, 2);
RRT rrt1 = new RRT(5, validLocations, 600, 800, primaryPoint, secondaryPoint);
RRT rrt2 = new RRT(5, validLocations, 600, 800, secondaryPoint, primaryPoint);
//Stopwatch timer = new Stopwatch();
DateTime dt = DateTime.Now;
//timer.Start();
bool merged = rrt1.MergeWith(rrt2, 100);
///timer.Stop();
MessageBox.Show(string.Format("merged: {0} time: {1}", merged, DateTime.Now.Subtract(dt).TotalMilliseconds));//timer.Elapsed.TotalMilliseconds));
foreach (TreeEdge te in rrt1.Edges)
{
g.DrawLine(myPen, te.Source.Position, te.Destination.Position);
}
foreach (TreeNode tn in rrt1.Nodes)
{
DrawCrossHair(g, tn.Position, 4, Pens.Green);
}
MessageBox.Show(string.Format("{0} {1}", rrt1.Edges.Count, rrt1.Nodes.Count));
myPen = new Pen(Brushes.Orange, 2);
myPen.Color = Color.Red;
DrawCrossHair(g, primaryPoint, 10, myPen);
myPen.Color = Color.Yellow;
DrawCrossHair(g, secondaryPoint, 10, myPen);
if (!merged)
{
foreach (TreeEdge te in rrt2.Edges)
{
g.DrawLine(myPen, te.Source.Position, te.Destination.Position);
}
foreach (TreeNode tn in rrt2.Nodes)
{
DrawCrossHair(g, tn.Position, 4, Pens.Violet);
}
}
// LinkedList<TreeEdge> path = rrt1.CalcAndGetPath();
// foreach (TreeEdge te in path)
// {
// g.DrawLine(myPen, te.Source.Position, te.Destination.Position);
// }
/*
* int primaryIndex = getIndexOfPoint(primaryPoint, PointsArray);
*
* routePoints = Dijkestra.dijkestra(primaryIndex, PointsArray.Length, AdjacencyMatrix);
*
*
* int destIndex = getIndexOfPoint(secondaryPoint,PointsArray);
* int startIndex = getIndexOfPoint(primaryPoint, PointsArray);
*
* Pen penTmp = new Pen(Brushes.Red,4);
* DrawCrossHair(g, PointsArray[destIndex], 10, penTmp);
*
* penTmp.Color = Color.Yellow;
* DrawCrossHair(g, PointsArray[startIndex], 10, penTmp);
*
*
* int routePointsCount = 0;
*
* int index = destIndex;
* while (true)
* {
* g.DrawLine(myPen, PointsArray[index], PointsArray[routePoints[index]]);
* index = routePoints[index];
*
*
*
* if (index == startIndex)
* {
* g.DrawLine(myPen, PointsArray[index], PointsArray[routePoints[index]]);
* break;
* }
* routePointsCount++;
* }
*
*/
//MessageBox.Show(routePointsCount.ToString());
}