Tree.pde

class KdTree {
  KdTree _left, _right, _parent;
  Point _loc;
  float _x1, _x2, _y1, _y2;
  int _depth;
  color _relationC;
  color _boundaryC;
  boolean _dir; // Vertical->True, Horizontal->False
  boolean _parDir; // Left->True, Right->False
  BoundingBox _bb;
  
  public KdTree(KdTree parent, boolean parDir, Point p, boolean vert, int depth, BoundingBox bb) {
    _parent = parent;
    _loc = p;
    _dir = vert;
    _parDir = parDir;
    _depth = depth;
    _bb = bb;
    _boundaryC = colorizeBoundary(depth);
    _relationC = colorizeRelation(depth);
    _left = null;
    _right = null;
    _bb.setColor (colorizeBox(depth));

    calcBoundary();    
  }
  
  
  void insert(Point p) {
    KdTree parent = search(p);
    
    if(parent._dir) {            // If parent is vertical
      if(parent._loc._x > p._x) {
        BoundingBox bb = new BoundingBox(parent._bb.x1, parent._loc._x, parent._bb.y1, parent._bb.y2);
        parent._left = new KdTree(parent, true, p, !parent._dir, parent._depth + 1, bb);
      } else {
        BoundingBox bb = new BoundingBox(parent._loc._x, parent._bb.x2, parent._bb.y1, parent._bb.y2);
        parent._right = new KdTree(parent, false, p, !parent._dir, parent._depth + 1, bb);
      }
    } else {
      if(parent._loc._y > p._y) {
        BoundingBox bb = new BoundingBox(parent._bb.x1, parent._bb.x2, parent._bb.y1, parent._loc._y);
        parent._left = new KdTree(parent, true, p, !parent._dir, parent._depth + 1, bb);
      } else {
        BoundingBox bb = new BoundingBox(parent._bb.x1, parent._bb.x2, parent._loc._y, parent._bb.y2);
        parent._right = new KdTree(parent, false, p, !parent._dir, parent._depth + 1, bb);
      }
    }
  }

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

  ArrayList<Point> report() {
    ArrayList<Point> rightRes = new ArrayList<Point>();
    ArrayList<Point> leftRes = new ArrayList<Point>();
    leftRes.add(_loc);

    if(_left != null)
      leftRes = merge(leftRes, _left.report());    
    if(_right != null)
      rightRes = _right.report();

    addBox (_bb);

    return merge(rightRes, leftRes);
  }
  
  ArrayList<Point> queryBox(BoundingBox range) {
    ArrayList<Point> rightRes = new ArrayList<Point>();
    ArrayList<Point> leftRes = new ArrayList<Point>();
    addBox(_bb); 
    if(inBox(_loc, range)) {
      rightRes.add(_loc);   
    }   
    
    if(_left != null) {
      if(containsBox(range, _left._bb)) 
        leftRes = _left.report();

      else if(intersectsBox(_left._bb, range)) 
        leftRes = _left.queryBox(range);
    }
    
    if(_right != null) {
      if(containsBox(range, _right._bb)) 
        rightRes = merge(_right.report(), rightRes);
      
      else if(intersectsBox(_right._bb, range)) 
        rightRes = merge(_right.queryBox(range), rightRes);
    }
    return merge(rightRes, leftRes);
  }
  
  ArrayList<Point> queryCircle(Point c, float r) {
    ArrayList<Point> rightRes = new ArrayList<Point>();
    ArrayList<Point> leftRes = new ArrayList<Point>();
    addBox(_bb); 
    if(inCircle(c, r, _loc)) 
      rightRes.add(_loc);      
    
    if(_left != null) {
      if(circleContainsBox(_left._bb, c._x, c._y, r)) 
        leftRes = _left.report();

      else if(rectCircleIntersect(_left._bb, c, r)) 
        leftRes = _left.queryCircle(c, r);
    }
    
    if(_right != null) {
      if(circleContainsBox(_right._bb, c._x, c._y, r)) 
        rightRes = merge(_right.report(), rightRes);
      
      else if(rectCircleIntersect(_right._bb, c, r)) 
        rightRes = merge(_right.queryCircle(c, r), rightRes);
    }
    return merge(rightRes, leftRes);
  }
  
  void drawRelations() {
    pushStyle();
    strokeWeight(i_relationWidth);
    stroke(_relationC);
    if(_left != null) {
      line(_loc._x, _loc._y, _left._loc._x, _left._loc._y);
      _left.drawRelations();
    }
    if(_right != null) {
      line(_loc._x, _loc._y, _right._loc._x, _right._loc._y);
      _right.drawRelations();
    }
    popStyle();
  }

  void drawBoundaries () {
    pushStyle();
    strokeWeight(i_boundaryWidth);
    stroke(_boundaryC);
    line(_x1, _y1, _x2, _y2);
    popStyle();
    if(_left != null) {
      _left.drawBoundaries();
    }
    if(_right != null) {
      _right.drawBoundaries();
    }
  }
  
  void calcBoundary () {
    if(_parent == null) {     // Has no Parent, infinite in both directions
      if (_dir) {
        _x1 = _loc._x;
        _x2 = _loc._x;
        _y1 = f_INFINITY;
        _y2 = -f_INFINITY;
      } else {
        _x1 = f_INFINITY;
        _x2 = -f_INFINITY;
        _y1 = _loc._y;
        _y2 = _loc._y;
      }
    } else {                 // Has a parent, half line is made
      if (_dir) {
        _x1 = _loc._x;
        _x2 = _loc._x;
        _y1 = _parent._loc._y;
        if (_parDir) {
          _y2 = -f_INFINITY;
        } else {
          _y2 = f_INFINITY;
        }
      } else {
        _x1 = _parent._loc._x;
        _y1 = _loc._y;
        _y2 = _loc._y;
        if(_parDir) {
          _x2 = -f_INFINITY;
        } else {
          _x2 = f_INFINITY;
        }
      }
      
      if (_parent._parent != null && _parent._parent._parent != null) { // is depth at least depth 3, might be bounded in both dir 
        if (_parDir != _parent._parent._parDir) { // Will "curl" into a prev bar
          if(_dir) {
            _y2 = _parent._parent._parent._loc._y;
          } else {
            _x2 = _parent._parent._parent._loc._x;
          }
        } else {  // Who knows? but it will share same bound as par's par
          if(_dir) {
            _y2 = _parent._parent._y2;
          } else {
            _x2 = _parent._parent._x2;
          }
        }
      }
    }
  }

  color colorizeRelation (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)));
  }

  color colorizeBoundary (int depth) {
    float d = depth * 0.2;
    return color((int)255*abs(sin(d + 0.3)), (int)255*abs(sin(d+3.0)), (int)255*abs(sin(d+0.7))); 
  }

  color colorizeBox (int depth) {
    float d = depth* 0.2;
    int alpha = 0;
    if (depth != 0) {
      alpha = 20;
    }
    return color((int)255*abs(sin(5*d+0.3)), (int)255*abs(cos(3.1*d+2.0)), (int)255*abs(sin(20* d+0.7)), alpha);
  }
}