diff --git a/irteus/Makefile b/irteus/Makefile
index 5be1311c4..6eca83b4a 100644
--- a/irteus/Makefile
+++ b/irteus/Makefile
@@ -90,7 +90,7 @@ IRTEUSGLDLLS=$(addprefix $(INSTALLOBJDIR)/,$(IRTEUSGL_LSFX))
 IRTEUSGL_C=$(addsuffix .c,$(IRTEUSGL))
 IRTEUSGL_H=$(addsuffix .h,$(IRTEUSGL))
 
-IRTCOBJECTS=$(INSTALLOBJDIR)/irtc.$(OSFX) $(INSTALLOBJDIR)/irtgeoc.$(OSFX)
+IRTCOBJECTS=$(INSTALLOBJDIR)/irtc.$(OSFX) $(INSTALLOBJDIR)/irtgeoc.$(OSFX) $(INSTALLOBJDIR)/ckdtree.$(OSFX)
 IRTGCOBJECTS=$(INSTALLOBJDIR)/CPQP.$(OSFX) $(INSTALLOBJDIR)/euspqp.$(OSFX)
 IRTIMGCOBJECTS=$(INSTALLOBJDIR)/euspng.$(OSFX)
 NROBJECTS=$(INSTALLOBJDIR)/nr.$(OSFX)
@@ -202,6 +202,7 @@ $(INSTALLOBJDIR)/euspng.$(OSFX): euspng.c
 	$(CC) $(CFLAGS) -c euspng.c $(OBJOPT)$(INSTALLOBJDIR)/euspng.$(OSFX)
 $(INSTALLOBJDIR)/nr.$(OSFX): nr.c
 	$(CC) $(CFLAGS) -c nr.c $(OBJOPT)$(INSTALLOBJDIR)/nr.$(OSFX)
-
+$(INSTALLOBJDIR)/ckdtree.$(OSFX): ckdtree.c
+	$(CC) $(CFLAGS) -c ckdtree.c $(OBJOPT)$(INSTALLOBJDIR)/ckdtree.$(OSFX)
 $(INSTALLLIBDIR)/jpegmemcd.$(LSFX): $(EUSDIR)/lisp/image/jpeg/jpegmemcd.c
 	(cd $(EUSDIR)/lisp/image/jpeg/;make ARCH=$(ARCHDIR) LIBDIR=$(INSTALLLIBDIR) OBJDIR=$(INSTALLOBJDIR))
