diff --git a/MeshModel/src/main/java/cz/fidentis/analyst/mesh/KdTree/KdTreeImpl.java b/MeshModel/src/main/java/cz/fidentis/analyst/mesh/KdTree/KdTreeImpl.java
index d287b1d31f5a63dfadf48ac3f65e1f4fd28e60f3..732c576038b050fcb81db9805df5f05e4eae6cb6 100644
--- a/MeshModel/src/main/java/cz/fidentis/analyst/mesh/KdTree/KdTreeImpl.java
+++ b/MeshModel/src/main/java/cz/fidentis/analyst/mesh/KdTree/KdTreeImpl.java
@@ -8,11 +8,14 @@ import java.util.*;
 
 public class KdTreeImpl {
     private final KdNode root;
+    private  List<MeshPoint> points;
 
     private final static int X_AXIS = 0;
     private final static int Y_AXIS = 1;
     private final static int Z_AXIS = 2;
 
+
+/*
     private static class Pair {
         private MeshPoint p;
         private Integer index;
@@ -29,7 +32,7 @@ public class KdTreeImpl {
         public Integer getIndex() {
             return index;
         }
-    }
+    }*/
 
 /**
     private static final Comparator<Pair> X_COMPARATOR = (o1, o2) -> {
@@ -57,23 +60,36 @@ public class KdTreeImpl {
     };**/
 
 
+
+    private final Comparator<Integer> XIndex_COMPARATOR = Comparator.comparingDouble(i -> this.points.get(i).getPosition().x);
+
+    private final Comparator<Integer> YIndex_COMPARATOR = Comparator.comparingDouble(i -> this.points.get(i).getPosition().y);
+
+    private final Comparator<Integer> ZIndex_COMPARATOR = Comparator.comparingDouble(i -> this.points.get(i).getPosition().z);
+
+/*
     private static final Comparator<Pair> X_COMPARATOR = Comparator.comparingDouble(o -> o.getP().getPosition().x);
 
     private static final Comparator<Pair> Y_COMPARATOR = Comparator.comparingDouble(o -> o.getP().getPosition().y);
 
     private static final Comparator<Pair> Z_COMPARATOR = Comparator.comparingDouble(o -> o.getP().getPosition().z);
+*/
+
 
     public KdTreeImpl(List<MeshPoint> points) {
         if(points == null || points.isEmpty()){
             this.root = null;
+            this.points = null;
             return;
         }
 
-        List<Integer> sortedByX = sortPoints( X_AXIS, points);
-        List<Integer> sortedByY = sortPoints( Y_AXIS, points);
-        List<Integer> sortedByZ = sortPoints( Z_AXIS, points);
+        this.points = points;
+
+        List<Integer> sortedByX = sortPoints( X_AXIS/*, points*/);
+        List<Integer> sortedByY = sortPoints( Y_AXIS/*, points*/);
+        List<Integer> sortedByZ = sortPoints( Z_AXIS/*, points*/);
 
-        root = buildTree(null, sortedByX, sortedByY, sortedByZ, 0, points);
+        root = buildTree(null, sortedByX, sortedByY, sortedByZ, 0/*, points*/);
 
     }
 
@@ -90,9 +106,13 @@ public class KdTreeImpl {
     }
 
 
-    private List<Integer> sortPoints(/*List<Integer> points,*/ int level, List<MeshPoint> p){
-        List<Integer> sortedListIndex = new ArrayList<>(p.size());
-        List<Pair> sortedListPairs = new ArrayList<>(p.size());
+    private List<Integer> sortPoints(/*List<Integer> points,*/ int level /*List<MeshPoint> p*/){
+        List<Integer> sortedListIndex = new ArrayList<>(this.points.size());
+        for(int i = 0; i < points.size(); i++){
+            sortedListIndex.add(i);
+        }
+
+        /*List<Pair> sortedListPairs = new ArrayList<>(p.size());
 
         for (int i = 0; i < p.size(); i++ ) {
             sortedListPairs.add(new Pair(p.get(i), i));
@@ -109,12 +129,21 @@ public class KdTreeImpl {
         for(Pair pair : sortedListPairs) {
             sortedListIndex.add(pair.getIndex());
         }
+        return sortedListIndex;*/
+
+        if(level % 3 == 0){
+            sortedListIndex.sort(XIndex_COMPARATOR);
+        }else if(level % 3 == 1){
+            sortedListIndex.sort(YIndex_COMPARATOR);
+        }else if(level % 3 == 2){
+            sortedListIndex.sort(ZIndex_COMPARATOR);
+        }
         return sortedListIndex;
 
     }
 
 
-    private KdNode buildTree(KdNode parent, List<Integer> byX, List<Integer> byY, List<Integer> byZ, int level, List<MeshPoint> points) {
+    private KdNode buildTree(KdNode parent, List<Integer> byX, List<Integer> byY, List<Integer> byZ, int level/*, List<MeshPoint> points*/) {
         KdNode node = null;
         int mid;
         int midIndex;
@@ -131,19 +160,19 @@ public class KdTreeImpl {
             //but also keep the ordering by their axis
             if (level % 3 == 0){
                 midIndex = byX.get(mid);
-                node = new KdNode(midIndex, level, points.get(byX.get(mid)), parent);
+                node = new KdNode(midIndex, level, this.points.get(byX.get(mid)), parent);
 
                 splitTree(mid, leftX, byX, leftY, byY, leftZ, byZ);
 
             }else if(level % 3 == 1){
                 midIndex = byY.get(mid);
-                node = new KdNode(midIndex, level, points.get(byY.get(mid)), parent);
+                node = new KdNode(midIndex, level, this.points.get(byY.get(mid)), parent);
 
                 splitTree(mid, leftY, byY, leftX, byX, leftZ, byZ);
 
             }else{
                 midIndex = byZ.get(mid);
-                node = new KdNode(midIndex, level, points.get(byZ.get(mid)), parent);
+                node = new KdNode(midIndex, level, this.points.get(byZ.get(mid)), parent);
 
                 splitTree(mid, leftZ, byZ, leftY, byY, leftX, byX);
 
@@ -157,8 +186,8 @@ public class KdTreeImpl {
             leftY.removeAll(Collections.singleton(midIndex));
             leftZ.removeAll(Collections.singleton(midIndex));
 
-            node.setLesser(buildTree(node, leftX, leftY, leftZ, level + 1, points));
-            node.setGreater(buildTree(node, byX, byY, byZ, level + 1, points));
+            node.setLesser(buildTree(node, leftX, leftY, leftZ, level + 1/*, points*/));
+            node.setGreater(buildTree(node, byX, byY, byZ, level + 1/*, points*/));
         }
 
         return node;