public bool Connectivity() { var node = nodes.ElementAt(0); foreach (var n in nodes) { var dsf = new DepthFirstSearch(this, node.id); if (!dsf.DFS(this, n.id) && n.id != node.id) { return(true); } } return(false); }
void Start() { destinationPosition = destination.transform.position; agentPosition = agent.transform.position; nodesGenerator.genNodesGrid(ref nodeGrid); nodesGenerator.SetNearestNode(ref nodeGrid, ref agentPosition, ref originNode); Debug.Log("ORIGIN: " + originNode.position); nodesGenerator.SetNearestNode(ref nodeGrid, ref destinationPosition, ref goalNode); currentNode = originNode; openedNodes.Add(currentNode); pathToGoal.Add(destinationPosition); switch (pathFindingType) { case TypeOfPathFinding.BreadthFirstSearch: breadthFirstSearch.BFS(ref currentNode, ref openedNodes, ref goalNode, ref goalFound); break; case TypeOfPathFinding.DepthFirstSearch: depthFirstSearch.DFS(ref currentNode, ref openedNodes, ref goalNode, ref goalFound); break; case TypeOfPathFinding.Dijkstra: dijkstra.DijkstraAlgorithm(ref currentNode, ref openedNodes, ref goalNode, ref goalFound); break; case TypeOfPathFinding.AStar: aStar.AStarAlgorithm(ref currentNode, ref openedNodes, ref goalNode, ref goalFound); break; default: break; } }
public bool FindPath(Point food, Screen screen) { try { State start = new State(this.Head); State goal = new State(food); State result; if (algorithm == Algorithms.BreadthFirstSearch) { result = BreadthFirstSearch.BFS(start, goal, screen); } else { result = DepthFirstSearch.DFS(start, goal, screen); } if (result != null)//tìm thấy đường đi { this.stepIndex = 0; while (result.Previous != null) { path[stepIndex] = result.Coordinates; stepIndex++; result = result.Previous; } return(true); } return(false); } catch { return(false); } }
public void MergePlanes() { GraphAdjList planeGraph = new GraphAdjList(extractionPlaneRec.Count); for (int i = 0; i < extractionPlaneRec.Count; i++) { for (int j = i + 1; j < extractionPlaneRec.Count; j++) { double dis1 = 0; for (int loop = 0; loop < extractionPlaneRec[i].Value; loop++) { dis1 += Math.Abs(extractionPlaneRec[i].Points[loop].x * extractionPlaneRec[j].NormalABC.x + extractionPlaneRec[i].Points[loop].y * extractionPlaneRec[j].NormalABC.y + extractionPlaneRec[i].Points[loop].z * extractionPlaneRec[j].NormalABC.z - extractionPlaneRec[j].D) / extractionPlaneRec[j].D; } dis1 /= extractionPlaneRec[i].Value; double dis2 = 0; for (int loop = 0; loop < extractionPlaneRec[j].Value; loop++) { dis2 += Math.Abs(extractionPlaneRec[j].Points[loop].x * extractionPlaneRec[i].NormalABC.x + extractionPlaneRec[j].Points[loop].y * extractionPlaneRec[i].NormalABC.y + extractionPlaneRec[j].Points[loop].z * extractionPlaneRec[i].NormalABC.z - extractionPlaneRec[i].D) / extractionPlaneRec[i].D; } dis2 /= extractionPlaneRec[j].Value; double dis = (dis1 * extractionPlaneRec[j].Value + dis2 * extractionPlaneRec[i].Value) / (extractionPlaneRec[i].Value + extractionPlaneRec[j].Value); if (extractionPlaneRec[i].NormalABC.Dot(extractionPlaneRec[j].NormalABC) > Math.Cos(10 * Math.PI / 180) && dis < 0.04) { planeGraph.AddEdge(i, j); } } } int groupIdx = 0; int[] clusterInd = new int[extractionPlaneRec.Count]; bool[] visited = new bool[extractionPlaneRec.Count]; DepthFirstSearch dfs = new DepthFirstSearch(); for (int i = 0; i < extractionPlaneRec.Count; i++) { if (!visited[i]) { dfs.DFS(planeGraph, ref visited, i, ref clusterInd, groupIdx); groupIdx++; } } mergedPlaneRec = new List <pointPlaneClass>(); for (int i = 0; i < groupIdx; i++) { mergedPlaneRec.Add(new pointPlaneClass(i, 0)); } for (int i = 0; i < extractionPlaneRec.Count; i++) { mergedPlaneRec[clusterInd[i]].Value += extractionPlaneRec[i].Value; mergedPlaneRec[clusterInd[i]].Points.AddRange(extractionPlaneRec[i].Points); mergedPlaneRec[clusterInd[i]].PointsIdx.AddRange(extractionPlaneRec[i].PointsIdx); foreach (int pi in extractionPlaneRec[i].PointsIdx) { pointLabels[pi] = clusterInd[i]; } } for (int i = 0; i < mergedPlaneRec.Count; i++) { MyVector4 returnedvalue = pointToPlaneClustering.RANSACNormal(mergedPlaneRec[i].Points, null, 0.0002, 0.9, 100); mergedPlaneRec[i].NormalABC = new MyVector3(returnedvalue.x, returnedvalue.y, returnedvalue.z); mergedPlaneRec[i].D = returnedvalue.w; } }