// Creates a test graph, grid of 10 by 10
public static GraphState CreateTest()
{
int n = 10;
List <IState> vertices = new List <IState>();
for (int i = 0; i < n; i++)
{
for (int j = 0; j < n; j++)
{
vertices.Add(new DiscreteState(i, j));
}
}
GraphState g = new GraphState(vertices);
for (int i = 0; i < n - 1; i++)
{
for (int j = 0; j < n - 1; j++)
{
g.AddEdge(new DiscreteState(i, j), new DiscreteState(i + 1, j));
g.AddEdge(new DiscreteState(i, j), new DiscreteState(i, j + 1));
}
}
for (int i = 0; i < n - 1; i++)
{
g.AddEdge(new DiscreteState(i, n - 1), new DiscreteState(i + 1, n - 1));
g.AddEdge(new DiscreteState(n - 1, i), new DiscreteState(n - 1, i + 1));
}
return(g);
}
// Construct discrete graph from file
// It is returning 3 values thus out modifiers
// Parameter neigh tells which neighborhood to use
// Parameter obstacles is filled with vectors
public static void CreateDiscreteFromFile(string filename, int neigh,
out GraphState g, out IState start, out IState goal,
List <Vector3> obstacles)
{
StreamReader sr = new StreamReader(filename);
try {
// Read start coordinates
string[] sxy = sr.ReadLine().Split(' ');
start = new DiscreteState(sxy[0], sxy[1]);
// Read goal coordinates
string[] gxy = sr.ReadLine().Split(' ');
goal = new DiscreteState(gxy[0], gxy[1]);
// Read dimensions of obstacle matrix
string[] dim = sr.ReadLine().Split(' ');
int ydim = int.Parse(dim[0]);
int xdim = int.Parse(dim[1]);
// Read the obstacle matrix
int[,] A = new int[ydim, xdim];
for (int y = 0; y < ydim; y++)
{
string[] bits = sr.ReadLine().Split(' ');
for (int x = 0; x < xdim; x++)
{
A[y, x] = int.Parse(bits[x]);
}
}
// Create all vertices, it creates full ydim by xdim
// vertex space even though some vertices are obstacles
// and have no connections
List <IState> vertices = new List <IState>();
for (int y = 0; y < ydim; y++)
{
for (int x = 0; x < xdim; x++)
{
vertices.Add(new DiscreteState(x, y));
if (A[y, x] == 1) // If it is obstacle, add to list
{
obstacles.Add(new Vector3(x, 0.0f, y));
}
}
}
// Picking the right neighborhood list
List <Vector2> moves = null;
if (neigh == 4)
{
moves = movesN4;
}
else if (neigh == 8)
{
moves = movesN8;
}
else if (neigh == 16)
{
moves = movesN16;
}
else
{
throw new ArgumentException("Neighborhood does not exist.");
}
// Adding edges
g = new GraphState(vertices);
foreach (IState vertex in vertices)
{
DiscreteState state = vertex as DiscreteState;
int x = state.x;
int y = state.y;
Vector2 pos = new Vector2(x, y);
if (A[y, x] == 1) // Vertex is obstacle
{
continue;
}
// Iterate over all neighbors (moves)
foreach (Vector2 move in moves)
{
Vector2 newPos = pos + move;
x = (int)newPos.x;
y = (int)newPos.y;
// Check if edge should exist
// Add edge with current vertex, neighbor vertex
// and magnitude (norm) as weight
if (x >= 0 && x < xdim && y >= 0 && y < ydim &&
A[y, x] == 0)
{
g.AddEdge(
state,
new DiscreteState(x, y),
move.magnitude
);
}
}
}
} finally {
sr.Close(); // Closing stream
}
}
// Creates visibility graph from the given data
public static GraphState CreateVisibilityGraph(int n, int[] button,
Vector2[] points, Vector2 start, Vector2 goal, List <Polygon> polys)
{
// Initialize vertices collection
List <Vector2> vertices = new List <Vector2>(points);
vertices.Add(start);
vertices.Add(goal);
// Initialize polygon and edges collection
List <Edge> edges = new List <Edge>();
List <Vector2> buffer = new List <Vector2>();
for (int i = 0; i < n; i++)
{
buffer.Add(points[i]);
if (button[i] == 3)
{
Polygon newPol = new Polygon(buffer);
polys.Add(newPol);
foreach (Edge e in newPol.IterEdges())
{
edges.Add(e);
}
buffer.Clear();
}
}
// Initialize graph with vertices
List <IState> vert = new List <IState>();
foreach (Vector2 v in vertices)
{
vert.Add(new ContinuousState(v.x, v.y));
}
GraphState g = new GraphState(vert);
// Iterate over all vertices
// This part is O(n^3)
foreach (Vector2 f in vertices)
{
foreach (Vector2 s in vertices)
{
if (f.Equals(s)) // Same vertex
{
continue;
}
// Create current edge
Edge curr = new Edge(f, s);
bool intersects = false;
// Iterate over all edges
// This is inner loop
foreach (Edge e in edges)
{
if (curr.Intersect(e)) // Check each edge
{
intersects = true;
break;
}
}
// If there is any kind of intersection with polygon
// continue to next case
if (intersects)
{
continue;
}
// Checking if midpoint is inside polygon
Vector2 p = (f + s) / 2.0f;
// Find which polygon edge belongs to
Polygon pol = null;
foreach (Polygon polTmp in polys)
{
// Only interested if edge is not actual edge of the
// polygon, but a contact between any other 2 points
// that do not form and edge
if (polTmp.ContainsVertex(f) &&
polTmp.ContainsVertex(s) &&
!polTmp.ContainsEdge(curr))
{
pol = polTmp;
break;
}
}
// Checking if the point is inside the polygon
if (pol != null && pol.IsInside(p))
{
continue;
}
// Otherwise, current edge is visible and is added tp the graph
IState a = new ContinuousState(f.x, f.y);
IState b = new ContinuousState(s.x, s.y);
g.AddEdge(a, b, Vector2.Distance(f, s));
}
}
return(g);
}
