Added code to prevent edges from crossing

graph.cpp

@@ -1,5 +1,6 @@
 #include "graph.h"
 #include "mathutils.h"
+#include "debug.h"
 
 Graph::Graph()
 {
@@ -25,7 +26,35 @@ bool Graph::vertex_present(int x, int y, int r)
 	if (MathUtils::distance(v->x, v->y, x, y) <= v->r) return true;
     }
 
-	return false;
+    return false;
+}
+
+
+bool Graph::vertex_would_overlap(int x, int y, int r)
+{
+    for (list<Vertex*>::iterator cursor = vertices.begin();
+	 cursor != vertices.end(); cursor++)
+    {
+	Vertex* v = *cursor;
+	if (MathUtils::distance(v->x, v->y, x, y) <= v->r + r) return true;
+    }
+
+    return false;
+}
+
+
+bool Graph::crosses_edge(Edge e)
+{
+    for (list<Edge>::iterator cursor = edges.begin();
+	 cursor != edges.end(); cursor++)
+    {
+	Edge c = *cursor;
+	if (MathUtils::lines_intersect(c.a->x, c.a->y, c.b->x, c.b->y,
+				       e.a->x, e.a->y, e.b->x, e.b->y))
+	    return true;
+    }
+
+    return false;
 }
 
 
@@ -56,21 +85,42 @@ void Graph::add_vertex(int x, int y, int r)
     Vertex* v = new Vertex(x, y, r);
 
     // Make sure the nodes won't overlap
-    if (vertex_present(v->x, v->y, v->r + r)) return;
-
-    vertices.push_back(v);
+    if (vertex_would_overlap(v->x, v->y, v->r))
+    {
+#ifdef DEBUG
+	fprintf(stderr, "debug: Graph::add_vertex(): failed to add due to vertex collision: x=%d, y=%d, r=%d\n", v->x, v->y, v->r);
+#endif
+	delete v;
+	return;
+    }
 
     if (current_vertex == NULL)
     {
 	current_vertex = v;
+	vertices.push_back(v);
 	return;
     }
 
     Edge e;
     e.a = current_vertex;
     e.b = v;
+
+    if (crosses_edge(e))
+    {
+#ifdef DEBUG
+	fprintf(stderr, "debug: Graph::add_vertex(): failed to add due to edge collision: x1=%d, y1=%d, x2=%d, y2=%d\n", e.a->x, e.a->y, e.b->x, e.b->y);
+#endif
+	delete v;
+	return;
+    }
+
+    vertices.push_back(v);
     edges.push_back(e);
     current_vertex = v;
+
+#ifdef DEBUG
+    fprintf(stderr, "debug: Graph::add_vertex(): added: x=%d, y=%d, r=%d\n", v->x, v->y, v->r);
+#endif
 }