backported kd tree improvements from monav project: faster with base case 8

DataStructures/StaticKDTree.h

@@ -34,6 +34,8 @@ KD Tree coded by Christian Vetter, Monav Project
 
 namespace KDTree {
 
+#define KDTREE_BASESIZE (8)
+
 template< unsigned k, typename T >
 class BoundingBox {
 public:
@@ -83,6 +85,14 @@ public:
     struct InputPoint {
         T coordinates[k];
         Data data;
+        bool operator==( const InputPoint& right )
+		                {
+            for ( int i = 0; i < k; i++ ) {
+                if ( coordinates[i] != right.coordinates[i] )
+                    return false;
+            }
+            return true;
+		                }
     };
 
     StaticKDTree( std::vector< InputPoint > * points ){
@@ -105,7 +115,7 @@ public:
             Tree tree = s.top();
             s.pop();
 
-			if ( tree.left == tree.right )
+            if ( tree.right - tree.left < KDTREE_BASESIZE )
                 continue;
 
             Iterator middle = tree.left + ( tree.right - tree.left ) / 2;
@@ -136,8 +146,17 @@ public:
             if ( distance( tree.box, point.coordinates ) >= nearestDistance )
                 continue;
 
-			if ( tree.left == tree.right )
+            if ( tree.right - tree.left < KDTREE_BASESIZE ) {
+                for ( unsigned i = tree.left; i < tree.right; i++ ) {
+                    double newDistance = distance( kdtree[i].coordinates, point.coordinates );
+                    if ( newDistance < nearestDistance ) {
+                        nearestDistance = newDistance;
+                        *result = kdtree[i];
+                        found = true;
+                    }
+                }
                 continue;
+            }
 
             Iterator middle = tree.left + ( tree.right - tree.left ) / 2;
 
@@ -166,7 +185,6 @@ public:
                 s.push( second );
             }
         }
-
         return found;
     }