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