diff --git a/irteus/ckdtree.c b/irteus/ckdtree.c
new file mode 100644
index 000000000..a3d9eda28
--- /dev/null
+++ b/irteus/ckdtree.c
@@ -0,0 +1,427 @@
+#pragma init (register_cvoxel)
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <math.h>
+#include <setjmp.h>
+#include <errno.h>
+#include <sys/time.h>
+
+#include "eus.h"
+
+extern pointer ___ckdtree();
+static register_ckdtree()
+  { add_module_initializer("___ckdtree", ___ckdtree);}
+
+extern void *kdtree_init (eusfloat_t *points, int nPts, int dim);
+extern void kdtree_k_search (eusinteger_t kdTree, eusfloat_t* points, int k, eusfloat_t eps, eusfloat_t *ret[], eusinteger_t *ret_idx);
+extern int  kdtree_fr_search (eusinteger_t kdTree, eusfloat_t* points, int k, eusfloat_t r, eusfloat_t eps, eusfloat_t *ret[], eusinteger_t *ret_idx[]);
+extern void kdtree_quit (eusinteger_t kdtree);
+extern void kdtree_k_search_matrix (eusinteger_t kdTree, eusfloat_t* points, int n, int k, eusfloat_t eps, eusfloat_t* ret[], eusinteger_t* ret_idx, eusfloat_t* dlst);
+
+pointer C_KDTREE_INIT (ctx,n,argv)
+     register context *ctx;
+     int n;
+     register pointer argv[];
+{
+  eusfloat_t *points;
+  int n_points, dim;
+  points = argv[0]->c.fvec.fv;
+
+  n_points = ckintval(argv[1]);
+  dim = ckintval(argv[2]);
+
+  return makeint((eusinteger_t)(kdtree_init(points, n_points, dim)));
+}
+// copy from jskgeoc.c
+#define colsize(p) (intval(p->c.ary.dim[1]))
+#define rowsize(p) (intval(p->c.ary.dim[0]))
+#define ismatrix(p) ((isarray(p) && \
+                      p->c.ary.rank==makeint(2) && \
+                      elmtypeof(p->c.ary.entity)==ELM_FLOAT))
+pointer C_KDTREE_K_SEARCH_MATRIX (ctx,n,argv)
+     register context *ctx;
+     int n;
+     register pointer argv[];
+{
+  numunion nu;
+  eusinteger_t kdtree = ckintval(argv[0]);
+  eusfloat_t* points;
+  int k = ckintval(argv[2]);
+  int rownum;
+  eusfloat_t eps = ckfltval(argv[3]);
+  eusfloat_t *ret;
+  eusinteger_t *idx = NULL;
+  eusfloat_t *dlst = NULL;
+  /* addr mat k eps (index-vector) (distance-vector) */
+
+  ckarg2(4,6);
+  if(n > 4) {
+    if (isintvector(argv[4])) {
+      idx = argv[4]->c.ivec.iv;
+    } else {
+      idx = (eusinteger_t *)ckintval(argv[4]);
+    }
+  }
+  if(n > 5) {
+    if (isfltvector(argv[5])) {
+      dlst = argv[5]->c.fvec.fv;
+    } else {
+      dlst = (eusfloat_t *)ckintval(argv[5]);
+    }
+  }
+
+  if(!ismatrix(argv[1])) error(E_TYPEMISMATCH);
+  //colsize(argv[1]); // have to check dimension
+  rownum = rowsize(argv[1]);
+  points = argv[1]->c.ary.entity->c.fvec.fv;
+
+  kdtree_k_search_matrix(kdtree, points, rownum, k, eps, &ret, idx, dlst);
+
+  return NIL;
+}
+
+pointer C_KDTREE_K_SEARCH_MATRIX_RAW (ctx,n,argv)
+     register context *ctx;
+     int n;
+     register pointer argv[];
+{
+  numunion nu;
+  eusfloat_t *spts, *dpts;
+  int i,j,k = ckintval(argv[2]);
+  int snum, dnum, col;
+  eusfloat_t eps = ckfltval(argv[3]);
+  eusfloat_t least_min = 1.0e300;
+  eusinteger_t *idx = NULL;
+  eusfloat_t *dlst = NULL;
+
+  /* mat mat k eps (index-vector) (distance-vector) */
+  ckarg2(4,6);
+  if(n > 4) {
+    if (isintvector(argv[4])) {
+      idx = argv[4]->c.ivec.iv;
+    } else {
+      idx = (eusinteger_t *)ckintval(argv[4]);
+    }
+  }
+  if(n > 5) {
+    if (isfltvector(argv[5])) {
+      dlst = argv[5]->c.fvec.fv;
+    } else {
+      dlst = (eusfloat_t *)ckintval(argv[5]);
+    }
+  }
+
+  if(!ismatrix(argv[0])) error(E_TYPEMISMATCH);
+  if(!ismatrix(argv[1])) error(E_TYPEMISMATCH);
+
+  dnum = rowsize(argv[0]);
+  dpts = argv[0]->c.ary.entity->c.fvec.fv;
+  col = colsize(argv[0]);
+
+  snum = rowsize(argv[1]);
+  spts = argv[1]->c.ary.entity->c.fvec.fv;
+  if(col != colsize(argv[1])) error(E_TYPEMISMATCH);
+
+  eusfloat_t *stmp, *dtmp;
+  for(i = 0; i < snum; i++) {
+    eusfloat_t dmin = 1.0e300;
+    eusinteger_t id = -1;
+    stmp = spts + i * col;
+
+    for(j = 0; j < dnum; j++) {
+      dtmp = dpts + j * col;
+      eusfloat_t dist = 0.0, d;
+
+      for(k = 0; k < col; k++) {
+        d = stmp[k] - ( *dtmp++ );
+        dist += d * d;
+      }
+
+      if(dist < dmin) {
+        dmin = dist;
+        id = j;
+      }
+    }
+    if(idx != NULL) idx[i] = id;
+    {
+      eusfloat_t tmp = sqrt(dmin);
+      if(dlst != NULL) dlst[i] = tmp;
+      if(tmp < least_min) least_min = tmp;
+    }
+  }
+  return makeflt(least_min);
+}
+
+pointer C_KDTREE_K_SEARCH (ctx,n,argv)
+     register context *ctx;
+     int n;
+     register pointer argv[];
+{
+  numunion nu; int i, pc = 0;
+  eusinteger_t kdtree = ckintval(argv[0]);
+  eusfloat_t* point = argv[1]->c.fvec.fv;
+  int k = ckintval(argv[2]);
+  eusfloat_t eps = ckfltval(argv[3]);
+  eusfloat_t *ret;
+  eusinteger_t *idx = NULL;
+  if(n > 3) {
+    if (isintvector(argv[4])) {
+      idx = argv[4]->c.ivec.iv;
+    } else {
+      idx = (eusinteger_t *)ckintval(argv[4]);
+    }
+  }
+
+  kdtree_k_search(kdtree, point, k, eps, &ret, idx);
+
+  int dim = vecsize(argv[1]);
+  pointer mat = makematrix(ctx,k,dim); vpush(mat); pc++;
+  for (i = 0; i < k*dim; i++){
+    mat->c.ary.entity->c.fvec.fv[i] = ret[i];
+  }
+  free(ret);
+  while(pc-->0) vpop();
+  return mat;
+}
+
+pointer C_KDTREE_FR_SEARCH (ctx,n,argv)
+     register context *ctx;
+     int n;
+     register pointer argv[];
+{
+  numunion nu; int i, pc = 0;
+  eusinteger_t kdtree = ckintval(argv[0]);
+  eusfloat_t* point = argv[1]->c.fvec.fv;
+  int k = ckintval(argv[2]);
+  eusfloat_t radius = ckfltval(argv[3]);
+  eusfloat_t eps = ckfltval(argv[4]);
+  eusfloat_t *ret;
+  eusinteger_t *idx = NULL;
+  eusinteger_t use_idx = 0;
+  if(n > 4) {
+    use_idx = intval(argv[5]);
+  }
+
+  k = kdtree_fr_search(kdtree, point, k, radius, eps, &ret, &idx);
+
+  if ( k <= 0 ) { return NIL;}
+
+  pointer mat;
+  if(!use_idx) {
+    int dim = vecsize(argv[1]);
+    mat = makematrix(ctx,k,dim); vpush(mat); pc++;
+    for (i = 0; i < k*dim; i++){
+      mat->c.ary.entity->c.fvec.fv[i] = ret[i];
+    }
+  } else {
+    mat = makevector(C_INTVECTOR, k); vpush(mat); pc++;
+    for(i = 0; i < k; i++) {
+      mat->c.ivec.iv[i] = idx[i];
+    }
+  }
+  free(ret);
+  free(idx);
+
+  while(pc-->0) vpop();
+  return mat;
+}
+
+pointer C_KDTREE_QUIT (ctx,n,argv)
+     register context *ctx;
+     int n;
+     register pointer argv[];
+{
+  eusinteger_t kdtree = intval(argv[0]);
+  kdtree_quit(kdtree);
+
+  return NIL;
+}
+
+#if 0
+// for speed up calculating distance between point cloud
+pointer C_KDTREE_SEARCH_AND_CALC_DISTANCE (ctx,n,argv)
+     register context *ctx;
+     int n;
+     register pointer argv[];
+{
+  // src->model points, dst->measured points
+  //           0        1           2        3           4   5         6        7           8
+  // kdtree_addr, src-mat, src-normal, dst-mat, dst-normal, vp(dst), idx-vec, dist-vec, point_step
+  numunion nu;
+  int i;
+  eusinteger_t kdtree = ckintval(argv[0]);
+  int k = 1;
+  int rownum;
+  eusfloat_t eps = 4.0e-7;//single-float-epsilon
+  eusfloat_t *ret;
+  eusinteger_t *idx = NULL;
+  eusfloat_t *dlst = NULL;
+  int point_step = 3;
+  int pnum = 0;
+  eusfloat_t *points;
+  eusfloat_t *normals;
+  eusfloat_t *spts;
+  eusfloat_t *dpts;
+  eusfloat_t *dnormals;
+  eusfloat_t *vp;
+
+  ckarg2(8,9);
+  if (isintvector(argv[6])) {
+    idx = argv[6]->c.ivec.iv;
+  } else {
+    idx = ckintval(argv[6]);
+  }
+  if (isfltvector(argv[7])) {
+    dlst = argv[7]->c.fvec.fv;
+  } else {
+    dlst = ckintval(argv[7]);
+  }
+  if(n > 8) {
+    point_step = ckintval(argv[8]);
+  }
+
+  if(!ismatrix(argv[1])) error(E_TYPEMISMATCH);
+  //colsize(argv[1]); // have to check dimension
+  rownum = rowsize(argv[1]);
+  points = argv[1]->c.ary.entity->c.fvec.fv;
+  normals = argv[2]->c.ary.entity->c.fvec.fv;
+  vp = argv[5]->c.fvec.fv;
+  spts = (eusfloat_t *)malloc(sizeof(eusfloat_t) * rownum * point_step);
+
+  // make new points
+  for(i=0;i<rownum;i++) {
+    eusfloat_t vx = vp[0] - points[i*point_step + 0];
+    eusfloat_t vy = vp[1] - points[i*point_step + 1];
+    eusfloat_t vz = vp[2] - points[i*point_step + 2];
+    eusfloat_t nx = normals[i*3 + 0];
+    eusfloat_t ny = normals[i*3 + 1];
+    eusfloat_t nz = normals[i*3 + 2];
+    idx[i] = -1;
+    if (nx * vx  + ny * vy + nz * vz > 0) {
+      memcpy(&(spts[pnum*point_step]), &(points[i*point_step]), sizeof(eusfloat_t)*point_step);
+      pnum++;
+    }
+  }
+
+  kdtree_k_search_matrix(kdtree, spts, pnum, k, eps, &ret, idx, dlst);
+
+  dpts = argv[3]->c.ary.entity->c.fvec.fv;
+  dnormals = argv[4]->c.ary.entity->c.fvec.fv;
+  // recalc_dist
+  for(i=0;i<pnum;i++) {
+    int j = idx[i];
+    eusfloat_t sx = spts[i*point_step + 0];
+    eusfloat_t sy = spts[i*point_step + 1];
+    eusfloat_t sz = spts[i*point_step + 2];
+    eusfloat_t dx = dpts[j*point_step + 0];
+    eusfloat_t dy = dpts[j*point_step + 1];
+    eusfloat_t dz = dpts[j*point_step + 2];
+    eusfloat_t nx = dnormals[j*3 + 0];
+    eusfloat_t ny = dnormals[j*3 + 1];
+    eusfloat_t nz = dnormals[j*3 + 2];
+    dlst[i] = abs((sx - dx) * nx + (sy - dy) * ny + (sz - dz) * nz) * dlst[i];
+  }
+  free(spts);
+  return makeint(pnum);
+}
+#endif
+// for speed up calculating distance between point cloud
+pointer C_KDTREE_SEARCH_AND_CALC_DISTANCE (ctx,n,argv)
+     register context *ctx;
+     int n;
+     register pointer argv[];
+{
+  // src -> measured points, dest -> model points
+  //           0        1        2           3        4         5           6
+  // kdtree_addr, src-mat, dst-mat, dst-normal, idx-vec, dist-vec, point_step
+  numunion nu;
+  int i;
+  eusinteger_t kdtree = ckintval(argv[0]);
+  int k = 1;
+  int rownum;
+  eusfloat_t eps = 4.0e-7;//single-float-epsilon
+  eusfloat_t *ret;
+  eusinteger_t *idx = NULL;
+  eusfloat_t *dlst = NULL;
+  int point_step = 3;
+
+  eusfloat_t *points;
+  eusfloat_t *normals;
+  eusfloat_t *spts;
+  eusfloat_t *dpts;
+  eusfloat_t *dnormals;
+  eusfloat_t *vp;
+
+  ckarg2(6,7);
+  if (isintvector(argv[4])) {
+    idx = argv[4]->c.ivec.iv;
+  } else {
+    idx = (eusinteger_t *)ckintval(argv[4]);
+  }
+  if (isfltvector(argv[5])) {
+    dlst = argv[5]->c.fvec.fv;
+  } else {
+    dlst = (eusfloat_t *)ckintval(argv[5]);
+  }
+  if(n > 6) {
+    point_step = ckintval(argv[6]);
+  }
+
+  if(!ismatrix(argv[1])) error(E_TYPEMISMATCH);
+  if(!ismatrix(argv[2])) error(E_TYPEMISMATCH);
+  if(!ismatrix(argv[3])) error(E_TYPEMISMATCH);
+  //colsize(argv[1]); // have to check dimension
+  rownum = rowsize(argv[1]);
+  points = argv[1]->c.ary.entity->c.fvec.fv;
+
+  kdtree_k_search_matrix(kdtree, points, rownum, k, eps, &ret, idx, dlst);
+
+  dpts = argv[2]->c.ary.entity->c.fvec.fv;
+  dnormals = argv[3]->c.ary.entity->c.fvec.fv;
+  // recalc_dist
+  for(i=0;i<rownum;i++) {
+    int j = idx[i];
+    eusfloat_t sx = points[i*point_step + 0];
+    eusfloat_t sy = points[i*point_step + 1];
+    eusfloat_t sz = points[i*point_step + 2];
+    eusfloat_t dx = dpts[j*point_step + 0];
+    eusfloat_t dy = dpts[j*point_step + 1];
+    eusfloat_t dz = dpts[j*point_step + 2];
+    eusfloat_t nx = dnormals[j*3 + 0];
+    eusfloat_t ny = dnormals[j*3 + 1];
+    eusfloat_t nz = dnormals[j*3 + 2];
+
+    dlst[i] *= fabs(((sx - dx) * nx) + ((sy - dy) * ny) + ((sz - dz) * nz));
+  }
+
+  return NIL;
+}
+
+pointer ___ckdtree(ctx, n, argv, env)
+register context *ctx; int n; pointer *argv; pointer env;
+{
+ defun(ctx,"C-KDTREE-INIT", argv[0],C_KDTREE_INIT);
+ defun(ctx,"C-KDTREE-K-SEARCH", argv[0],C_KDTREE_K_SEARCH);
+ defun(ctx,"C-KDTREE-K-SEARCH-MATRIX", argv[0],C_KDTREE_K_SEARCH_MATRIX);
+ defun(ctx,"C-KDTREE-K-SEARCH-MATRIX-RAW", argv[0],C_KDTREE_K_SEARCH_MATRIX_RAW);
+ defun(ctx,"C-KDTREE-FR-SEARCH", argv[0],C_KDTREE_FR_SEARCH);
+ defun(ctx,"C-KDTREE-QUIT", argv[0], C_KDTREE_QUIT);
+ defun(ctx,"C-KDTREE-K-DISTANCE-POINTS", argv[0], C_KDTREE_SEARCH_AND_CALC_DISTANCE);
+}
+
+/*
+ * $Log$
+ * Revision 1.1  2010-02-16 11:09:35  k-okada
+ * add
+ *
+ * Revision 1.3  2010/02/16 11:08:11  k-okada
+ * add log
+ *
+ * Revision 1.2  2010/02/16 11:05:36  k-okada
+ * add log
+ *
+ * 1.2 add log
+ * 1.1 add kdtree.c, CANN.C
+ */
diff --git a/irteus/irtext.l b/irteus/irtext.l
index 2a7f28df9..2ed4918e6 100644
--- a/irteus/irtext.l
+++ b/irteus/irtext.l
@@ -28,7 +28,7 @@
   (load-library
    (format nil "~A~A/lib/libirteus"
 	   *eusdir* (unix:getenv "ARCHDIR"))
-   '("irtmath" "irtutil" "irtc" "irtgeoc" "irtgraph" "pgsql")))
+   '("irtmath" "irtutil" "irtc" "irtgeoc" "irtgraph" "pgsql" "ckdtree")))
 (defun load-irteusg ()
   (in-package "GEOMETRY")
   (load-library
diff --git a/irteus/irtgraph.l b/irteus/irtgraph.l
index 20872065b..0f30ae1db 100644
--- a/irteus/irtgraph.l
+++ b/irteus/irtgraph.l
@@ -565,6 +565,515 @@
     0.0)
   )
 
+#|
+(defclass kd-tree-node
+  :super node
+  :slots (parent children value axis))
+(defmethod kd-tree-node
+  (:init 
+   (val axs)
+   (setq value val axis axs)
+   (send-super :init value))
+  (:value () value)
+  (:axis () axis)
+  (:parent (&optional args) (if args (setq parent args)) parent)
+  (:children (&optional args) (if args (setq children args)) children)
+  )
+
+(defclass kd-tree
+  :super tree
+  :slots (key))
+
+(defmethod kd-tree
+  (:init
+   (plist &key ((:key k) #'identity))
+   (let ()
+     (setq key k)
+     (setq root (send self :build plist))
+     (send self :add-node root)
+     self))
+  (:select-sort-axis
+   (plist)
+   (let* ((axis 0)
+          (v (v- (apply #'vmax plist) (apply #'vmin plist)))
+          (m (elt v axis))
+          ;;(vi (car plist)) (va (car plist))
+          ;;(len (length plist))
+          ;;(p (car plist)) (pp (cdr plist)) m
+          )
+#|
+     (while pp
+       (setq vi (vmin p vi))
+       (setq va (vmax p va))
+       (setq p (car pp) pp (cdr pp)))
+     (setq v (v- va vi))
+     (setq m (elt v axis))
+|#
+     (dotimes (i (length v))
+       (if (> (elt v i) m) (setq m (elt v i) axis i)))
+     axis))
+  ;;
+  ;; build kdtree with recuresive methods (deperecated)
+  ;;
+  (:build
+   (plist)
+   (let ((len (length plist))
+         idx plist-short axis
+         med nd lnd rnd lbuf rbuf (skipp t) mm pp)
+     ;;
+     (if (> len 1000)
+         (setq plist-short (subseq plist 0 1000) idx 500)
+       (setq plist-short plist idx (/ len 2)))
+
+     (setq axis (send self :select-sort-axis (mapcar key plist-short)))
+     (setq plist-short (sort plist-short
+                             #'(lambda (x y) (<= (elt (funcall key x) axis)
+                                                 (elt (funcall key y) axis)))))
+     (setq med (elt plist-short idx))
+
+     (dolist (p plist)
+       (setq pp (funcall key p)
+             mm (funcall key med))
+       (cond ((and skipp (v= mm pp)) (setq skipp nil med p))
+             ((< (elt pp axis) (elt mm axis)) (push p lbuf))
+             ((>= (elt pp axis) (elt mm axis)) (push p rbuf))))
+
+     (setq nd (instance kd-tree-node :init med axis))
+     (when lbuf
+       (setq lnd (send self :build lbuf))
+       (send lnd :parent nd)
+       (send self :add-node lnd)
+       (send self :add-arc nd lnd))
+     (when rbuf
+       (setq rnd (send self :build rbuf))
+       (send rnd :parent nd)
+       (send self :add-node rnd)
+       (send self :add-arc nd rnd))
+     (send nd :children (list lnd rnd))
+     nd))
+  ;;
+  ;; build kdtree with non-recuresive methods (ALIVE)
+  ;;
+  (:build
+   (plist)
+   (let* (len plist-short axis idx med nd lnd rnd lbuf rbuf pp mm skipp
+          (build-seq (list (list plist nil t nil))) cur-build)
+     ;;
+     (setq cur-build build-seq)
+     (while cur-build
+       (setq plist (caar cur-build))
+       (setq len (length plist))
+       (if (> len 1000)
+           (setq plist-short (subseq plist 0 1000) idx 500)
+         (setq plist-short plist idx (/ len 2)))
+       (setq axis (send self :select-sort-axis (mapcar key plist-short)))
+       (setq plist-short (sort plist-short
+                               #'(lambda (x y) (<= (elt (funcall key x) axis)
+                                                   (elt (funcall key y) axis)))))
+       (setq med (elt plist-short idx))
+
+       (setq skipp t rbuf nil lbuf nil)
+       (dolist (p plist)
+         (setq pp (funcall key p)
+               mm (funcall key med))
+         (cond ((and skipp (v= mm pp)) (setq skipp nil med p))
+               ((< (elt pp axis) (elt mm axis)) (push p lbuf))
+               ((>= (elt pp axis) (elt mm axis)) (push p rbuf))))
+
+       (setq nd (instance kd-tree-node :init med axis))
+       (if (cdr build-seq) (send self :add-node nd))
+       (when (elt (car cur-build) 2)
+         (setf (elt (car cur-build) 2) nd))
+       (when (elt (car cur-build) 3)
+         (setf (elt (car cur-build) 3) nd))
+       ;;
+       (when lbuf
+         (nconc build-seq (list (list lbuf nd t nil))))
+       (when rbuf
+         (nconc build-seq (list (list rbuf nd nil t))))
+       (setq cur-build (cdr cur-build)))
+     (setq cur-build (reverse build-seq))
+
+     (while cur-build
+       (let ((nd (elt (car cur-build) 1))
+             (lnd (elt (car cur-build) 2))
+             (rnd (elt (car cur-build) 3)))
+         (unless nd (return lnd))
+         (when lnd
+           (send self :add-arc nd lnd)
+           (send lnd :parent nd)
+           (send nd :children (list lnd (cadr (send nd :children)))))
+         (when rnd
+           (send self :add-arc nd rnd)
+           (send rnd :parent nd)
+           (send nd :children (list (car (send nd :children)) rnd)))
+         (setq cur-build (cdr cur-build))
+         ))
+     ))
+  ;;
+  ;; search same node from kdtree (deperecated)
+  ;;
+  (:search (&rest args) 
+           (send (send* self :search-node args) :value))
+  (:search-node
+   (p &key ((:root rootnode) root) (debug) ((:epsilon eps) single-float-epsilon))
+   (let (value d axis children (pp (funcall key p)) vv
+               (nodeptr rootnode) (mindist 10000.0)
+               (minnode rootnode))
+     (if debug (warn ";; ~A << search target~%" p))
+     (while nodeptr
+       (setq value (send nodeptr :value)
+             axis  (send nodeptr :axis)
+             vv    (funcall key value))
+       (if debug (warn ";; ~A (~A)~%" value axis))
+       (if (< (setq d (distance pp vv)) mindist)
+           (setq mindist d minnode nodeptr))
+       (when (eps= mindist 0 eps)
+         (if debug (warn ";; ~A (~A) << result~%" (send minnode :value) mindist))
+         (return-from :search-node minnode))
+       (setq children (send nodeptr :children))
+       (cond ((< (elt pp axis) (elt vv axis))
+              (setq nodeptr (car children)))
+             (t
+              (setq nodeptr (cadr children)))))
+     (if debug (warn ";; ~A (~A) << result?~%" (send minnode :value) mindist))
+     minnode))
+  ;;
+  ;; search nearest neighbor node (ALIVE)
+  ;;
+  (:search (p &rest args) 
+           (send (send* self :search-nn-node root p args) :value))
+  (:search-nn-node
+   (nodeptr p &key (mindist 10000.0) ((:epsilon eps) single-float-epsilon)
+            (minnode nodeptr) debug)
+   (let (value axis pp vv d childptr)
+     (if debug (warn ";; ~A << search target~%" p))
+     (setq value (send nodeptr :value)
+           axis  (send nodeptr :axis)
+           pp    (funcall key p)
+           vv    (funcall key value))
+     (if debug (warn ";; ~A (~A)~%" value axis))
+     (when (< (setq d (distance pp vv)) mindist)
+       (setq mindist d minnode nodeptr)
+       (when (eps= mindist 0 eps)
+         (if debug (warn ";; ~A (~A) << result~%" (send minnode :value) mindist))
+         (return-from :search-nn-node minnode)))
+
+     (setq childptr (send nodeptr :children))
+     (when (and (car childptr)
+                (< (elt pp axis) (elt vv axis)))
+       (setq minnode (send self :search-nn-node (car childptr) p
+                           :mindist mindist :epsilon eps :minnode minnode
+                           :debug debug))
+       (setq mindist (min mindist
+                          (distance pp (funcall key (send minnode :value)))))
+       (when (and (cadr childptr)
+                  (< (- (elt vv axis) (elt pp axis)) mindist))
+         (setq minnode (send self :search-nn-node (cadr childptr) p
+                             :mindist mindist :epsilon eps :minnode minnode
+                             :debug debug))
+         (setq mindist (min mindist
+                            (distance pp (funcall key (send minnode :value)))))
+         ))
+     (when (and (cadr childptr)
+                (>= (elt pp axis) (elt vv axis)))
+       (setq minnode (send self :search-nn-node (cadr childptr) p
+                           :mindist mindist :epsilon eps :minnode minnode
+                           :debug debug))
+       (setq mindist (min mindist
+                          (distance pp (funcall key (send minnode :value)))))
+       (when (and (car childptr)
+                  (< (- (elt pp axis) (elt vv axis) ) mindist))
+         (setq minnode (send self :search-nn-node (car childptr) p
+                             :mindist mindist :epsilon eps :minnode minnode
+                             :debug debug))
+         (setq mindist (min mindist
+                            (distance pp (funcall key (send minnode :value)))))
+         ))
+     minnode))
+  )
+|#
+
+(defclass kd-tree
+  :super propertied-object
+  :slots (obj key return-type npoints points))
+(defmethod kd-tree
+  (:init
+    (plist &key ((:key k) #'identity))
+    (let (parray)
+      (sys::dispose-hook self t)
+      (setq key k)
+      (cond
+       ((arrayp plist)
+        (setq points plist npoints (array-dimension plist 0))
+        (setq parray plist return-type array))
+       ((listp plist)
+        (setq points plist npoints (length plist))
+        (let ((i 0))
+          (setq parray (make-matrix (length plist) (length (car plist)))
+                return-type cons)
+          (dolist (p plist)
+            (setf (matrix-row parray i) p)
+            (incf i))))
+       (t (warn "unsupported type ~A~%" plist)))
+      (setq obj (c-kdtree-init (array-entity parray)
+                               (array-dimension parray 0)
+                               (array-dimension parray 1)))
+      self))
+  ;;
+  (:points () points)
+  (:number-of-points () npoints)
+  (:c-search (type p
+              &key ((:epsilon eps) single-float-epsilon) (k 4) (radius) debug
+        return-index)
+    (let (ret idx-lst)
+      (case
+       type
+       (:search
+         (setq idx-lst (if return-index (make-array 1 :element-type :integer) 0))
+         (setq ret (c-kdtree-k-search obj p 1 eps idx-lst))
+         (return-from :c-search (if return-index (elt idx-lst 0) (matrix-row ret 0))))
+       (:nearest-neighbor
+         (setq idx-lst (if return-index (make-array k :element-type :integer) 0))
+         (setq ret (c-kdtree-k-search obj p k eps idx-lst)))
+       (:fixed-radius
+   (if return-index
+       (setq idx-lst (c-kdtree-fr-search obj p k radius eps 1))
+     (setq ret (c-kdtree-fr-search obj p k radius eps 0))))
+       (t (warn "unknown type ~A~%" type)))
+      (cond
+       (return-index (setq ret idx-lst))
+       ((and ret (eq return-type cons))
+  (let ((k (array-dimension ret 0)) r)
+          (dotimes (i k)
+            (push (matrix-row ret i) r))
+          (setq ret (nreverse r)))))
+      ret))
+  (:search (&rest args)
+    "search nearest neighbor (just one point)"
+    (send* self :c-search :search args))
+  (:nearest-neighbor (&rest args)
+    "search nearest k neighbors, use :k keyword"
+    (send* self :c-search :nearest-neighbor args))
+  (:fixed-radius (p &rest args &key (k 0) &allow-other-keys)
+    "search k points in fixed radius from p, if k=0 search all points in fixed radius"
+    (send* self :c-search :fixed-radius p :k k args))
+  (:dispose () (if obj (c-kdtree-quit obj)))
+  (:quit () (if obj (prog1 (c-kdtree-quit obj) (setq obj nil))))
+  (:workingp () obj)
+  (:search-corresponding-index (mat &key (idx) (dist) (k 1) ((:epsilon eps) single-float-epsilon))
+   (let* ((ret-idx (if idx idx (make-array (* k (array-dimension mat 0)) :element-type :integer)))
+          (dlst (if dist dist 0)))
+     (c-kdtree-k-search-matrix obj mat k eps ret-idx dlst)
+     ret-idx))
+  )
+
+(defclass dynamic-kdtree
+  :super propertied-object
+  :slots (n trees))
+(defmethod dynamic-kdtree
+  (:init
+   ()
+   (setq n 12)
+   (setq trees (make-array n))
+   )
+
+  (:insert
+   (pt)
+   (let (j)
+     (block loop
+       (dotimes (i n)
+   (when (null (aref trees i))
+     (setq j i)
+     (return-from loop))
+   )
+
+       ;; expand arrays if necessary
+       (warn "expand the table of kd trees~%")
+       (setq j n)
+       (setq n (* 2 n))
+       (let ((newtrees (make-array (* 2 n))))
+   (dotimes (i (length trees))
+     (aset newtrees i (aref trees i)))
+   (setq trees newtrees))
+       )
+
+     ;; collect points in the smaller trees
+     ;; clean points in the trees
+     (let (pts)
+       (dotimes (i j)
+   ;; (format t "pts[~a]: ~a~%" i (send (aref trees i) :points))
+   (setq pts (append (send (aref trees i) :points) pts))
+   (sys::dispose-hook (aref trees i) nil)
+   )
+       ;; (format t "new pts[~a]: ~a~%" j (cons pt pts))
+       (aset trees j (instance kd-tree :init (cons pt pts)))
+       )
+     (dotimes (i j) (aset trees i nil))
+     )
+   )
+
+  (:npoints
+   ()
+   (let ((np 0))
+     (dotimes (i n)
+       (when (aref trees i)
+   (setq np (+ np (send (aref trees i) :number-of-points)))))
+     np)
+   )
+
+  (:nearest-neighbor
+   (pt)
+   (let ((mind most-positive-float)
+   nnp)
+     (dotimes (i n)
+       (if (aref trees i)
+     (let* ((nnp-candidate
+       (car (send (aref trees i) :nearest-neighbor pt :k 1)))
+      (d (distance nnp-candidate pt)))
+       (when (< d mind)
+         (setq mind d nnp nnp-candidate))
+       )))
+     (list mind nnp))
+   )
+
+  )
+
+#|
+;;(setq k (instance kd-tree :init
+;                  (list #f(2 3) #f(5 3) #f(8 3) #f(3 4) #f(3 4) #f(3 4) #f(6 2))))
+;;(print (send k :search #f(5 3)))
+
+(defun bench-kd-tree-1 (&optional (pnum 1000))
+  (let (tm points ret)
+    (setq *random-state* (coerce (unix::gettimeofday) integer-vector))
+    (setq tm (instance mtimer :init))
+    (dotimes (i pnum)
+      (setq pp (float-vector (- 0.0 (random 100.0))
+                             (- 0.0 (random 100.0))
+                             (- 0.0 (random 0.0))))
+      (push pp points))
+    (let (dist minp (mindist 1000))
+      (dolist (p points)
+        (setq dist (distance p #f(0 0 0)))
+        (when (< dist mindist) (setq mindist dist minp p)))
+      (print (list 'full-search minp minp)))
+    ;;
+    (send tm :start)
+    (setq k (instance kd-tree :init points))
+    (print (list 'tree-creationg (send tm :stop)))
+    (send tm :start)
+    (setq ret (send k :search #f(0 0 0)))
+    (print (list 'tree-retrieving (send tm :stop)))
+    (print ret)
+    ))
+;(bench-kd-tree-1)
+
+(defun bench-kd-tree-2 (&key (node-num 1000) ;; up to 50000
+                     (data-size 5)
+                     (debug))
+  (let (points data-vec search-vector
+               (tm (instance mtimer :init)))
+    (format t "node-num: ~a node-num: ~a~%" node-num data-size)
+    (dotimes (i node-num)
+      (setq data-vec (instantiate float-vector data-size))
+      (dotimes (j data-size)
+        (setf (elt data-vec j) (random 100.0)))
+      (setf (elt data-vec 0) 0)
+      (push data-vec points))
+    (setq search-vector (elt points (random node-num)))
+    ;;(setq search-vector (instantiate float-vector data-size))
+    (send tm :start)
+    (setq k (instance kd-tree :init points))
+    (print (list 'tree-creationg (send tm :stop)))
+    (send tm :start)
+    (setq ret (send k :search search-vector :debug debug))
+    (print (list 'tree-retrieving (send tm :stop)))
+    (print (list 'ret ret))
+    (print (list 'ans search-vector))
+  ))
+ 
+;(bench-kd-tree-2) 
+;;(bench-kd-tree-2 :node-num 10000 :data-size 100 :debug t)
+(bench-kd-tree-2 :node-num 10 :debug t)
+;(send k :write-to-pdf "kd-tree.pdf")
+
+(setq *random-state* (integer-vector (elt (unix::gettimeofday) 0) (elt (unix::gettimeofday) 1)))
+(setq *tm* (instance mtimer :init))
+
+(defun kdt-test (&key (iteration 1) ((:size ss) 100) (eps single-float-epsilon))
+  (setq smat (make-matrix ss 3))
+  (setq dmat (make-matrix ss 3))
+
+  (setq dlst0 (instantiate float-vector ss))
+  (setq ilst0 (instantiate integer-vector ss))
+  (setq dlst1 (instantiate float-vector ss))
+  (setq ilst1 (instantiate integer-vector ss))
+
+  (setq ret nil)
+
+  (dotimes (i iteration)
+    (let (tmp)
+      (dotimes (i ss)
+        (setf (matrix-row smat i)
+              (scale 500 (random-vector)))
+        (setf (matrix-row dmat i)
+              (scale 500 (random-vector))))
+
+      (send *tm* :start)
+      (setq kdt (instance kd-tree :init dmat))
+      (push (* 1000 (send *tm* :stop)) tmp)
+
+      (send *tm* :start)
+      (send kdt :search-corresponding-index smat :idx ilst0 :dist dlst0 :epsilon eps)
+      (push (* 1000 (send *tm* :stop)) tmp)
+
+      (send *tm* :start)
+      (c-kdtree-k-search-matrix-raw dmat smat 1 single-float-epsilon ilst1 dlst1)
+      (push (* 1000 (send *tm* :stop)) tmp)
+
+      ;;check
+      (dotimes (i ss)
+        (unless (= (elt ilst0 i) (elt ilst1 i))
+          (warn "error : ~A != ~A / ~F != ~F (~F)~%"
+                (elt ilst0 i) (elt ilst1 i) (elt dlst0 i) (elt dlst1 i)
+                (- (elt dlst0 i) (elt dlst1 i)))
+          ))
+      (push (coerce (nreverse tmp) float-vector) ret)
+      (sys::gc)
+      ))
+  (vector-mean ret))
+;; time check
+(dotimes (i 21)
+  (let ((ss (round (expt 10 (/ (+ i 5) 5.0))))
+        itr)
+    (setq itr (min 300 (/ 300000 ss)))
+    (pprint (list ss itr (kdt-test :iteration itr :size ss)))))
+
+;; Z800 3.2GHz
+ point num, iteration, #f(kdtree-initialize kdtree-search all-search)
+(10 300 #f(0.020607 0.01184 0.00527))
+(16 300 #f(0.023363 0.01502 0.005663))
+(25 300 #f(0.02536 0.0196 0.007447))
+(40 300 #f(0.02975 0.02929 0.011687))
+(63 300 #f(0.04038 0.045263 0.02073))
+(100 300 #f(0.052463 0.071803 0.04253))
+(158 300 #f(0.07182 0.114343 0.094477))
+(251 300 #f(0.10217 0.18909 0.2217))
+(398 300 #f(0.152513 0.312573 0.531877))
+(631 300 #f(0.236863 0.517203 1.30164))
+(1000 300 #f(0.375293 0.85893 3.20897))
+(1585 189 #f(0.608921 1.41394 8.21947))
+(2512 119 #f(1.00201 2.44978 20.4663))
+(3981 75 #f(1.65612 4.295 51.1048))
+(6310 47 #f(2.71764 7.35921 127.994))
+(10000 30 #f(4.5811 12.3736 323.977))
+(15849 18 #f(7.55239 20.7259 817.415))
+(25119 11 #f(12.2907 34.6122 2069.27))
+(39811 7 #f(20.4027 58.1197 5185.1))
+(63096 4 #f(33.9372 109.535 13027.1))
+(100000 3 #f(56.9617 222.269 32723.4))
+|#
 
 ;;=======================================
 ;; samples