Added code to prevent edges from crossing

debug.h

@@ -3,8 +3,10 @@
 
 #ifdef DEBUG
 #include <iostream>
+#include <cstdio>
 using std::cerr;
 using std::endl;
+using std::fprintf;
 #endif
 
 #endif

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
 }
 
 

graph.h

@@ -43,6 +43,8 @@ class Graph
     Vertex* get_current_vertex() { return current_vertex; }
 
  private:
+    bool vertex_would_overlap(int x, int y, int r);
+    bool crosses_edge(Edge e);
     void add_vertex(int x, int y, int r);
 
     Vertex* current_vertex;

mathutils.cpp

@@ -7,3 +7,20 @@ float MathUtils::distance(float x1, float y1, float x2, float y2)
     float dx = x2 - x1;
     return sqrt(dy*dy + dx*dx);
 }
+
+// Algorithm found at http://paulbourke.net/geometry/lineline2d/
+bool MathUtils::lines_intersect(int x1, int y1, int x2, int y2,
+				int x3, int y3, int x4, int y4)
+{
+    int denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
+
+    int a = (x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3);
+    int b = (x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3);
+
+    if (denom == 0) return false;
+
+    float ua = (float)a / denom;
+    float ub = (float)b / denom;
+
+    return ua > 0 && ua < 1 && ub > 0 && ub < 1;
+}

mathutils.h

@@ -9,6 +9,9 @@ class MathUtils
  public:
     // returns the cartesian distance of two points
     static float distance(float x1, float y1, float x2, float y2);
+
+    static bool lines_intersect(int x1, int y1, int x2, int y2,
+				int x3, int y3, int x4, int y4);
 };
 
 #endif