diff --git a/Tree.pde b/Tree.pde
new file mode 100644
index 0000000..e69de29
diff --git a/kdtest/Properties.pde b/kdtest/Properties.pde
index 47adea7..e15e4b3 100644
--- a/kdtest/Properties.pde
+++ b/kdtest/Properties.pde
@@ -31,10 +31,12 @@ color c_pointDefault = c_WHITE;
 color c_pointSelect = color (100, 255, 100);
 
 // Help Strings
-int i_helpStrings = 8;
+int i_helpStrings = 10;
 String[] s_helpStrings = {
    "h: Toggle Help",
    "t: Toggle Console",
+   "r: Toggle Tree",
+   "e: Toggle Gnoman",
    "f: Scale",
    "d: Pan",
    "s: Rotate",
diff --git a/kdtest/Tree.pde b/kdtest/Tree.pde
new file mode 100644
index 0000000..809923b
--- /dev/null
+++ b/kdtest/Tree.pde
@@ -0,0 +1,85 @@
+class KdTree {
+  KdTree _left, _right;
+  Point _loc;
+  int _depth;
+  color _c;
+  boolean _dir; // Vertical->True, Horizontal->False
+
+  public KdTree(Point p, boolean vert, int depth) {
+    _loc = p;
+    _dir = vert;
+    _depth = depth;
+    _c = colorize(depth);
+
+    _left = null;
+    _right = null;
+  }
+
+  void insert(Point p) {
+    KdTree parent = search(p);
+    if(parent._dir) {            // If parent is vertical
+      if(parent._loc._x > p._x) {
+        parent._left = new KdTree(p, !parent._dir, parent._depth + 1);
+      } else {
+        parent._right = new KdTree(p, !parent._dir, parent._depth + 1);
+      }
+    } else {
+      if(parent._loc._y > p._y) {
+        parent._left = new KdTree(p, !parent._dir, parent._depth + 1);
+      } else {
+        parent._right = new KdTree(p, !parent._dir, parent._depth + 1);
+      }
+    }
+  }
+
+  KdTree search(Point p) {
+    if(_dir) {                   // If vertical
+      if(p._x < _loc._x) {       // If p is to the left of _loc
+        if(_left != null) {
+          return _left.search (p);
+        } else {
+          return this;
+        }
+      } else {                    // Then p is to the right of _loc
+        if(_right != null) {
+          return _right.search (p);
+        } else {
+          return this;
+        } 
+      }
+    } else {                      // If Horizontal
+      if(p._y < _loc._y) {        // If p is under _loc
+        if(_left != null) {
+          return _left.search(p);
+        } else {
+          return this;
+        }
+      } else {                   // Then p is over _loc
+        if(_right != null) {
+          return _right.search(p);
+        } else {
+          return this;
+        }
+      }
+    }
+  }
+
+  void draw() {
+    pushStyle();
+    stroke(_c);
+    if (_left != null) {
+      line(_loc._x, _loc._y, _left._loc._x, _left._loc._y);
+      _left.draw();
+    }
+    if (_right != null) {
+      line(_loc._x, _loc._y, _right._loc._x, _right._loc._y);
+      _right.draw();
+    }
+    popStyle();
+  }
+
+  color colorize (int depth) {
+    float d = depth * 0.2;
+    return color((int)255*cos(d) * cos(d), (int)255*abs(sin(d + PI)), (int)255*abs(sin(d+0.5)));
+  }
+}
diff --git a/kdtest/kdtest.pde b/kdtest/kdtest.pde
index f9cf563..0aa9ac5 100644
--- a/kdtest/kdtest.pde
+++ b/kdtest/kdtest.pde
@@ -1,10 +1,11 @@
 Console con;
 Camera cam;
 Mode statusLine;
+KdTree tree;
 
 ArrayList<Point> pointList;
 
-boolean drawCon, drawHelp;
+boolean drawCon, drawHelp, drawTree, drawGnoman;
 float epsilon = 0.000001;
 int G_mode;
 // 1 = SCALE
@@ -23,9 +24,13 @@ void setup() {
   statusLine = new Mode();
 
   pointList = new ArrayList<Point>();
+  tree = null;
 
   drawCon = false;
   drawHelp = true;
+  drawGnoman = true;
+  drawTree = true;
+
   G_mode = 1;
 }
 
@@ -34,7 +39,10 @@ void draw() {
   
   pushMatrix();
     cam.update();
-    drawGnoman(200);
+    if(drawGnoman) drawGnoman(200);
+    if(drawTree && tree != null) {
+      tree.draw();
+    }
     for(Point p: pointList) {
       p.draw();
     }
@@ -80,11 +88,20 @@ void keyPressed() {
     case 'c':
     case 'C':
       pointList.clear();
+      tree = null;
       break;
     case 'h':
     case 'H':
       drawHelp = !drawHelp;
       break;
+    case 'r':
+    case 'R':
+      drawTree = !drawTree;
+      break;
+    case 'e':
+    case 'E':
+      drawGnoman = !drawGnoman;
+      break;
   } 
 }
 
@@ -124,10 +141,16 @@ void drawGnoman(int r) {
 
 void mousePressed() {
   if(G_mode == 4) {
-    pointList.add(cam.transform(new Point(mouseX, mouseY)));
+    Point p = cam.transform(new Point(mouseX, mouseY));
+    pointList.add(p);
+    if(tree != null) {
+      tree.insert(p);
+    }
+    if(tree == null) {
+      tree = new KdTree (p, true, 0); 
+    }
   }
   cam.mousePressed();
-  con.print("Pressed");
 }
 
 void mouseDragged() {