us.ihmc.simulationconstructionset.util.KDTree Maven / Gradle / Ivy

Go to download
Show more of this group Show more artifacts with this name
Show all versions of simulation-construction-set
Simulation Construction Set
There is a newer version: 0.25.2
package us.ihmc.simulationconstructionset.util;

import java.io.BufferedReader;
import java.io.FileReader;
import java.io.IOException;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.LinkedHashSet;
import java.util.Random;
import java.util.StringTokenizer;

import us.ihmc.euclid.tuple3D.Point3D;

/**
 * Use this class to efficiently search for closest points to various test points of interest. The
 * dimensionality of the points is arbitrary (D > 0), and the tree's splitting is binary (K = 2).
 */
public class KDTree
{
   // Allow some member data changes in case of point pruning.
   private final boolean hasObjects;
   private final int maxPointsInLeaves;
   private final Object[] objects;
   private final KDNode root;
   private LinkedHashSet novelPointIndexes;
   private static final boolean DEBUG = false;

   /**
    * Creates a KDTree from an array of points and an equally sized array of objects. The value
    * MaxPointsInLeaves specifies the maximum number of points in a leaf Node. Use a small value (5-20)
    * unless building the tree takes too long.
    *
    * @param points            double[][]
    * @param objects           Object[]
    * @param maxPointsInLeaves int
    */
   public KDTree(double[][] points, Object[] objects, int maxPointsInLeaves)
   {
      if (points == objects)
      {
         throw new RuntimeException("Do not use the same keys and values in constructing a KDTree. If they are the same, then just use the key constructor");
      }

      makeAllPointsNovel(points.length);

      hasObjects = true;
      this.objects = objects;
      this.maxPointsInLeaves = maxPointsInLeaves;

      if (points.length != objects.length)
      {
         System.err.println("KDTree::KDTree(): number of points and objects differ.  Creating empty KDTree.");
         root = null;
      }
      else
      {
         root = new KDNode(points, 0, points.length - 1, maxPointsInLeaves);
      }
   }

   /**
    * Creates a KDTree from an array of points and an equally sized array of objects. The value
    * MaxPointsInLeaves specifies the maximum number of points in a leaf Node. Use a small value (5-20)
    * unless building the tree takes too long.
    *
    * @param points            double[][]
    * @param maxPointsInLeaves int
    */
   public KDTree(double[][] points, int maxPointsInLeaves)
   {
      hasObjects = false;
      objects = null;
      this.maxPointsInLeaves = maxPointsInLeaves;
      makeAllPointsNovel(points.length);
      root = new KDNode(points, 0, points.length - 1, maxPointsInLeaves);
   }

   /**
    * Creates a KDTree from an array of (X, Y) terrain points and an equally sized array of (Z) terrain
    * heights. The value MaxPointsInLeaves specifies the maximum number of points in a leaf Node. Use a
    * small value (5-20) unless building the tree takes too long.
    *
    * @param points            double[][]
    * @param maxPointsInLeaves int
    */
   public KDTree(String BDITerrainFilePath, int maxPointsInLeaves)
   {
      hasObjects = false;
      objects = null;
      this.maxPointsInLeaves = maxPointsInLeaves;
      double[][] points = loadPoints3D(BDITerrainFilePath);
      makeAllPointsNovel(points.length);
      root = new KDNode(points, 0, points.length - 1, maxPointsInLeaves);
   }

   private void makeAllPointsNovel(int numPoints)
   {
      novelPointIndexes = new LinkedHashSet<>();

      for (int i = 0; i < numPoints; i++)
      {
         novelPointIndexes.add(i);
      }
   }

   /**
    * Returns the list of points. The user may not have this in advance if the points are loaded from a
    * file.
    *
    * @return double[][]
    */
   public double[][] getPoints()
   {
      return root.points;
   }

   public void getExtents(double[] min, double[] max)
   {
      root.getExtents(min, max);
   }

