public int RunSimulation()
{
if (Data.UniqueListOfNodes.Count == null || Data.UniqueListOfNodes.Count < 2)
{
Console.WriteLine("\n\nTrivial or non-existent graph, exiting.");
return(0);
}
//Get the user to enter the starting and destination node
int startNode = getUserToEnterNode(1);
int destinationNode = getUserToEnterNode(2);
if (startNode == destinationNode)
{
Console.WriteLine("\n\nTrivial case, i.e. distance from node to itself is 0, exiting.");
return(0);
}
Console.WriteLine("\n\nRunning simulation.");
//Generate and populate the GraphAdjList object
GraphAdjList graph = new GraphAdjList(Data.UniqueListOfNodes.Count());
for (int i = 0; i < Data.IndexI.Count; i++)
{
graph.AddEdge(Data.IndexI[i], Data.IndexJ[i]);
}
BreadthFirstSearch bfs = new BreadthFirstSearch(graph, startNode, destinationNode);
if (bfs.distTo[destinationNode] != -1)
{
Console.WriteLine("\n\nNode {0} and Node {1} linked after {2} steps. Exiting.", startNode, destinationNode, bfs.distTo[destinationNode]);
}
else
{
Console.WriteLine("\n\nNode {0} and Node {1} are not linked. Exiting.", startNode, destinationNode);
}
return(1);
}
public void TestAdjacencyListGraphTraversal()
{
GraphAdjList g = new GraphAdjList(4);
g.AddEdge(0, 1);
g.AddEdge(0, 2);
g.AddEdge(1, 2);
g.AddEdge(2, 0);
g.AddEdge(2, 3);
g.AddEdge(3, 3);
string dfs = g.DepthFirstTraversal(2).Trim();
Assert.IsTrue(string.Equals(dfs, "2 0 1 3", System.StringComparison.OrdinalIgnoreCase));
string bfs = g.BreadthFirstTraversal(2).Trim();
Assert.IsTrue(string.Equals(bfs, "2 0 3 1", System.StringComparison.OrdinalIgnoreCase));
}
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;
}
}
