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 AddBeginPointAroundStartNode(TreeNode node, TreeNode beginPoint, float stepSize)
{
// stepSize = 20;
float distanceBetweenNodesX = beginPoint.Position.X - node.Position.X;
float distanceBetweenNodesY = beginPoint.Position.Y - node.Position.Y;
float incline = Math.Abs(distanceBetweenNodesY / distanceBetweenNodesX);
float Xstep = stepSize * stepSize * Math.Sign(distanceBetweenNodesX);
float Ystep = stepSize * stepSize * Math.Sign(distanceBetweenNodesY); // add incline *
// int steps = (int)(Math.Abs(distanceBetweenNodesX / stepSize));
PointF randomPoint = new PointF(node.Position.X, node.Position.Y);
PointF lastRandomPoint = new PointF(-1, -1);
randomPoint.X += Xstep;
randomPoint.Y += Ystep;
if (PointIsValid(randomPoint))
{
lastRandomPoint = randomPoint;
}
if (lastRandomPoint.X < 0 || lastRandomPoint.Y < 0 || lastRandomPoint.X > maxWidth || lastRandomPoint.Y > maxHeight)
{
return;
}
TreeNode newNode = new TreeNode(lastRandomPoint);
// Make each other neighbour
TreeEdge edge1 = node.AddNeighbour(newNode);
TreeEdge edge2 = newNode.AddNeighbour(node);
treeEdges.AddFirst(edge1);
treeEdges.AddFirst(edge2);
// Add new node to tree:
treeNodes.AddFirst(newNode);
}
public TreeNode ExtendTreeRand(TreeNode extNode, int StepsNum, int stepSize)
{
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;
}
}
float distanceBetweenNodesX = extNode.Position.X - nearest.Position.X;
float distanceBetweenNodesY = extNode.Position.Y - nearest.Position.Y;
if (distanceBetweenNodesX * distanceBetweenNodesX + distanceBetweenNodesY * distanceBetweenNodesY > (stepSize * stepSize))
{
float dist = (float)Math.Sqrt(distanceBetweenNodesX * distanceBetweenNodesX + distanceBetweenNodesY * distanceBetweenNodesY);
distanceBetweenNodesX = (distanceBetweenNodesX / dist) * 100;
distanceBetweenNodesY = (distanceBetweenNodesY / dist) * 100;
}
if (Math.Abs(distanceBetweenNodesX) < 10 && Math.Abs(distanceBetweenNodesY) < 10)
{
StepsNum = 1;
}
float Xstep = distanceBetweenNodesX / StepsNum;
float Ystep = distanceBetweenNodesY / StepsNum;
PointF randomPoint = new PointF(nearest.Position.X, nearest.Position.Y);
PointF lastRandomPoint = new PointF(-1, -1);
randomPoint.X += Xstep;
randomPoint.Y += Ystep;
if (PointIsValid(randomPoint))
{
lastRandomPoint = randomPoint;
}
else
{
}
if (lastRandomPoint.X < 0 || lastRandomPoint.Y < 0)
{
return(null);
}
TreeNode newNode = new TreeNode(lastRandomPoint);
// Make each other neighbour
TreeEdge edge1 = nearest.AddNeighbour(newNode);
TreeEdge edge2 = newNode.AddNeighbour(nearest);
treeEdges.AddFirst(edge1);
treeEdges.AddFirst(edge2);
treeNodes.AddFirst(newNode);
return(newNode);
}
