public static void TestDeleteLeafFromTree() { var root = new SimpleTreeNode <int>(1, null); var tree = new SimpleTree <int>(root); var node2 = new SimpleTreeNode <int>(2, root); tree.AddChild(root, node2); var node3 = new SimpleTreeNode <int>(3, root); tree.AddChild(root, node3); var node4 = new SimpleTreeNode <int>(4, node3); tree.AddChild(node3, node4); var count = tree.Count(); var leafCount = tree.LeafCount(); Assert.AreEqual(4, count); Assert.AreEqual(2, leafCount); tree.DeleteNode(node4); count = tree.Count(); leafCount = tree.LeafCount(); Assert.AreEqual(3, count); Assert.AreEqual(2, leafCount); Assert.AreEqual(true, root.Children.Contains(node2)); Assert.AreEqual(true, root.Children.Contains(node3)); Assert.IsNull(node3.Children); }
private void CreateShortcut(SimpleTreeNode <Node> simpNode, Dictionary <string, List <ShortcutEntity> > shortcutDict) { var key = this.GetKey(simpNode.Data.Answer, simpNode.Data.ImageName); if (shortcutDict.ContainsKey(key)) { var shortcuts = shortcutDict[key]; var shortcut = this.RemoveShortcut(shortcuts, simpNode.Data.Id); if (shortcuts.Count == 0) { shortcutDict.Remove(key); } shortcut.Text = simpNode.Data.Answer; shortcut.ImageName = simpNode.Data.ImageName; shortcut.Code = simpNode.Data.Code; if (string.IsNullOrEmpty(shortcut.Title) && !string.IsNullOrEmpty(simpNode.Data.Question)) { shortcut.Title = simpNode.Data.Question; } simpNode.Tag = shortcut; } else { var shortcut = EntityHelper.Create <ShortcutEntity>(this._dbAccount); shortcut.Text = simpNode.Data.Answer; shortcut.Code = simpNode.Data.Code; shortcut.ImageName = simpNode.Data.ImageName; shortcut.Title = simpNode.Data.Question; simpNode.Tag = shortcut; } }
public void FindAfterDelete() { var root = new SimpleTreeNode <int>(0, null); var node1 = new SimpleTreeNode <int>(1, null); var node2 = new SimpleTreeNode <int>(2, null); var node3 = new SimpleTreeNode <int>(3, null); var node4 = new SimpleTreeNode <int>(4, null); var node5 = new SimpleTreeNode <int>(5, null); var tree = new SimpleTree <int>(root); tree.AddChild(root, node1); tree.AddChild(root, node2); tree.AddChild(root, node3); tree.AddChild(node3, node4); tree.AddChild(node3, node5); Assert.True(tree.FindNodesByValue(5).Count == 1); tree.DeleteNode(node5); tree.DeleteNode(node2); Assert.True(tree.Count() == 4); Assert.True(tree.FindNodesByValue(5).Count == 0); Assert.True(tree.GetAllNodes().Count == 4); Assert.True(tree.LeafCount() == 2); }
private void CreateNewShortcut(TreeNode tn, List <TreeNode> nds, SimpleTreeNode <Node> n) { ShortcutEntity shortcut; if (string.IsNullOrEmpty(n.Data.ImageName)) { shortcut = (nds.FirstOrDefault(k => { var sc = k as ShortcutEntity; return(sc.Text == n.Data.Answer); }) as ShortcutEntity); } else { shortcut = (nds.FirstOrDefault(k => { var sc = k as ShortcutEntity; return(sc.ImageName == n.Data.ImageName); }) as ShortcutEntity); } if (shortcut == null) { shortcut = EntityHelper.Create <ShortcutEntity>(_dbAccount); shortcut.Text = n.Data.Answer; shortcut.Code = n.Data.Code; shortcut.ImageName = n.Data.ImageName; _dbAccessor.AddNodeToTargetNode(shortcut, tn.EntityId); } else if (shortcut.Code != n.Data.Code) { shortcut.Code = n.Data.Code; _dbAccessor.SaveNodeToDb(shortcut); } }
public void GetPackagesReturnPrereleasePackages() { // Arrange MockPackageRepository repository = new MockPackageRepository(); int numberOfPackages = 3; IPackage[] packages = new IPackage[numberOfPackages]; for (int i = 0; i < numberOfPackages; i++) { packages[i] = PackageUtility.CreatePackage("A" + i, "1.0-alpha"); repository.AddPackage(packages[i]); } SimpleTreeNode node = CreateSimpleTreeNode(repository); // Act var producedPackages = node.GetPackages(allowPrereleaseVersions: true).ToList(); // Assert Assert.Equal(packages.Length, producedPackages.Count); for (int i = 0; i < numberOfPackages; i++) { Assert.Same(packages[i], producedPackages[i]); } }
public void GetPackagesReturnPrereleasePackagesIfToldSo() { // Arrange var sourceRepository = new MockPackageRepository(); sourceRepository.AddPackage(PackageUtility.CreatePackage("Azo1", "2.0")); sourceRepository.AddPackage(PackageUtility.CreatePackage("Azo2", "3.0-alpha")); sourceRepository.AddPackage(PackageUtility.CreatePackage("B1", "2.0")); sourceRepository.AddPackage(PackageUtility.CreatePackage("B2", "4.0")); IVsExtensionsTreeNode parentTreeNode = new Mock <IVsExtensionsTreeNode>().Object; PackagesProviderBase provider = new MockPackagesProvider() { IncludePrerelease = true }; var baseNode = new SimpleTreeNode(provider, "Online", parentTreeNode, sourceRepository); var searchNode = new PackagesSearchNode(provider, parentTreeNode, baseNode, "Azo"); // Act var packages = searchNode.GetPackages(allowPrereleaseVersions: true).ToList(); // Assert Assert.Equal(2, packages.Count); Assert.Equal("Azo1", packages[0].Id); Assert.Equal(new SemanticVersion("2.0"), packages[0].Version); Assert.Equal("Azo2", packages[1].Id); Assert.Equal(new SemanticVersion("3.0-alpha"), packages[1].Version); }
public IBinaryTree FindPath(IBinaryTree tree) { SimpleTreeNode[,] nodes = tree.GetNodes(); int lines = nodes.GetLength(0); _preprocessedNodes = new SimpleTreeNode[lines, lines]; _largestValues = new SimpleTreeNode[lines, lines]; bool topIsEven = nodes[0, 0].Value % 2 == 0; bool lineCountIsEven = lines % 2 == 0; bool currentLineIsEven = topIsEven; for (int i = 0; i < lines; i++) { for (int j = 0; j <= i; j++) { _largestValues[i, j] = new SimpleTreeNode(nodes[i, j]); _preprocessedNodes[i, j] = new SimpleTreeNode() { Value = (nodes[i, j].Value % 2 == 0) == currentLineIsEven ? nodes[i, j].Value : (int?)null }; } currentLineIsEven = !currentLineIsEven; } for (int i = lines - 2; i >= 0; i--) { for (int j = 0; j <= i; j++) { int?left = _preprocessedNodes[i + 1, j].Value; int?rigth = _preprocessedNodes[i + 1, j + 1].Value; if (!left.HasValue) { _largestValues[i, j].Value += _largestValues[i + 1, j + 1].Value; nodes[i, j].PathDirection = 1; } else if (!rigth.HasValue) { _largestValues[i, j].Value += _largestValues[i + 1, j].Value; } else { if (_largestValues[i + 1, j].Value.Value > _largestValues[i + 1, j + 1].Value.Value) { _largestValues[i, j].Value += _largestValues[i + 1, j].Value; } else { _largestValues[i, j].Value += _largestValues[i + 1, j + 1].Value; nodes[i, j].PathDirection = 1; } } } } return(tree); }
/// <summary> /// Grow tree towards a specified point /// </summary> private bool Extend(Vector2 extendPoint, SimpleTreeNode <Vector2> goalNode, List <SimpleTreeNode <Vector2> > nodeList) { bool foundPath = false; double distance; SimpleTreeNode <Vector2> newNode = new SimpleTreeNode <Vector2>(); SimpleTreeNode <Vector2> neighbor = FindNearestNeighbor(extendPoint, out distance, nodeList); if (distance > stepSize) { newNode.Value = neighbor.Value + stepSize * (extendPoint - neighbor.Value) / distance; } else { newNode.Value = extendPoint; } if (CheckObstacles(newNode, neighbor)) { neighbor.Children.Add(newNode); nodeList.Add(newNode); if (newNode.Value == goalNode.Value) { foundPath = true; } } return(foundPath); }
public void TestGetAllNodes_3() { SimpleTreeNode <int> root = new SimpleTreeNode <int>(1, null); SimpleTreeNode <int> node1 = new SimpleTreeNode <int>(11, null); SimpleTreeNode <int> node2 = new SimpleTreeNode <int>(12, null); SimpleTreeNode <int> node3 = new SimpleTreeNode <int>(13, null); SimpleTreeNode <int>[] nodes = new SimpleTreeNode <int>[] { root, node1, node2, node3 }; SimpleTree <int> tree = new SimpleTree <int>(root); tree.AddChild(root, node1); tree.AddChild(root, node2); tree.AddChild(root, node3); List <SimpleTreeNode <int> > list = tree.GetAllNodes(); int index = 0; foreach (SimpleTreeNode <int> node in list) { Assert.AreEqual(nodes[index], node); index++; } }
public void TestCountLeaf_3() { SimpleTreeNode <int> root = new SimpleTreeNode <int>(0, null); SimpleTree <int> tree = new SimpleTree <int>(root); Assert.AreEqual(1, tree.LeafCount()); }
public void forest() { SimpleTreeNode <int> a1 = new SimpleTreeNode <int>(1, null); SimpleTreeNode <int> b1 = new SimpleTreeNode <int>(2, a1); SimpleTreeNode <int> b2 = new SimpleTreeNode <int>(3, a1); SimpleTreeNode <int> b3 = new SimpleTreeNode <int>(6, a1); SimpleTreeNode <int> c1 = new SimpleTreeNode <int>(5, b1); SimpleTreeNode <int> c2 = new SimpleTreeNode <int>(7, b1); SimpleTreeNode <int> c3 = new SimpleTreeNode <int>(4, b2); SimpleTreeNode <int> c4 = new SimpleTreeNode <int>(8, b3); SimpleTreeNode <int> d1 = new SimpleTreeNode <int>(9, c4); SimpleTreeNode <int> d2 = new SimpleTreeNode <int>(10, c4); SimpleTree <int> tree = new SimpleTree <int>(a1); tree.AddChild(a1, b1); tree.AddChild(a1, b2); tree.AddChild(a1, b3); tree.AddChild(b1, c1); tree.AddChild(b1, c2); tree.AddChild(b2, c3); tree.AddChild(b3, c4); tree.AddChild(c4, d1); tree.AddChild(c4, d2); List <int> list = tree.EvenTrees(); if (list[0] != 1 && list[1] != 3 && list[2] != 1 && list[3] != 6) { Assert.Fail(); } if (tree.LeafCount() != 2) { Assert.Fail(); } }
public static string ToParenthesesFormat(SimpleTreeNode tree) { var sb = new StringBuilder(); WriteNode(tree, sb); return(sb.ToString()); }
public void Completed(Guid guid, string filename, uint depth, Guid parentguid) { WFLogger.NLogger.Info("Completed: Guid={0} Filename={1} Depth={2} ParentGuid={3}", guid, filename, depth, parentguid); SimpleTreeNode <TrackingData> srctreenode = null; SimpleTreeNode <TrackingData> srctree = this.TrackingTree.FirstOrDefault(b => (srctreenode = b.Find(guid)) != null); // could not find (guid,filename) in tracking tree // try parentguid if (srctreenode == null) { srctree = this.TrackingTree.FirstOrDefault(b => (srctreenode = b.Find(parentguid)) != null); srctreenode = srctreenode.Children.Add(new TrackingData(guid, filename)); } if (srctreenode != null) { srctreenode.Value.Notified = true; TreeNode [] tn = this.TreeView.Nodes.Find(guid.ToString(), true); if (tn != null && tn.Count() > 0) { tn[0].Text = filename; // guid.ToString(); } } }
public static List <SimpleTreeNode <DS.TagNode> > GetNodeList(this SimpleTreeNode <DS.TagNode> dnod) { var ret = new List <SimpleTreeNode <DS.TagNode> >(); ret.Add(dnod); ret.AddRange(dnod.Children.GetNodeList()); return(ret); }
public virtual void Visit(SimpleTreeNode <TLeaf> treeNode) { var node1 = treeNode.AsLeaf; if (ReferenceEquals(node1, null) == false) { Visit(node1); return; } var node2 = treeNode.AsBranchDictionaryByIndex; if (ReferenceEquals(node2, null) == false) { Visit(node2); return; } var node3 = treeNode.AsBranchDictionaryByName; if (ReferenceEquals(node3, null) == false) { Visit(node3); return; } var node4 = treeNode.AsBranchList; if (ReferenceEquals(node4, null) == false) { Visit(node4); return; } var node5 = treeNode.AsNodeDictionaryByIndex; if (ReferenceEquals(node5, null) == false) { Visit(node5); return; } var node6 = treeNode.AsNodeDictionaryByName; if (ReferenceEquals(node6, null) == false) { Visit(node6); return; } var node7 = treeNode.AsNodeList; if (ReferenceEquals(node7, null) == false) { Visit(node7); } }
public void TestTree_1Count_J() { SimpleTreeNode <int> node_1 = new SimpleTreeNode <int>(1, null); SimpleTree <int> tree = new SimpleTree <int>(node_1); List <int> forest = tree.EvenTrees(); Assert.IsTrue(forest.Count == 0); }
private void _OuputTrackingTree(SimpleTreeNode <TrackingData> tree, int offset) { Console.WriteLine(string.Format("{0}{1}-{2}", ((new string(' ', offset)).ToString(System.Globalization.CultureInfo.CurrentCulture)), tree.Value.Guid, tree.Value.Filename)); WFLogger.NLogger.Trace("{0}{1}-{2}", ((new string(' ', offset)).ToString(System.Globalization.CultureInfo.CurrentCulture)), tree.Value.Guid, tree.Value.Filename); offset += 2; foreach (var children in tree.Children) { _OuputTrackingTree(children, offset); } }
public void Create() { var root = new SimpleTreeNode <int>(1, null); var tree = new SimpleTree <int>(root); Assert.True(tree.Count() == 1); Assert.True(tree.FindNodesByValue(1).Count == 1); Assert.True(tree.GetAllNodes().Count == 1); Assert.True(tree.LeafCount() == 1); }
public static void TestAddNodeToTree() { var root = new SimpleTreeNode <int>(1, null); var newNode = new SimpleTreeNode <int>(2, null); var tree = new SimpleTree <int>(root); tree.AddChild(root, newNode); Assert.AreEqual(root, newNode.Parent); Assert.AreEqual(true, root.Children.Contains(newNode)); }
// Возвращает массив узлов для класса SimpleTree<int> // private SimpleTreeNode <int>[] GetNodesArray_4() { SimpleTreeNode <int>[] nodes = new SimpleTreeNode <int>[] { new SimpleTreeNode <int>(1, null), // 0 new SimpleTreeNode <int>(11, null), // 1 new SimpleTreeNode <int>(12, null), // 2 new SimpleTreeNode <int>(121, null), // 3 }; return(nodes); }
private void DrawChildren(SimpleTreeNode <Vector2> node) { foreach (SimpleTreeNode <Vector2> child in node.Children) { Vector2[] treeLine = new Vector2[2]; treeLine[0] = node.Value; treeLine[1] = child.Value; GLUtility.DrawLines(new GLPen(Color.Blue, 1.0f), treeLine); DrawChildren(child); } }
public void PropertyNameIsCorrect() { // Arrange MockPackageRepository repository = new MockPackageRepository(); string category = "Mock node"; SimpleTreeNode node = CreateSimpleTreeNode(repository, category: category); // Act & Assert Assert.Equal(category, node.Name); }
private void DrawChildren(SimpleTreeNode<Vector2> node) { foreach (SimpleTreeNode<Vector2> child in node.Children) { Vector2[] treeLine = new Vector2[2]; treeLine[0] = node.Value; treeLine[1] = child.Value; GLUtility.DrawLines(new GLPen(Color.Blue, 1.0f), treeLine); DrawChildren(child); } }
public void SupportsPrereleasePackagesMatchRepositoryBehavior(bool supportsPrereleasePackage) { // Arrange var repository = new Mock <IPackageRepository>(); repository.Setup(r => r.SupportsPrereleasePackages).Returns(supportsPrereleasePackage); SimpleTreeNode node = CreateSimpleTreeNode(repository.Object); // Act && Assert Assert.Equal(supportsPrereleasePackage, node.SupportsPrereleasePackages); }
public void TestGetAllNodes_2() { SimpleTreeNode <int> root = new SimpleTreeNode <int>(1, null); SimpleTree <int> tree = new SimpleTree <int>(root); List <SimpleTreeNode <int> > list = tree.GetAllNodes(); foreach (SimpleTreeNode <int> node in list) { Assert.AreEqual(root, node); } }
// Возвращает массив узлов для класса SimpleTree<int> // private SimpleTreeNode <int>[] GetNodesArray_3() { SimpleTreeNode <int>[] nodes = new SimpleTreeNode <int>[] { new SimpleTreeNode <int>(1, null), // 0 new SimpleTreeNode <int>(11, null), // 1 new SimpleTreeNode <int>(111, null), // 2 new SimpleTreeNode <int>(1111, null), // 3 new SimpleTreeNode <int>(11111, null), // 4 new SimpleTreeNode <int>(111111, null) // 5 }; return(nodes); }
/// <summary> /// Construct a path by working backwards from the last node until you reach the first node in the list /// </summary> private List <SimpleTreeNode <Vector2> > BuildPath(List <SimpleTreeNode <Vector2> > nodeList) { List <SimpleTreeNode <Vector2> > newPath = new List <SimpleTreeNode <Vector2> >(); SimpleTreeNode <Vector2> endNode = nodeList[nodeList.Count - 1]; SimpleTreeNode <Vector2> node = endNode; while (node != nodeList[0]) { newPath.Insert(0, node); node = node.Parent; } return(newPath); }
private IEnumerable <Location> GetAllPredecessors(SimpleTreeNode <Location> node) { List <Location> predecessors = new List <Location>(); while (node != null) { predecessors.Add(node.Value); node = node.Parent; } predecessors.Reverse(); return(predecessors); }
private IEnumerable<Location> GetAllPredecessors(SimpleTreeNode<Location> node) { List<Location> predecessors = new List<Location>(); while (node != null) { predecessors.Add(node.Value); node = node.Parent; } predecessors.Reverse(); return predecessors; }
public IEnumerable<Location> GetRoute(Location source, Location destination, MazeObject[,] maze) { int rows = maze.GetLength(0); int cols = maze.GetLength(1); bool[,] visited = new bool[rows, cols]; Queue<SimpleTreeNode<Location>> treeNodesQueue = new Queue<SimpleTreeNode<Location>>(); SimpleTreeNode<Location> root = new SimpleTreeNode<Location>(source); treeNodesQueue.Enqueue(root); visited[source.Row, source.Col] = true; while (treeNodesQueue.Count > 0) { SimpleTreeNode<Location> node = treeNodesQueue.Dequeue(); if (node.Value == destination) { // traverse the tree bottom-up to get the shortest path IEnumerable<Location> route = GetAllPredecessors(node); return route; } SimpleTreeNode<Location>[] childNodes = new SimpleTreeNode<Location>[] { new SimpleTreeNode<Location>(new Location(node.Value.Row, node.Value.Col - 1)), // left new SimpleTreeNode<Location>(new Location(node.Value.Row, node.Value.Col + 1)), // right new SimpleTreeNode<Location>(new Location(node.Value.Row - 1, node.Value.Col)), // up new SimpleTreeNode<Location>(new Location(node.Value.Row + 1, node.Value.Col)) // down }; node.Children.AddRange(childNodes); foreach (SimpleTreeNode<Location> child in childNodes) { if (child.Value.Row >= 0 && child.Value.Row < rows && child.Value.Col >= 0 && child.Value.Col < cols && !(maze[child.Value.Row, child.Value.Col].Type == MazeObjectType.Brick) && !visited[child.Value.Row, child.Value.Col]) { node.Children.Add(child); treeNodesQueue.Enqueue(child); visited[child.Value.Row, child.Value.Col] = true; } } } return new List<Location>(); }
public IEnumerable <Location> GetRoute(Location source, Location destination, MazeObject[,] maze) { int rows = maze.GetLength(0); int cols = maze.GetLength(1); bool[,] visited = new bool[rows, cols]; Queue <SimpleTreeNode <Location> > treeNodesQueue = new Queue <SimpleTreeNode <Location> >(); SimpleTreeNode <Location> root = new SimpleTreeNode <Location>(source); treeNodesQueue.Enqueue(root); visited[source.Row, source.Col] = true; while (treeNodesQueue.Count > 0) { SimpleTreeNode <Location> node = treeNodesQueue.Dequeue(); if (node.Value == destination) { // traverse the tree bottom-up to get the shortest path IEnumerable <Location> route = GetAllPredecessors(node); return(route); } SimpleTreeNode <Location>[] childNodes = new SimpleTreeNode <Location>[] { new SimpleTreeNode <Location>(new Location(node.Value.Row, node.Value.Col - 1)), // left new SimpleTreeNode <Location>(new Location(node.Value.Row, node.Value.Col + 1)), // right new SimpleTreeNode <Location>(new Location(node.Value.Row - 1, node.Value.Col)), // up new SimpleTreeNode <Location>(new Location(node.Value.Row + 1, node.Value.Col)) // down }; node.Children.AddRange(childNodes); foreach (SimpleTreeNode <Location> child in childNodes) { if (child.Value.Row >= 0 && child.Value.Row < rows && child.Value.Col >= 0 && child.Value.Col < cols && !(maze[child.Value.Row, child.Value.Col].Type == MazeObjectType.Brick) && !visited[child.Value.Row, child.Value.Col]) { node.Children.Add(child); treeNodesQueue.Enqueue(child); visited[child.Value.Row, child.Value.Col] = true; } } } return(new List <Location>()); }
private void ConstructNodeTree(SimpleTreeNode <Node> simpNode) { var entityId = (simpNode.Tag as TreeNode).EntityId; for (int i = 0; i < simpNode.Children.Count; i++) { var n = simpNode.Children[i].Tag as TreeNode; var prev = (i > 0) ? (simpNode.Children[i - 1].Tag as TreeNode) : null; var next = (i < simpNode.Children.Count - 1) ? (simpNode.Children[i + 1].Tag as TreeNode) : null; n.ParentId = entityId; n.PrevId = ((prev != null) ? prev.EntityId : null); n.NextId = ((next != null) ? next.EntityId : null); } }
public void TestCountLeaf_4() { SimpleTreeNode <int> root = new SimpleTreeNode <int>(0, null); SimpleTreeNode <int> node1 = new SimpleTreeNode <int>(0, null); SimpleTreeNode <int> node2 = new SimpleTreeNode <int>(0, null); SimpleTreeNode <int> node3 = new SimpleTreeNode <int>(0, null); SimpleTree <int> tree = new SimpleTree <int>(root); tree.AddChild(root, node1); tree.AddChild(root, node2); tree.AddChild(root, node3); Assert.AreEqual(3, tree.LeafCount()); }
List<ProjectItemCodeDocument> GenerateDependcyHirarchy(ProjectItemCodeDocument document, SimpleTreeNode<ProjectItemCodeDocument> parentNode) { List<ProjectItemCodeDocument> hirarchy = new List<ProjectItemCodeDocument>(); SimpleTreeNode<ProjectItemCodeDocument> currentNode; if(!_codeDocuments.ContainsKey(document)) return hirarchy; // for each include in this document foreach(var doc in _codeDocuments[document]) { // Ignore Selfincludes if(document.Equals(doc)) { continue; } SimpleTreeNode<ProjectItemCodeDocument> parent = parentNode; while((parent = parent.Parent) != null) { if(parent.Value.Equals(doc)) { throw new DependencyTreeException( string.Format("Cyclyc reference flow detected in the dependency Tree. --> {0}", doc.Name), doc); } } currentNode = new SimpleTreeNode<ProjectItemCodeDocument>(doc); parentNode.Children.Add(currentNode); foreach(var doctoAdd in GenerateDependcyHirarchy(doc, currentNode)) { if(!hirarchy.Contains(doctoAdd)) hirarchy.Add(doctoAdd); } if(!hirarchy.Contains(doc)) hirarchy.Add(doc); } return hirarchy; }
/// <summary> /// Check obstacle avoidance by intersecting the line segment between two nodes /// </summary> private bool CheckObstacles(SimpleTreeNode<Vector2> nodeOne, SimpleTreeNode<Vector2> nodeTwo) { bool isSafe = true; LineSegment ls = new LineSegment(nodeOne.Value, nodeTwo.Value); foreach (Polygon poly in bloatedObstacles) { if (poly.DoesIntersect(ls)) { isSafe = false; break; } } return isSafe; }
/// <summary> /// Grow tree towards a specified point /// </summary> private bool Extend(Vector2 extendPoint, SimpleTreeNode<Vector2> goalNode, List<SimpleTreeNode<Vector2>> nodeList) { bool foundPath = false; double distance; SimpleTreeNode<Vector2> newNode = new SimpleTreeNode<Vector2>(); SimpleTreeNode<Vector2> neighbor = FindNearestNeighbor(extendPoint, out distance, nodeList); if (distance > stepSize) { newNode.Value = neighbor.Value + stepSize * (extendPoint - neighbor.Value) / distance; } else { newNode.Value = extendPoint; } if (CheckObstacles(newNode, neighbor)) { neighbor.Children.Add(newNode); nodeList.Add(newNode); if (newNode.Value == goalNode.Value) { foundPath = true; } } return foundPath; }
/// <summary> /// Find the nearest neighbor on the tree for a new point by checking every node on the tree in turn /// </summary> private SimpleTreeNode<Vector2> FindNearestNeighbor(Vector2 point, out double distance, List<SimpleTreeNode<Vector2>> nodeList) { double newDistance; distance = double.PositiveInfinity; SimpleTreeNode<Vector2> nearestNode = new SimpleTreeNode<Vector2>(); foreach (SimpleTreeNode<Vector2> node in nodeList) { newDistance = (node.Value - point).Length; if (newDistance < distance) { distance = newDistance; nearestNode = node; } } return nearestNode; }
/// <summary> /// Plan a path starting at a specified node through a list of waypoint nodes /// </summary> /// <returns>A list of nodes that includes the last waypoint reached but does not include the initial node. /// Returs null if the first waypoint is not reachable after a fixed number of iterations.</returns> private List<SimpleTreeNode<Vector2>> FindPath(SimpleTreeNode<Vector2> startNode, List<SimpleTreeNode<Vector2>> waypointList) { //Declare local variables int pointCounter; int iterationCounter; bool foundPath; bool pathFinderFailed; List<SimpleTreeNode<Vector2>> path = new List<SimpleTreeNode<Vector2>>(); //If there are no active waypoints, return null if (waypointList.Count == 0) return null; //Find a path between the startNode and the first waypoint pointCounter = 20; iterationCounter = 0; foundPath = false; nodeList = new List<SimpleTreeNode<Vector2>>(); nodeList.Add(startNode); //we have to add the startNode so that the nearest neighbor search can initialise. Remove this node later. while (!foundPath) { //Every 20 iterations replace the random point with the first waypoint to bias the search if (pointCounter < 20) { Vector2 randomPoint = new Vector2((double)rand.Next(minX, maxX), (double)rand.Next(minY, maxY)); foundPath = Extend(randomPoint, waypointList[0], nodeList); pointCounter++; iterationCounter++; } else { foundPath = Extend(waypointList[0].Value, waypointList[0], nodeList); pointCounter = 0; iterationCounter++; } //Stop if it takes too long to plan a path to the first waypoint if (iterationCounter > 1000) return null; } //build the path between the startNode and the first waypoint and add it to the output path.AddRange(BuildPath(nodeList)); //If there is more tha one waypoint, repeat this process for all remaining waypoints if (activeWaypoints.Count > 1) { for (int i = 0; i < activeWaypoints.Count - 1; i++) { pointCounter = 20; iterationCounter = 0; foundPath = false; pathFinderFailed = false; nodeList.Clear(); nodeList.Add(activeWaypoints[i]); while (!foundPath) { //Every 20 iterations replace the random point with the first waypoint to bias the search if (pointCounter < 20) { Vector2 randomPoint = new Vector2((double)rand.Next(minX, maxX), (double)rand.Next(minY, maxY)); foundPath = Extend(randomPoint, waypointList[i + 1], nodeList); pointCounter++; iterationCounter++; } else { foundPath = Extend(waypointList[i + 1].Value, waypointList[i + 1], nodeList); pointCounter = 0; iterationCounter++; } //Stop if it takes too long to plan a path to the waypoint if (iterationCounter > 1000) { pathFinderFailed = true; break; } } if (pathFinderFailed) break; //Stop the for loop if no path ca be found between waypoints //build the path between the waypoints and add it to the output path.AddRange(BuildPath(nodeList)); } } return path; }
private void PlannerThread(object o) { //Declare variables List<SimpleTreeNode<Vector2>> waypointsToRemove; SimpleTreeNode<Vector2> roverNode; List<SimpleTreeNode<Vector2>> nodesToKeep; SimpleTreeNode<Vector2> lastSafeWaypoint; List<SimpleTreeNode<Vector2>> currentPath; List<SimpleTreeNode<Vector2>> newPath; List<Vector2> activeWaypointValues; while (running) { Thread.Sleep(50); lock (plannerLock) { //add ghetto fix to robot inside polygon issue foreach (Polygon poly in bloatedObstacles) { if (poly.IsInside(currentLocation)) { bloatedObstacles = new List<Polygon>(); foreach (Polygon smallPoly in obstacles) { Polygon vehiclePolygon = Polygon.VehiclePolygonWithRadius(vehicleRadius); bloatedObstacles.Add(Polygon.ConvexMinkowskiConvolution(smallPoly, vehiclePolygon)); } } } //Create a copy of the output list of nodes to work on, losing all parent/child relationships in the process lastPlannedPath = new List<SimpleTreeNode<Vector2>>(); foreach (SimpleTreeNode<Vector2> node in outputNodeList) { lastPlannedPath.Add(new SimpleTreeNode<Vector2>(node.Value)); } //Create a copy of the waypoints list and see any of them are now inside obstacles activeWaypoints = new List<SimpleTreeNode<Vector2>>(); foreach (SimpleTreeNode<Vector2> node in userWaypoints) { activeWaypoints.Add(node); } //Check to see if any of the waypoints are inside obstacles, if so remove them from the list waypointsToRemove = new List<SimpleTreeNode<Vector2>>(); foreach (SimpleTreeNode<Vector2> waypoint in activeWaypoints) { foreach (Polygon poly in bloatedObstacles) { if (poly.IsInside(waypoint.Value)) { waypointsToRemove.Add(waypoint); break; } } } foreach (SimpleTreeNode<Vector2> waypoint in waypointsToRemove) { activeWaypoints.Remove(waypoint); } //Check to see if the active waypoint list is empty. If so, output a empty path //and go to the next iteration of the main planning loop if (activeWaypoints.Count == 0) { UpdateLastPath(new List<SimpleTreeNode<Vector2>>()); continue; } //Create a new node at the current rover location, add it to the node list from the last planned path, //and set it as the parent of the first node in that list. Make sure to remove this node form the list //before outputting roverNode = new SimpleTreeNode<Vector2>(currentLocation); lastPlannedPath.Insert(0, roverNode); activeWaypoints.Insert(0, roverNode); //Check to see how much of the last path is still valid nodesToKeep = new List<SimpleTreeNode<Vector2>>(); lastSafeWaypoint = lastPlannedPath[0]; nodesToKeep.Add(lastPlannedPath[0]); activeWaypointValues = new List<Vector2>(); int safeWaypoints = 1; //You always clear the first waypoint, as that is the current rover position foreach (SimpleTreeNode<Vector2> waypoint in activeWaypoints) { activeWaypointValues.Add(waypoint.Value); } if (lastPlannedPath.Count > 1) { if (CheckObstacles(lastPlannedPath[0], lastPlannedPath[1])) { for (int i = 1; i < lastPlannedPath.Count; i++) { if (CheckObstacles(lastPlannedPath[i], lastPlannedPath[i - 1])) { nodesToKeep.Add(lastPlannedPath[i]); } else break; if (activeWaypointValues.Contains(lastPlannedPath[i].Value)) { lastSafeWaypoint = lastPlannedPath[i]; activeWaypointValues.Remove(lastPlannedPath[i].Value); safeWaypoints++; } } } } //Initialize a new path to plan from by including all of the safe nodes in the previous solution //up to and including the last safe waypoint. currentPath = new List<SimpleTreeNode<Vector2>>(); for (int i = 0; i <= lastPlannedPath.IndexOf(lastSafeWaypoint); i++) { currentPath.Add(lastPlannedPath[i]); } //Remove cleared waypoints from active list waypointsToRemove.Clear(); for (int i = 0; i < safeWaypoints; i++) { waypointsToRemove.Add(activeWaypoints[i]); } foreach (SimpleTreeNode<Vector2> waypoint in waypointsToRemove) { activeWaypoints.Remove(waypoint); } //plan a path from the last safe waypoint to the end of the active waypoints list //and add it to the current path. Note that noew the active waypoints list starts //at the first waypoint after the last safe one. newPath = FindPath(lastSafeWaypoint, activeWaypoints); if (newPath != null) { currentPath.AddRange(newPath); } //remove the node at the rover's current location and update the last planned path currentPath.RemoveAt(0); UpdateLastPath(currentPath); } } }