private PointF?FindMate(Edge edge)
{
PointF?mate = null;
// parameter for vectors intersection
var t = Double.MaxValue;
var bestT = t;
// get normal to the edge
Edge normal = new Edge(edge);
normal.Rotate();
foreach (var point in points)
{
if (IsRightPoint(edge, point))
{
Edge g = new Edge(edge.Destination, point);
// normal to edge g
g.Rotate();
// find point of intersections 2 normals
t = -normal.Intersect(g);
if (t < bestT)
{
bestT = t;
mate = point;
}
}
}
return(mate);
}
public void Intersect2D()
{
var a = new Edge(new Vertice(-1, -1), new Vertice(1, 1));
var b = new Edge(new Vertice(-1, 1), new Vertice(1, -1));
var x = a.Intersect(b);
Assert.IsTrue(x.HasValue && x.Value.X == 0 && x.Value.Y == 0);
a = new Edge(new Vertice(-1, -1), new Vertice(-1, 1));
b = new Edge(new Vertice(1, -1), new Vertice(1, 1));
x = a.Intersect(b);
Assert.IsFalse(x.HasValue);
}
// Check if point is inside polygon
// Using RayCaster algorithm
public bool IsInside(Vector2 p)
{
int count = 0;
Edge ray = new Edge(p, new Vector2(float.MaxValue, p.y));
foreach (Edge e in edges)
{
if (ray.Intersect(e))
{
count++;
}
}
return(count % 2 == 1);
}
// 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);
}
