// Equality
public override bool Equals(object other) {
if (!(other is ContinuousState)) {
return false;
}
ContinuousState o = (ContinuousState) other;
return this.x.Equals(o.x) && this.y.Equals(o.y);
}
// Euclidian distance fo continuous state
public static float HCont(IState curr, IState goal)
{
ContinuousState a = curr as ContinuousState;
ContinuousState b = goal as ContinuousState;
return(Vector2.Distance(new Vector2(a.x, a.y), new Vector2(b.x, b.y)));
}
// Function reads the file and creates a visibility graph from the data
public static void CreatePolyFromFile(String filename,
out GraphState graph, out IState start, out IState goal,
List <Polygon> polys)
{
StreamReader sr = new StreamReader(filename);
try {
// Read start
string[] sxy = sr.ReadLine().Split(' ');
start = new ContinuousState(sxy[0], sxy[1]);
// Read goal
string[] gxy = sr.ReadLine().Split(' ');
goal = new ContinuousState(gxy[0], gxy[1]);
// Read number of vertices
int count = int.Parse(sr.ReadLine());
int[] button = new int[count];
Vector2[] vertices = new Vector2[count];
// Read vertices and buttons
for (int i = 0; i < count; i++)
{
string[] line = sr.ReadLine().Split(' ');
vertices[i] = new Vector2(
float.Parse(line[0]), float.Parse(line[1]));
button[i] = int.Parse(line[2]);
}
// Create visibility
graph = CreateVisibilityGraph(count, button, vertices,
start.ToVector2(), goal.ToVector2(), polys);
} finally {
sr.Close(); // Close 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);
}
