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