Tree.pde

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)));
  }
}