   // // Delete points in the KDTree such that the remaining
   // // points are no closer than minDistanceBetweenPoints from each other.
   // // The deleted points cannot be restored once this call is made.
   // public void prunePoints(double minDistanceBetweenPoints)
   // {
   //     int numPoints = root.points.length;
   //     int numDimensions = root.points[0].length;
   //
   //     // Track which points are not in the covering set.
   //     // Start with all points in the cover and remove
   //     // points incrementally.
   //     HashSet inCover = new LinkedHashSet();
   //     for (int i=0; i coverIndexes = new ArrayList();
   //
   //     // Loop over all points.  For any point still in the
   //     // cover, find all other points nearby and remove them
   //     // from the cover.
   //     for (int i=0; i coveredIndexes = closestPointIndexesInSubset(root.points[i], minDistanceBetweenPoints, inCover);
   //             inCover.removeAll(coveredIndexes);
   //         }
   //     }
   //
   //     // Collect points in the cover and rebuild the tree with these points only.
   //     // If Objects are associated with points, we collect the objects also.
   //     int numPointsInCover = coverIndexes.size();
   //     double[][] newPoints = new double[numPointsInCover][numDimensions];
   //     if (!hasObjects)
   //     {
   //         for (int i=0; i inCover = novelPointIndexes;

      ArrayList coverIndexes = new ArrayList<>();

      // Loop over all points.  For any point still in the
      // cover, find all other points nearby and remove them
      // from the cover.
      for (int i = 0; i < numPoints; i++)
      {
         if (inCover.contains(i))
         {
            coverIndexes.add(i);
            @SuppressWarnings("unused")
            ArrayList coveredIndexes = getClosestNovelPointIndexes(root.points[i], minDistanceBetweenPoints, false);
            if (DEBUG)
            {
               System.err.println("Found cover point; number of novel points remaining: " + inCover.size());
            }

            // inCover.removeAll(coveredIndexes);
         }
      }

      // Collect points in the cover and rebuild the tree with these points only.
      // If Objects are associated with points, we collect the objects also.
      int numPointsInCover = coverIndexes.size();
      double[][] newPoints = new double[numPointsInCover][numDimensions];
      if (!hasObjects)
      {
         for (int i = 0; i < numPointsInCover; i++)
         {
            newPoints[i] = root.points[coverIndexes.get(i)];
         }

         //       this.root = new KDNode(newPoints, 0, newPoints.length - 1, maxPointsInLeaves);

         return new KDTree(newPoints, maxPointsInLeaves);
      }
      else
      {
         Object[] newObjects = new Object[numPointsInCover];
         for (int i = 0; i < numPointsInCover; i++)
         {
            newPoints[i] = root.points[coverIndexes.get(i)];
            newObjects[i] = objects[coverIndexes.get(i)];
         }

         return new KDTree(newPoints, newObjects, maxPointsInLeaves);

         //       this.objects = newObjects;
         //       this.root = new KDNode(newPoints, 0, newPoints.length - 1, maxPointsInLeaves);
      }
   }

   /**
    * closestPoint
    *
    * @param testPoint double[]
    * @return double[]
    */
   public double[] closestPoint(double[] testPoint)
   {
      return root.closestPoint(testPoint);
   }

   public double[] closestPoint(double[] testPoint, double maxDistance)
   {
      return root.closestPoint(testPoint, maxDistance * maxDistance);
   }

   public double[] closestPointBruteForceSearch(double[] testPoint)
   {
      return root.closestPointBruteForceSearch(testPoint);
   }

   public Object closestObject(double[] testPoint)
   {
      if (!hasObjects)
      {
         System.err.println("KDTree::closestObject(): no objects exist. Returning null.");

         return null;
      }

      int closestIndex = root.closestPointIndex(testPoint);

      return objects[closestIndex];
   }

   public Object closestObject(double[] testPoint, double maxDistance)
   {
      if (!hasObjects)
      {
         System.err.println("KDTree::closestObject(): no objects exist. Returning null.");

         return null;
      }

      int closestIndex = root.closestPointIndex(testPoint, maxDistance * maxDistance, null, null);
      if (closestIndex == -1)
         return null;

      return objects[closestIndex];
   }

   public ArrayList