Cluster - kdtree

// This builds on TSlackTree which is a 2D implementation of KD-Tree.
// wrote this to see how well it would work (Comparable to SplitTPA)
// It's a lot faster as soon as we are talking about +/-400 points and up.
// This is O(n log n), and effectively come close to linear performance.

function TSlackTree.Clusters(radX,radY: Double): T2DPointArray;
var
  rescount, qcount: Int32;
  sqx, sqy, sqxy: Double;

  function Fits(p,c: TPoint): Boolean;
  begin
    //Result := (Abs(p.x-c.x) < radX) and (Abs(p.y-c.y) < radY);
    Result := (Sqr(p.x-c.x)*sqy)+(Sqr(p.y-c.y)*sqx) <= sqxy;
  end;

  procedure Cluster(test: TPoint; var result:TPointArray; this: PSlackNode; depth:Int32=0);
  var
    goright:Boolean = False;
    goleft: Boolean = False;
  begin
    if depth and 1 = 0 then begin
      goleft  := test.x - radX <= this^.split.x;
      goright := test.x + radX >= this^.split.x;
    end else begin
      goleft  := test.y - radY <= this^.split.y;
      goright := test.y + radY >= this^.split.y;
    end;

    if (not this^.hidden) and ((goleft=goright)=True) and Fits(test, this^.split) then
    begin
      if rescount = Length(result) then Setlength(result, rescount*2);
      result[rescount] := this^.split;
      Inc(rescount);
      this^.hidden := True;
      Cluster(this^.split, result, @self.data[0], 0);
    end;

    if goleft  and (this^.l <> -1) then Cluster(test, result, @self.data[this^.l], depth+1);
    if goright and (this^.r <> -1) then Cluster(test, result, @self.data[this^.r], depth+1);
  end;
var
  i,j,r:Int32;
  t: TIntegerArray;
begin
  sqx := Sqr(radx);
  sqy := Sqr(rady);
  sqxy := sqx*sqy;

  j := 0;
  for i:=0 to High(self.data) do
  begin
    if self.data[i].hidden then
      continue;

    SetLength(result, j+1);
    rescount := 0;
    SetLength(result[j], 64);

    Cluster(self.data[i].split, result[j], @self.data[0]);
    self.data[i].hidden := True;
    SetLength(result[j], rescount);
    Inc(j);
  end;
end;