bundles/org.simantics.scenegraph/src/com/infomatiq/jsi/rtree/RTree.java

   1 //   RTree.java\r
   2 //   Java Spatial Index Library\r
   3 //   Copyright (C) 2002-2005 Infomatiq Limited\r
   4 //   Copyright (C) 2008-2010 aled@sourceforge.net\r
   5 //  \r
   6 //  This library is free software; you can redistribute it and/or\r
   7 //  modify it under the terms of the GNU Lesser General Public\r
   8 //  License as published by the Free Software Foundation; either\r
   9 //  version 2.1 of the License, or (at your option) any later version.\r
  10 //  \r
  11 //  This library is distributed in the hope that it will be useful,\r
  12 //  but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  13 //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU\r
  14 //  Lesser General Public License for more details.\r
  15 //  \r
  16 //  You should have received a copy of the GNU Lesser General Public\r
  17 //  License along with this library; if not, write to the Free Software\r
  18 //  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA\r
  19 \r
  20 package com.infomatiq.jsi.rtree;\r
  21 \r
  22 import gnu.trove.TIntArrayList;\r
  23 import gnu.trove.TIntObjectHashMap;\r
  24 import gnu.trove.TIntProcedure;\r
  25 import gnu.trove.TIntStack;\r
  26 \r
  27 import java.util.Properties;\r
  28 \r
  29 import com.infomatiq.jsi.Point;\r
  30 import com.infomatiq.jsi.Rectangle;\r
  31 import com.infomatiq.jsi.PriorityQueue;\r
  32 import com.infomatiq.jsi.SpatialIndex;\r
  33 \r
  34 /**\r
  35  * Stub replacement for org.apache.log4j.Logger to prevent dependency on log4j.\r
  36  * \r
  37  * @author Tuukka Lehtonen\r
  38  */\r
  39 class Logger {\r
  40     String name;\r
  41 \r
  42     public Logger(String name) {\r
  43         this.name = name;\r
  44     }\r
  45 \r
  46     public static Logger getLogger(String name) {\r
  47         return new Logger(name);\r
  48     }\r
  49 \r
  50     public void warn(String string) {\r
  51         //System.out.println(name + ": WARN " + string);\r
  52     }\r
  53 \r
  54     public boolean isDebugEnabled() {\r
  55         return false;\r
  56     }\r
  57 \r
  58     public void debug(String string) {\r
  59         //System.out.println(name + ": DEBUG " + string);\r
  60     }\r
  61 \r
  62     public void error(String string) {\r
  63         System.out.println(name + ": ERROR " + string);\r
  64     }\r
  65 }\r
  66 \r
  67 /**\r
  68  * <p>This is a lightweight RTree implementation, specifically designed \r
  69  * for the following features (in order of importance): \r
  70  * <ul>\r
  71  * <li>Fast intersection query performance. To achieve this, the RTree \r
  72  * uses only main memory to store entries. Obviously this will only improve\r
  73  * performance if there is enough physical memory to avoid paging.</li>\r
  74  * <li>Low memory requirements.</li>\r
  75  * <li>Fast add performance.</li>\r
  76  * </ul></p> \r
  77  * \r
  78  * <p>The main reason for the high speed of this RTree implementation is the \r
  79  * avoidance of the creation of unnecessary objects, mainly achieved by using\r
  80  * primitive collections from the trove4j library.</p>\r
  81  * \r
  82  * @author aled@sourceforge.net\r
  83  * @version 1.0b8\r
  84  */\r
  85 public class RTree implements SpatialIndex {\r
  86   private static final Logger log = Logger.getLogger(RTree.class.getName());\r
  87   private static final Logger deleteLog = Logger.getLogger(RTree.class.getName() + "-delete");\r
  88   \r
  89   private static final String version = "1.0b8";\r
  90   \r
  91   // parameters of the tree\r
  92   private final static int DEFAULT_MAX_NODE_ENTRIES = 10;\r
  93   int maxNodeEntries;\r
  94   int minNodeEntries;\r
  95   \r
  96   // map of nodeId -> node object\r
  97   // TODO eliminate this map - it should not be needed. Nodes\r
  98   // can be found by traversing the tree.\r
  99   private TIntObjectHashMap<Node> nodeMap = new TIntObjectHashMap<Node>();\r
 100   \r
 101   // internal consistency checking - set to true if debugging tree corruption\r
 102   private final static boolean INTERNAL_CONSISTENCY_CHECKING = false;\r
 103   \r
 104   // used to mark the status of entries during a node split\r
 105   private final static int ENTRY_STATUS_ASSIGNED = 0;\r
 106   private final static int ENTRY_STATUS_UNASSIGNED = 1; \r
 107   private byte[] entryStatus = null;\r
 108   private byte[] initialEntryStatus = null;\r
 109   \r
 110   // stacks used to store nodeId and entry index of each node \r
 111   // from the root down to the leaf. Enables fast lookup\r
 112   // of nodes when a split is propagated up the tree.\r
 113   private TIntStack parents = new TIntStack();\r
 114   private TIntStack parentsEntry = new TIntStack();\r
 115   \r
 116   // initialisation\r
 117   private int treeHeight = 1; // leaves are always level 1\r
 118   private int rootNodeId = 0;\r
 119   private int size = 0;\r
 120   \r
 121   // Enables creation of new nodes\r
 122   private int highestUsedNodeId = rootNodeId; \r
 123   \r
 124   // Deleted node objects are retained in the nodeMap, \r
 125   // so that they can be reused. Store the IDs of nodes\r
 126   // which can be reused.\r
 127   private TIntStack deletedNodeIds = new TIntStack();\r
 128   \r
 129   // List of nearest rectangles. Use a member variable to\r
 130   // avoid recreating the object each time nearest() is called.\r
 131   private TIntArrayList nearestIds = new TIntArrayList();\r
 132   private TIntArrayList savedValues = new TIntArrayList();\r
 133   private float savedPriority = 0;\r
 134 \r
 135   // List of nearestN rectangles\r
 136   private SortedList nearestNIds = new SortedList();\r
 137   \r
 138   // List of nearestN rectanges, used in the alternative nearestN implementation.\r
 139   private PriorityQueue distanceQueue = \r
 140     new PriorityQueue(PriorityQueue.SORT_ORDER_ASCENDING);\r
 141   \r
 142   /**\r
 143    * Constructor. Use init() method to initialize parameters of the RTree.\r
 144    */\r
 145   public RTree() {  \r
 146     return; // NOP    \r
 147   }\r
 148   \r
 149   //-------------------------------------------------------------------------\r
 150   // public implementation of SpatialIndex interface:\r
 151   //  init(Properties)\r
 152   //  add(Rectangle, int)\r
 153   //  delete(Rectangle, int)\r
 154   //  nearest(Point, TIntProcedure, float)\r
 155   //  intersects(Rectangle, TIntProcedure)\r
 156   //  contains(Rectangle, TIntProcedure)\r
 157   //  size()\r
 158   //-------------------------------------------------------------------------\r
 159   /**\r
 160    * <p>Initialize implementation dependent properties of the RTree.\r
 161    * Currently implemented properties are:\r
 162    * <ul>\r
 163    * <li>MaxNodeEntries</li> This specifies the maximum number of entries\r
 164    * in a node. The default value is 10, which is used if the property is\r
 165    * not specified, or is less than 2.\r
 166    * <li>MinNodeEntries</li> This specifies the minimum number of entries\r
 167    * in a node. The default value is half of the MaxNodeEntries value (rounded\r
 168    * down), which is used if the property is not specified or is less than 1.\r
 169    * </ul></p>\r
 170    * \r
 171    * @see com.infomatiq.jsi.SpatialIndex#init(Properties)\r
 172    */\r
 173   public void init(Properties props) {\r
 174     if (props == null) {\r
 175       // use sensible defaults if null is passed in.\r
 176       maxNodeEntries = 50;\r
 177       minNodeEntries = 20;\r
 178     } else {\r
 179       maxNodeEntries = Integer.parseInt(props.getProperty("MaxNodeEntries", "0"));\r
 180       minNodeEntries = Integer.parseInt(props.getProperty("MinNodeEntries", "0"));\r
 181       \r
 182       // Obviously a node with less than 2 entries cannot be split.\r
 183       // The node splitting algorithm will work with only 2 entries\r
 184       // per node, but will be inefficient.\r
 185       if (maxNodeEntries < 2) { \r
 186         log.warn("Invalid MaxNodeEntries = " + maxNodeEntries + " Resetting to default value of " + DEFAULT_MAX_NODE_ENTRIES);\r
 187         maxNodeEntries = DEFAULT_MAX_NODE_ENTRIES;\r
 188       }\r
 189       \r
 190       // The MinNodeEntries must be less than or equal to (int) (MaxNodeEntries / 2)\r
 191       if (minNodeEntries < 1 || minNodeEntries > maxNodeEntries / 2) {\r
 192         log.warn("MinNodeEntries must be between 1 and MaxNodeEntries / 2");\r
 193         minNodeEntries = maxNodeEntries / 2;\r
 194       }\r
 195     }\r
 196     \r
 197     entryStatus = new byte[maxNodeEntries];  \r
 198     initialEntryStatus = new byte[maxNodeEntries];\r
 199     \r
 200     for (int i = 0; i < maxNodeEntries; i++) {\r
 201       initialEntryStatus[i] = ENTRY_STATUS_UNASSIGNED;\r
 202     }\r
 203     \r
 204     Node root = new Node(rootNodeId, 1, maxNodeEntries);\r
 205     nodeMap.put(rootNodeId, root);\r
 206     \r
 207     log.debug("init() " + " MaxNodeEntries = " + maxNodeEntries + ", MinNodeEntries = " + minNodeEntries);\r
 208   }\r
 209   \r
 210   /**\r
 211    * @see com.infomatiq.jsi.SpatialIndex#add(Rectangle, int)\r
 212    */\r
 213   public void add(Rectangle r, int id) {\r
 214     if (log.isDebugEnabled()) {\r
 215       log.debug("Adding rectangle " + r + ", id " + id);\r
 216     }\r
 217     \r
 218     add(r.minX, r.minY, r.maxX, r.maxY, id, 1); \r
 219     \r
 220     size++;\r
 221     \r
 222     if (INTERNAL_CONSISTENCY_CHECKING) {\r
 223       checkConsistency();\r
 224     }\r
 225   }\r
 226   \r
 227   /**\r
 228    * Adds a new entry at a specified level in the tree\r
 229    */\r
 230   private void add(float minX, float minY, float maxX, float maxY, int id, int level) {\r
 231     // I1 [Find position for new record] Invoke ChooseLeaf to select a \r
 232     // leaf node L in which to place r\r
 233     Node n = chooseNode(minX, minY, maxX, maxY, level);\r
 234     Node newLeaf = null;\r
 235     \r
 236     // I2 [Add record to leaf node] If L has room for another entry, \r
 237     // install E. Otherwise invoke SplitNode to obtain L and LL containing\r
 238     // E and all the old entries of L\r
 239     if (n.entryCount < maxNodeEntries) {\r
 240       n.addEntry(minX, minY, maxX, maxY, id);\r
 241     } else {\r
 242       newLeaf = splitNode(n, minX, minY, maxX, maxY, id);  \r
 243     }\r
 244     \r
 245     // I3 [Propagate changes upwards] Invoke AdjustTree on L, also passing LL\r
 246     // if a split was performed\r
 247     Node newNode = adjustTree(n, newLeaf); \r
 248 \r
 249     // I4 [Grow tree taller] If node split propagation caused the root to \r
 250     // split, create a new root whose children are the two resulting nodes.\r
 251     if (newNode != null) {\r
 252       int oldRootNodeId = rootNodeId;\r
 253       Node oldRoot = getNode(oldRootNodeId);\r
 254       \r
 255       rootNodeId = getNextNodeId();\r
 256       treeHeight++;\r
 257       Node root = new Node(rootNodeId, treeHeight, maxNodeEntries);\r
 258       root.addEntry(newNode.mbrMinX, newNode.mbrMinY, newNode.mbrMaxX, newNode.mbrMaxY, newNode.nodeId);\r
 259       root.addEntry(oldRoot.mbrMinX, oldRoot.mbrMinY, oldRoot.mbrMaxX, oldRoot.mbrMaxY, oldRoot.nodeId);\r
 260       nodeMap.put(rootNodeId, root);\r
 261     }    \r
 262   } \r
 263   \r
 264   /**\r
 265    * @see com.infomatiq.jsi.SpatialIndex#delete(Rectangle, int)\r
 266    */\r
 267   public boolean delete(Rectangle r, int id) {\r
 268    // FindLeaf algorithm inlined here. Note the "official" algorithm \r
 269    // searches all overlapping entries. This seems inefficient to me, \r
 270    // as an entry is only worth searching if it contains (NOT overlaps)\r
 271    // the rectangle we are searching for.\r
 272    //\r
 273    // Also the algorithm has been changed so that it is not recursive.\r
 274     \r
 275     // FL1 [Search subtrees] If root is not a leaf, check each entry \r
 276     // to determine if it contains r. For each entry found, invoke\r
 277     // findLeaf on the node pointed to by the entry, until r is found or\r
 278     // all entries have been checked.\r
 279         parents.reset();\r
 280         parents.push(rootNodeId);\r
 281         \r
 282         parentsEntry.reset();\r
 283         parentsEntry.push(-1);\r
 284         Node n = null;\r
 285         int foundIndex = -1;  // index of entry to be deleted in leaf\r
 286         \r
 287         while (foundIndex == -1 && parents.size() > 0) {\r
 288           n = getNode(parents.peek());\r
 289           int startIndex = parentsEntry.peek() + 1;\r
 290       \r
 291       if (!n.isLeaf()) {\r
 292         deleteLog.debug("searching node " + n.nodeId + ", from index " + startIndex);\r
 293                   boolean contains = false;\r
 294         for (int i = startIndex; i < n.entryCount; i++) {\r
 295                     if (Rectangle.contains(n.entriesMinX[i], n.entriesMinY[i], n.entriesMaxX[i], n.entriesMaxY[i],\r
 296                                  r.minX, r.minY, r.maxX, r.maxY)) { \r
 297                       parents.push(n.ids[i]);\r
 298                       parentsEntry.pop();\r
 299                       parentsEntry.push(i); // this becomes the start index when the child has been searched\r
 300                       parentsEntry.push(-1);\r
 301                       contains = true;\r
 302             break; // ie go to next iteration of while()\r
 303                     }\r
 304                   }\r
 305         if (contains) {\r
 306           continue;\r
 307         }\r
 308       } else {\r
 309         foundIndex = n.findEntry(r.minX, r.minY, r.maxX, r.maxY, id);        \r
 310       }\r
 311       \r
 312       parents.pop();\r
 313       parentsEntry.pop();\r
 314         } // while not found\r
 315         \r
 316         if (foundIndex != -1) {\r
 317           n.deleteEntry(foundIndex);\r
 318       condenseTree(n);\r
 319       size--;\r
 320         }\r
 321         \r
 322     // shrink the tree if possible (i.e. if root node has exactly one entry,and that \r
 323     // entry is not a leaf node, delete the root (it's entry becomes the new root)\r
 324     Node root = getNode(rootNodeId);\r
 325     while (root.entryCount == 1 && treeHeight > 1)\r
 326     {\r
 327         deletedNodeIds.push(rootNodeId);\r
 328         root.entryCount = 0;\r
 329         rootNodeId = root.ids[0];\r
 330         treeHeight--;\r
 331         root = getNode(rootNodeId);\r
 332     }\r
 333     \r
 334     // if the tree is now empty, then set the MBR of the root node back to it's original state\r
 335     // (this is only needed when the tree is empty, as this is the only state where an empty node\r
 336     // is not eliminated)\r
 337     if (size == 0) {\r
 338       root.mbrMinX = Float.MAX_VALUE;\r
 339       root.mbrMinY = Float.MAX_VALUE;\r
 340       root.mbrMaxX = -Float.MAX_VALUE;\r
 341       root.mbrMaxY = -Float.MAX_VALUE;\r
 342     }\r
 343 \r
 344     if (INTERNAL_CONSISTENCY_CHECKING) {\r
 345       checkConsistency();\r
 346     }\r
 347         \r
 348     return (foundIndex != -1);\r
 349   }\r
 350   \r
 351   /**\r
 352    * @see com.infomatiq.jsi.SpatialIndex#nearest(Point, TIntProcedure, float)\r
 353    */\r
 354   public void nearest(Point p, TIntProcedure v, float furthestDistance) {\r
 355     Node rootNode = getNode(rootNodeId);\r
 356    \r
 357     float furthestDistanceSq = furthestDistance * furthestDistance;\r
 358     nearest(p, rootNode, furthestDistanceSq);\r
 359    \r
 360     nearestIds.forEach(v);\r
 361     nearestIds.reset();\r
 362   }\r
 363    \r
 364   private void createNearestNDistanceQueue(Point p, int count, float furthestDistance) {\r
 365     distanceQueue.reset();\r
 366     distanceQueue.setSortOrder(PriorityQueue.SORT_ORDER_DESCENDING);\r
 367     \r
 368     //  return immediately if given an invalid "count" parameter\r
 369     if (count <= 0) {\r
 370       return;\r
 371     }    \r
 372     \r
 373     parents.reset();\r
 374     parents.push(rootNodeId);\r
 375     \r
 376     parentsEntry.reset();\r
 377     parentsEntry.push(-1);\r
 378     \r
 379     // TODO: possible shortcut here - could test for intersection with the \r
 380     //       MBR of the root node. If no intersection, return immediately.\r
 381     \r
 382     float furthestDistanceSq = furthestDistance * furthestDistance;\r
 383     \r
 384     while (parents.size() > 0) {\r
 385       Node n = getNode(parents.peek());\r
 386       int startIndex = parentsEntry.peek() + 1;\r
 387       \r
 388       if (!n.isLeaf()) {\r
 389         // go through every entry in the index node to check\r
 390         // if it could contain an entry closer than the farthest entry\r
 391         // currently stored.\r
 392         boolean near = false;\r
 393         for (int i = startIndex; i < n.entryCount; i++) {\r
 394           if (Rectangle.distanceSq(n.entriesMinX[i], n.entriesMinY[i], \r
 395                                  n.entriesMaxX[i], n.entriesMaxY[i], \r
 396                                  p.x, p.y) <= furthestDistanceSq) {\r
 397             parents.push(n.ids[i]);\r
 398             parentsEntry.pop();\r
 399             parentsEntry.push(i); // this becomes the start index when the child has been searched\r
 400             parentsEntry.push(-1);\r
 401             near = true;\r
 402             break; // ie go to next iteration of while()\r
 403           }\r
 404         }\r
 405         if (near) {\r
 406           continue;\r
 407         }\r
 408       } else {\r
 409         // go through every entry in the leaf to check if \r
 410         // it is currently one of the nearest N entries.\r
 411         for (int i = 0; i < n.entryCount; i++) {\r
 412           float entryDistanceSq = Rectangle.distanceSq(n.entriesMinX[i], n.entriesMinY[i],\r
 413                                                    n.entriesMaxX[i], n.entriesMaxY[i],\r
 414                                                    p.x, p.y);\r
 415           int entryId = n.ids[i];\r
 416           \r
 417           if (entryDistanceSq <= furthestDistanceSq) {\r
 418             distanceQueue.insert(entryId, entryDistanceSq);\r
 419             \r
 420             while (distanceQueue.size() > count) {\r
 421               // normal case - we can simply remove the lowest priority (highest distance) entry\r
 422               int value = distanceQueue.getValue();\r
 423               float distanceSq = distanceQueue.getPriority();\r
 424               distanceQueue.pop();\r
 425               \r
 426               // rare case - multiple items of the same priority (distance)\r
 427               if (distanceSq == distanceQueue.getPriority()) {\r
 428                 savedValues.add(value);\r
 429                 savedPriority = distanceSq;\r
 430               } else {\r
 431                 savedValues.reset();\r
 432               }\r
 433             }\r
 434             \r
 435             // if the saved values have the same distance as the\r
 436             // next one in the tree, add them back in.\r
 437             if (savedValues.size() > 0 && savedPriority == distanceQueue.getPriority()) {\r
 438               for (int svi = 0; svi < savedValues.size(); svi++) {\r
 439                 distanceQueue.insert(savedValues.get(svi), savedPriority);\r
 440               }\r
 441               savedValues.reset();\r
 442             }\r
 443             \r
 444             // narrow the search, if we have already found N items\r
 445             if (distanceQueue.getPriority() < furthestDistanceSq && distanceQueue.size() >= count) {\r
 446               furthestDistanceSq = distanceQueue.getPriority();  \r
 447             }\r
 448           } \r
 449         }                       \r
 450       }\r
 451       parents.pop();\r
 452       parentsEntry.pop();  \r
 453     }\r
 454   }\r
 455   \r
 456   /**\r
 457    * @see com.infomatiq.jsi.SpatialIndex#nearestNUnsorted(Point, TIntProcedure, int, float)\r
 458    */\r
 459   public void nearestNUnsorted(Point p, TIntProcedure v, int count, float furthestDistance) {\r
 460     // This implementation is designed to give good performance\r
 461     // where\r
 462     //   o N is high (100+)\r
 463     //   o The results do not need to be sorted by distance.\r
 464     //     \r
 465     // Uses a priority queue as the underlying data structure. \r
 466     //   \r
 467     // The behaviour of this algorithm has been carefully designed to\r
 468     // return exactly the same items as the the original version (nearestN_orig), in particular,\r
 469     // more than N items will be returned if items N and N+x have the\r
 470     // same priority. \r
 471     createNearestNDistanceQueue(p, count, furthestDistance);\r
 472    \r
 473     while (distanceQueue.size() > 0) {\r
 474       v.execute(distanceQueue.getValue());\r
 475       distanceQueue.pop();\r
 476     }\r
 477   }\r
 478   \r
 479   /**\r
 480    * @see com.infomatiq.jsi.SpatialIndex#nearestN(Point, TIntProcedure, int, float)\r
 481    */\r
 482   public void nearestN(Point p, TIntProcedure v, int count, float furthestDistance) {\r
 483     createNearestNDistanceQueue(p, count, furthestDistance);\r
 484     \r
 485     distanceQueue.setSortOrder(PriorityQueue.SORT_ORDER_ASCENDING);\r
 486     \r
 487     while (distanceQueue.size() > 0) {\r
 488       v.execute(distanceQueue.getValue());\r
 489       distanceQueue.pop();\r
 490     }  \r
 491   }\r
 492     \r
 493   /**\r
 494    * @see com.infomatiq.jsi.SpatialIndex#nearestN(Point, TIntProcedure, int, float)\r
 495    * @deprecated Use new NearestN or NearestNUnsorted instead.\r
 496    * \r
 497    * This implementation of nearestN is only suitable for small values of N (ie less than 10).\r
 498    */ \r
 499   public void nearestN_orig(Point p, TIntProcedure v, int count, float furthestDistance) {\r
 500     // return immediately if given an invalid "count" parameter\r
 501     if (count <= 0) {\r
 502       return;\r
 503     }\r
 504     \r
 505     parents.reset();\r
 506     parents.push(rootNodeId);\r
 507     \r
 508     parentsEntry.reset();\r
 509     parentsEntry.push(-1);\r
 510     \r
 511     nearestNIds.init(count);\r
 512     \r
 513     // TODO: possible shortcut here - could test for intersection with the \r
 514     //       MBR of the root node. If no intersection, return immediately.\r
 515     \r
 516     float furthestDistanceSq = furthestDistance * furthestDistance;\r
 517     \r
 518     while (parents.size() > 0) {\r
 519       Node n = getNode(parents.peek());\r
 520       int startIndex = parentsEntry.peek() + 1;\r
 521       \r
 522       if (!n.isLeaf()) {\r
 523         // go through every entry in the index node to check\r
 524         // if it could contain an entry closer than the farthest entry\r
 525         // currently stored.\r
 526         boolean near = false;\r
 527         for (int i = startIndex; i < n.entryCount; i++) {\r
 528           if (Rectangle.distanceSq(n.entriesMinX[i], n.entriesMinY[i], \r
 529                                  n.entriesMaxX[i], n.entriesMaxY[i], \r
 530                                  p.x, p.y) <= furthestDistanceSq) {\r
 531             parents.push(n.ids[i]);\r
 532             parentsEntry.pop();\r
 533             parentsEntry.push(i); // this becomes the start index when the child has been searched\r
 534             parentsEntry.push(-1);\r
 535             near = true;\r
 536             break; // ie go to next iteration of while()\r
 537           }\r
 538         }\r
 539         if (near) {\r
 540           continue;\r
 541         }\r
 542       } else {\r
 543         // go through every entry in the leaf to check if \r
 544         // it is currently one of the nearest N entries.\r
 545         for (int i = 0; i < n.entryCount; i++) {\r
 546           float entryDistanceSq = Rectangle.distanceSq(n.entriesMinX[i], n.entriesMinY[i],\r
 547                                                    n.entriesMaxX[i], n.entriesMaxY[i],\r
 548                                                    p.x, p.y);\r
 549           int entryId = n.ids[i];\r
 550           \r
 551           if (entryDistanceSq <= furthestDistanceSq) {\r
 552             // add the new entry to the tree. Note that the higher the distance, the lower the priority\r
 553             nearestNIds.add(entryId, -entryDistanceSq);\r
 554             \r
 555             float tempFurthestDistanceSq = -nearestNIds.getLowestPriority();\r
 556             if (tempFurthestDistanceSq < furthestDistanceSq) {\r
 557               furthestDistanceSq = tempFurthestDistanceSq;  \r
 558             }\r
 559           } \r
 560         }                       \r
 561       }\r
 562       parents.pop();\r
 563       parentsEntry.pop();  \r
 564     }\r
 565    \r
 566     nearestNIds.forEachId(v);\r
 567   }\r
 568    \r
 569   /**\r
 570    * @see com.infomatiq.jsi.SpatialIndex#intersects(Rectangle, TIntProcedure)\r
 571    */\r
 572   public void intersects(Rectangle r, TIntProcedure v) {\r
 573     Node rootNode = getNode(rootNodeId);\r
 574     intersects(r, v, rootNode);\r
 575   }\r
 576 \r
 577   /**\r
 578    * @see com.infomatiq.jsi.SpatialIndex#contains(Rectangle, TIntProcedure)\r
 579    */\r
 580   public void contains(Rectangle r, TIntProcedure v) {\r
 581     // find all rectangles in the tree that are contained by the passed rectangle\r
 582     // written to be non-recursive (should model other searches on this?)\r
 583         \r
 584     parents.reset();\r
 585     parents.push(rootNodeId);\r
 586     \r
 587     parentsEntry.reset();\r
 588     parentsEntry.push(-1);\r
 589     \r
 590     // TODO: possible shortcut here - could test for intersection with the \r
 591     // MBR of the root node. If no intersection, return immediately.\r
 592     \r
 593     while (parents.size() > 0) {\r
 594       Node n = getNode(parents.peek());\r
 595       int startIndex = parentsEntry.peek() + 1;\r
 596       \r
 597       if (!n.isLeaf()) {\r
 598         // go through every entry in the index node to check\r
 599         // if it intersects the passed rectangle. If so, it \r
 600         // could contain entries that are contained.\r
 601         boolean intersects = false;\r
 602         for (int i = startIndex; i < n.entryCount; i++) {\r
 603           if (Rectangle.intersects(r.minX, r.minY, r.maxX, r.maxY, \r
 604                                    n.entriesMinX[i], n.entriesMinY[i], n.entriesMaxX[i], n.entriesMaxY[i])) {\r
 605             parents.push(n.ids[i]);\r
 606             parentsEntry.pop();\r
 607             parentsEntry.push(i); // this becomes the start index when the child has been searched\r
 608             parentsEntry.push(-1);\r
 609             intersects = true;\r
 610             break; // ie go to next iteration of while()\r
 611           }\r
 612         }\r
 613         if (intersects) {\r
 614           continue;\r
 615         }\r
 616       } else {\r
 617         // go through every entry in the leaf to check if \r
 618         // it is contained by the passed rectangle\r
 619         for (int i = 0; i < n.entryCount; i++) {\r
 620           if (Rectangle.contains(r.minX, r.minY, r.maxX, r.maxY, \r
 621                                  n.entriesMinX[i], n.entriesMinY[i], n.entriesMaxX[i], n.entriesMaxY[i])) {\r
 622             if (!v.execute(n.ids[i])) {\r
 623               return;\r
 624             }\r
 625           } \r
 626         }                       \r
 627       }\r
 628       parents.pop();\r
 629       parentsEntry.pop();  \r
 630     }\r
 631   }\r
 632 \r
 633   /**\r
 634    * @see com.infomatiq.jsi.SpatialIndex#size()\r
 635    */\r
 636   public int size() {\r
 637     return size;\r
 638   }\r
 639 \r
 640   /**\r
 641    * @see com.infomatiq.jsi.SpatialIndex#getBounds()\r
 642    */\r
 643   public Rectangle getBounds() {\r
 644     Rectangle bounds = null;\r
 645     \r
 646     Node n = getNode(getRootNodeId());\r
 647     if (n != null && n.entryCount > 0) {\r
 648       bounds = new Rectangle();\r
 649       bounds.minX = n.mbrMinX;\r
 650       bounds.minY = n.mbrMinY;\r
 651       bounds.maxX = n.mbrMaxX;\r
 652       bounds.maxY = n.mbrMaxY;\r
 653     }\r
 654     return bounds;\r
 655   }\r
 656     \r
 657   /**\r
 658    * @see com.infomatiq.jsi.SpatialIndex#getVersion()\r
 659    */\r
 660   public String getVersion() {\r
 661     return "RTree-" + version;\r
 662   }\r
 663   //-------------------------------------------------------------------------\r
 664   // end of SpatialIndex methods\r
 665   //-------------------------------------------------------------------------\r
 666   \r
 667   /**\r
 668    * Get the next available node ID. Reuse deleted node IDs if\r
 669    * possible\r
 670    */\r
 671   private int getNextNodeId() {\r
 672     int nextNodeId = 0;\r
 673     if (deletedNodeIds.size() > 0) {\r
 674       nextNodeId = deletedNodeIds.pop();\r
 675     } else {\r
 676       nextNodeId = 1 + highestUsedNodeId++;\r
 677     }\r
 678     return nextNodeId;\r
 679   }\r
 680 \r
 681   /**\r
 682    * Get a node object, given the ID of the node.\r
 683    */\r
 684   public Node getNode(int id) {\r
 685     return nodeMap.get(id);\r
 686   }\r
 687 \r
 688   /**\r
 689    * Get the highest used node ID\r
 690    */  \r
 691   public int getHighestUsedNodeId() {\r
 692     return highestUsedNodeId;\r
 693   }\r
 694 \r
 695   /**\r
 696    * Get the root node ID\r
 697    */\r
 698   public int getRootNodeId() {\r
 699     return rootNodeId; \r
 700   }\r
 701       \r
 702   /**\r
 703    * Split a node. Algorithm is taken pretty much verbatim from\r
 704    * Guttman's original paper.\r
 705    * \r
 706    * @return new node object.\r
 707    */\r
 708   private Node splitNode(Node n, float newRectMinX, float newRectMinY, float newRectMaxX, float newRectMaxY, int newId) {\r
 709     // [Pick first entry for each group] Apply algorithm pickSeeds to \r
 710     // choose two entries to be the first elements of the groups. Assign\r
 711     // each to a group.\r
 712     \r
 713     // debug code\r
 714     float initialArea = 0;\r
 715     if (log.isDebugEnabled()) {\r
 716       float unionMinX = Math.min(n.mbrMinX, newRectMinX);\r
 717       float unionMinY = Math.min(n.mbrMinY, newRectMinY);\r
 718       float unionMaxX = Math.max(n.mbrMaxX, newRectMaxX);\r
 719       float unionMaxY = Math.max(n.mbrMaxY, newRectMaxY);\r
 720       \r
 721       initialArea = (unionMaxX - unionMinX) * (unionMaxY - unionMinY);\r
 722     }\r
 723        \r
 724     System.arraycopy(initialEntryStatus, 0, entryStatus, 0, maxNodeEntries);\r
 725     \r
 726     Node newNode = null;\r
 727     newNode = new Node(getNextNodeId(), n.level, maxNodeEntries);\r
 728     nodeMap.put(newNode.nodeId, newNode);\r
 729     \r
 730     pickSeeds(n, newRectMinX, newRectMinY, newRectMaxX, newRectMaxY, newId, newNode); // this also sets the entryCount to 1\r
 731     \r
 732     // [Check if done] If all entries have been assigned, stop. If one\r
 733     // group has so few entries that all the rest must be assigned to it in \r
 734     // order for it to have the minimum number m, assign them and stop. \r
 735     while (n.entryCount + newNode.entryCount < maxNodeEntries + 1) {\r
 736       if (maxNodeEntries + 1 - newNode.entryCount == minNodeEntries) {\r
 737         // assign all remaining entries to original node\r
 738         for (int i = 0; i < maxNodeEntries; i++) {\r
 739           if (entryStatus[i] == ENTRY_STATUS_UNASSIGNED) {\r
 740             entryStatus[i] = ENTRY_STATUS_ASSIGNED;\r
 741             \r
 742             if (n.entriesMinX[i] < n.mbrMinX) n.mbrMinX = n.entriesMinX[i];\r
 743             if (n.entriesMinY[i] < n.mbrMinY) n.mbrMinY = n.entriesMinY[i];\r
 744             if (n.entriesMaxX[i] > n.mbrMaxX) n.mbrMaxX = n.entriesMaxX[i];\r
 745             if (n.entriesMaxY[i] > n.mbrMaxY) n.mbrMaxY = n.entriesMaxY[i];\r
 746             \r
 747             n.entryCount++;\r
 748           }\r
 749         }\r
 750         break;\r
 751       }   \r
 752       if (maxNodeEntries + 1 - n.entryCount == minNodeEntries) {\r
 753         // assign all remaining entries to new node\r
 754         for (int i = 0; i < maxNodeEntries; i++) {\r
 755           if (entryStatus[i] == ENTRY_STATUS_UNASSIGNED) {\r
 756             entryStatus[i] = ENTRY_STATUS_ASSIGNED;\r
 757             newNode.addEntry(n.entriesMinX[i], n.entriesMinY[i], n.entriesMaxX[i], n.entriesMaxY[i], n.ids[i]);\r
 758             n.ids[i] = -1; // an id of -1 indicates the entry is not in use\r
 759           }\r
 760         }\r
 761         break;\r
 762       }\r
 763       \r
 764       // [Select entry to assign] Invoke algorithm pickNext to choose the\r
 765       // next entry to assign. Add it to the group whose covering rectangle \r
 766       // will have to be enlarged least to accommodate it. Resolve ties\r
 767       // by adding the entry to the group with smaller area, then to the \r
 768       // the one with fewer entries, then to either. Repeat from S2\r
 769       pickNext(n, newNode);   \r
 770     }\r
 771       \r
 772     n.reorganize(this);\r
 773     \r
 774     // check that the MBR stored for each node is correct.\r
 775     if (INTERNAL_CONSISTENCY_CHECKING) {\r
 776       Rectangle nMBR = new Rectangle(n.mbrMinX, n.mbrMinY, n.mbrMaxX, n.mbrMaxY);\r
 777       if (!nMBR.equals(calculateMBR(n))) {\r
 778         log.error("Error: splitNode old node MBR wrong");\r
 779       }\r
 780       Rectangle newNodeMBR = new Rectangle(newNode.mbrMinX, newNode.mbrMinY, newNode.mbrMaxX, newNode.mbrMaxY);\r
 781       if (!newNodeMBR.equals(calculateMBR(newNode))) {\r
 782         log.error("Error: splitNode new node MBR wrong");\r
 783       }\r
 784     }\r
 785     \r
 786     // debug code\r
 787     if (log.isDebugEnabled()) {\r
 788       float newArea = Rectangle.area(n.mbrMinX, n.mbrMinY, n.mbrMaxX, n.mbrMaxY) + \r
 789                       Rectangle.area(newNode.mbrMinX, newNode.mbrMinY, newNode.mbrMaxX, newNode.mbrMaxY);\r
 790       float percentageIncrease = (100 * (newArea - initialArea)) / initialArea;\r
 791       log.debug("Node " + n.nodeId + " split. New area increased by " + percentageIncrease + "%");   \r
 792     }\r
 793       \r
 794     return newNode;\r
 795   }\r
 796   \r
 797   /**\r
 798    * Pick the seeds used to split a node.\r
 799    * Select two entries to be the first elements of the groups\r
 800    */\r
 801   private void pickSeeds(Node n, float newRectMinX, float newRectMinY, float newRectMaxX, float newRectMaxY, int newId, Node newNode) {\r
 802     // Find extreme rectangles along all dimension. Along each dimension,\r
 803     // find the entry whose rectangle has the highest low side, and the one \r
 804     // with the lowest high side. Record the separation.\r
 805     float maxNormalizedSeparation = -1; // initialize to -1 so that even overlapping rectangles will be considered for the seeds\r
 806     int highestLowIndex = -1;\r
 807     int lowestHighIndex = -1;\r
 808     \r
 809     // for the purposes of picking seeds, take the MBR of the node to include\r
 810     // the new rectangle aswell.\r
 811     if (newRectMinX < n.mbrMinX) n.mbrMinX = newRectMinX;\r
 812     if (newRectMinY < n.mbrMinY) n.mbrMinY = newRectMinY;\r
 813     if (newRectMaxX > n.mbrMaxX) n.mbrMaxX = newRectMaxX;\r
 814     if (newRectMaxY > n.mbrMaxY) n.mbrMaxY = newRectMaxY;\r
 815     \r
 816     float mbrLenX = n.mbrMaxX - n.mbrMinX;\r
 817     float mbrLenY = n.mbrMaxY - n.mbrMinY;\r
 818     \r
 819     if (log.isDebugEnabled()) {\r
 820       log.debug("pickSeeds(): NodeId = " + n.nodeId);\r
 821     }\r
 822     \r
 823     float tempHighestLow = newRectMinX;\r
 824     int tempHighestLowIndex = -1; // -1 indicates the new rectangle is the seed\r
 825     \r
 826     float tempLowestHigh = newRectMaxX;\r
 827     int tempLowestHighIndex = -1; // -1 indicates the new rectangle is the seed \r
 828     \r
 829     for (int i = 0; i < n.entryCount; i++) {\r
 830       float tempLow = n.entriesMinX[i];\r
 831       if (tempLow >= tempHighestLow) {\r
 832          tempHighestLow = tempLow;\r
 833          tempHighestLowIndex = i;\r
 834       } else {  // ensure that the same index cannot be both lowestHigh and highestLow\r
 835         float tempHigh = n.entriesMaxX[i];\r
 836         if (tempHigh <= tempLowestHigh) {\r
 837           tempLowestHigh = tempHigh;\r
 838           tempLowestHighIndex = i;\r
 839         }\r
 840       }\r
 841       \r
 842       // PS2 [Adjust for shape of the rectangle cluster] Normalize the separations\r
 843       // by dividing by the widths of the entire set along the corresponding\r
 844       // dimension\r
 845       float normalizedSeparation = mbrLenX == 0 ? 1 : (tempHighestLow - tempLowestHigh) / mbrLenX;\r
 846       if (normalizedSeparation > 1 || normalizedSeparation < -1) {\r
 847         log.error("Invalid normalized separation X");\r
 848       }\r
 849       \r
 850       if (log.isDebugEnabled()) {\r
 851               log.debug("Entry " + i + ", dimension X: HighestLow = " + tempHighestLow + \r
 852                         " (index " + tempHighestLowIndex + ")" + ", LowestHigh = " +\r
 853                         tempLowestHigh + " (index " + tempLowestHighIndex + ", NormalizedSeparation = " + normalizedSeparation);\r
 854       }\r
 855           \r
 856       // PS3 [Select the most extreme pair] Choose the pair with the greatest\r
 857       // normalized separation along any dimension.\r
 858       // Note that if negative it means the rectangles overlapped. However still include\r
 859       // overlapping rectangles if that is the only choice available.\r
 860       if (normalizedSeparation >= maxNormalizedSeparation) {\r
 861         highestLowIndex = tempHighestLowIndex;\r
 862         lowestHighIndex = tempLowestHighIndex;\r
 863         maxNormalizedSeparation = normalizedSeparation;\r
 864       }\r
 865     }\r
 866     \r
 867     // Repeat for the Y dimension\r
 868     tempHighestLow = newRectMinY;\r
 869     tempHighestLowIndex = -1; // -1 indicates the new rectangle is the seed\r
 870     \r
 871     tempLowestHigh = newRectMaxY;\r
 872     tempLowestHighIndex = -1; // -1 indicates the new rectangle is the seed \r
 873     \r
 874     for (int i = 0; i < n.entryCount; i++) {\r
 875       float tempLow = n.entriesMinY[i];\r
 876       if (tempLow >= tempHighestLow) {\r
 877          tempHighestLow = tempLow;\r
 878          tempHighestLowIndex = i;\r
 879       } else {  // ensure that the same index cannot be both lowestHigh and highestLow\r
 880         float tempHigh = n.entriesMaxY[i];\r
 881         if (tempHigh <= tempLowestHigh) {\r
 882           tempLowestHigh = tempHigh;\r
 883           tempLowestHighIndex = i;\r
 884         }\r
 885       }\r
 886       \r
 887       // PS2 [Adjust for shape of the rectangle cluster] Normalize the separations\r
 888       // by dividing by the widths of the entire set along the corresponding\r
 889       // dimension\r
 890       float normalizedSeparation = mbrLenY == 0 ? 1 : (tempHighestLow - tempLowestHigh) / mbrLenY;\r
 891       if (normalizedSeparation > 1 || normalizedSeparation < -1) {\r
 892         log.error("Invalid normalized separation Y");\r
 893       }\r
 894       \r
 895       if (log.isDebugEnabled()) {\r
 896         log.debug("Entry " + i + ", dimension Y: HighestLow = " + tempHighestLow + \r
 897                   " (index " + tempHighestLowIndex + ")" + ", LowestHigh = " +\r
 898                   tempLowestHigh + " (index " + tempLowestHighIndex + ", NormalizedSeparation = " + normalizedSeparation);\r
 899       }\r
 900           \r
 901       // PS3 [Select the most extreme pair] Choose the pair with the greatest\r
 902       // normalized separation along any dimension.\r
 903       // Note that if negative it means the rectangles overlapped. However still include\r
 904       // overlapping rectangles if that is the only choice available.\r
 905       if (normalizedSeparation >= maxNormalizedSeparation) {\r
 906         highestLowIndex = tempHighestLowIndex;\r
 907         lowestHighIndex = tempLowestHighIndex;\r
 908         maxNormalizedSeparation = normalizedSeparation;\r
 909       }\r
 910     }\r
 911     \r
 912     // At this point it is possible that the new rectangle is both highestLow and lowestHigh.\r
 913     // This can happen if all rectangles in the node overlap the new rectangle.\r
 914     // Resolve this by declaring that the highestLowIndex is the lowest Y and,\r
 915     // the lowestHighIndex is the largest X (but always a different rectangle)\r
 916     if (highestLowIndex == lowestHighIndex) { \r
 917       highestLowIndex = -1;\r
 918       float tempMinY = newRectMinY;\r
 919       lowestHighIndex = 0;\r
 920       float tempMaxX = n.entriesMaxX[0];\r
 921       \r
 922       for (int i = 1; i < n.entryCount; i++) {\r
 923         if (n.entriesMinY[i] < tempMinY) {\r
 924           tempMinY = n.entriesMinY[i];\r
 925           highestLowIndex = i;\r
 926         }\r
 927         else if (n.entriesMaxX[i] > tempMaxX) {\r
 928           tempMaxX = n.entriesMaxX[i];\r
 929           lowestHighIndex = i;\r
 930         }\r
 931       }\r
 932     }\r
 933     \r
 934     // highestLowIndex is the seed for the new node.\r
 935     if (highestLowIndex == -1) {\r
 936       newNode.addEntry(newRectMinX, newRectMinY, newRectMaxX, newRectMaxY, newId);\r
 937     } else {\r
 938       newNode.addEntry(n.entriesMinX[highestLowIndex], n.entriesMinY[highestLowIndex], \r
 939                        n.entriesMaxX[highestLowIndex], n.entriesMaxY[highestLowIndex], \r
 940                        n.ids[highestLowIndex]);\r
 941       n.ids[highestLowIndex] = -1;\r
 942       \r
 943       // move the new rectangle into the space vacated by the seed for the new node\r
 944       n.entriesMinX[highestLowIndex] = newRectMinX;\r
 945       n.entriesMinY[highestLowIndex] = newRectMinY;\r
 946       n.entriesMaxX[highestLowIndex] = newRectMaxX;\r
 947       n.entriesMaxY[highestLowIndex] = newRectMaxY;\r
 948       \r
 949       n.ids[highestLowIndex] = newId;\r
 950     }\r
 951     \r
 952     // lowestHighIndex is the seed for the original node. \r
 953     if (lowestHighIndex == -1) {\r
 954       lowestHighIndex = highestLowIndex;\r
 955     }\r
 956     \r
 957     entryStatus[lowestHighIndex] = ENTRY_STATUS_ASSIGNED;\r
 958     n.entryCount = 1;\r
 959     n.mbrMinX = n.entriesMinX[lowestHighIndex];\r
 960     n.mbrMinY = n.entriesMinY[lowestHighIndex];\r
 961     n.mbrMaxX = n.entriesMaxX[lowestHighIndex];\r
 962     n.mbrMaxY = n.entriesMaxY[lowestHighIndex];\r
 963   }\r
 964 \r
 965   /** \r
 966    * Pick the next entry to be assigned to a group during a node split.\r
 967    * \r
 968    * [Determine cost of putting each entry in each group] For each \r
 969    * entry not yet in a group, calculate the area increase required\r
 970    * in the covering rectangles of each group  \r
 971    */\r
 972   private int pickNext(Node n, Node newNode) {\r
 973     float maxDifference = Float.NEGATIVE_INFINITY;\r
 974     int next = 0;\r
 975     int nextGroup = 0;\r
 976     \r
 977     maxDifference = Float.NEGATIVE_INFINITY;\r
 978    \r
 979     if (log.isDebugEnabled()) {\r
 980       log.debug("pickNext()");\r
 981     }\r
 982    \r
 983     for (int i = 0; i < maxNodeEntries; i++) {\r
 984       if (entryStatus[i] == ENTRY_STATUS_UNASSIGNED) {\r
 985         \r
 986         if (n.ids[i] == -1) {\r
 987           log.error("Error: Node " + n.nodeId + ", entry " + i + " is null");\r
 988         }\r
 989         \r
 990         float nIncrease = Rectangle.enlargement(n.mbrMinX, n.mbrMinY, n.mbrMaxX, n.mbrMaxY, \r
 991                                                 n.entriesMinX[i], n.entriesMinY[i], n.entriesMaxX[i], n.entriesMaxY[i]);\r
 992         float newNodeIncrease = Rectangle.enlargement(newNode.mbrMinX, newNode.mbrMinY, newNode.mbrMaxX, newNode.mbrMaxY,\r
 993                                                       n.entriesMinX[i], n.entriesMinY[i], n.entriesMaxX[i], n.entriesMaxY[i]);\r
 994 \r
 995         float difference = Math.abs(nIncrease - newNodeIncrease);\r
 996          \r
 997         if (difference > maxDifference) {\r
 998           next = i;\r
 999           \r
1000           if (nIncrease < newNodeIncrease) {\r
1001             nextGroup = 0; \r
1002           } else if (newNodeIncrease < nIncrease) {\r
1003             nextGroup = 1;\r
1004           } else if (Rectangle.area(n.mbrMinX, n.mbrMinY, n.mbrMaxX, n.mbrMaxY) < Rectangle.area(newNode.mbrMinX, newNode.mbrMinY, newNode.mbrMaxX, newNode.mbrMaxY)) {\r
1005             nextGroup = 0;\r
1006           } else if (Rectangle.area(newNode.mbrMinX, newNode.mbrMinY, newNode.mbrMaxX, newNode.mbrMaxY) < Rectangle.area(n.mbrMinX, n.mbrMinY, n.mbrMaxX, n.mbrMaxY)) {\r
1007             nextGroup = 1;\r
1008           } else if (newNode.entryCount < maxNodeEntries / 2) {\r
1009             nextGroup = 0;\r
1010           } else {\r
1011             nextGroup = 1;\r
1012           }\r
1013           maxDifference = difference; \r
1014         }\r
1015         if (log.isDebugEnabled()) {\r
1016           log.debug("Entry " + i + " group0 increase = " + nIncrease + ", group1 increase = " + newNodeIncrease +\r
1017                     ", diff = " + difference + ", MaxDiff = " + maxDifference + " (entry " + next + ")");\r
1018         }\r
1019       }\r
1020     }\r
1021     \r
1022     entryStatus[next] = ENTRY_STATUS_ASSIGNED;\r
1023       \r
1024     if (nextGroup == 0) {\r
1025       if (n.entriesMinX[next] < n.mbrMinX) n.mbrMinX = n.entriesMinX[next];\r
1026       if (n.entriesMinY[next] < n.mbrMinY) n.mbrMinY = n.entriesMinY[next];\r
1027       if (n.entriesMaxX[next] > n.mbrMaxX) n.mbrMaxX = n.entriesMaxX[next];\r
1028       if (n.entriesMaxY[next] > n.mbrMaxY) n.mbrMaxY = n.entriesMaxY[next];\r
1029       n.entryCount++;\r
1030     } else {\r
1031       // move to new node.\r
1032       newNode.addEntry(n.entriesMinX[next], n.entriesMinY[next], n.entriesMaxX[next], n.entriesMaxY[next], n.ids[next]);\r
1033       n.ids[next] = -1;\r
1034     }\r
1035     \r
1036     return next; \r
1037   }\r
1038 \r
1039   /**\r
1040    * Recursively searches the tree for the nearest entry. Other queries\r
1041    * call execute() on an IntProcedure when a matching entry is found; \r
1042    * however nearest() must store the entry Ids as it searches the tree,\r
1043    * in case a nearer entry is found.\r
1044    * Uses the member variable nearestIds to store the nearest\r
1045    * entry IDs.\r
1046    * \r
1047    * TODO rewrite this to be non-recursive?\r
1048    */\r
1049   private float nearest(Point p, Node n, float furthestDistanceSq) {\r
1050     for (int i = 0; i < n.entryCount; i++) {\r
1051       float tempDistanceSq = Rectangle.distanceSq(n.entriesMinX[i], n.entriesMinY[i], n.entriesMaxX[i], n.entriesMaxY[i], p.x, p.y);\r
1052       if (n.isLeaf()) { // for leaves, the distance is an actual nearest distance \r
1053         if (tempDistanceSq < furthestDistanceSq) {\r
1054           furthestDistanceSq = tempDistanceSq;\r
1055           nearestIds.reset();\r
1056         }\r
1057         if (tempDistanceSq <= furthestDistanceSq) {\r
1058           nearestIds.add(n.ids[i]);\r
1059         }     \r
1060       } else { // for index nodes, only go into them if they potentially could have\r
1061                // a rectangle nearer than actualNearest\r
1062          if (tempDistanceSq <= furthestDistanceSq) {\r
1063            // search the child node\r
1064            furthestDistanceSq = nearest(p, getNode(n.ids[i]), furthestDistanceSq);\r
1065          }\r
1066       }\r
1067     }\r
1068     return furthestDistanceSq;\r
1069   }\r
1070   \r
1071   /** \r
1072    * Recursively searches the tree for all intersecting entries.\r
1073    * Immediately calls execute() on the passed IntProcedure when \r
1074    * a matching entry is found.\r
1075    * \r
1076    * TODO rewrite this to be non-recursive? Make sure it\r
1077    * doesn't slow it down.\r
1078    */\r
1079   private boolean intersects(Rectangle r, TIntProcedure v, Node n) {\r
1080     for (int i = 0; i < n.entryCount; i++) {\r
1081       if (Rectangle.intersects(r.minX, r.minY, r.maxX, r.maxY, n.entriesMinX[i], n.entriesMinY[i], n.entriesMaxX[i], n.entriesMaxY[i])) {\r
1082         if (n.isLeaf()) {\r
1083           if (!v.execute(n.ids[i])) {\r
1084             return false;\r
1085           }\r
1086         } else {\r
1087           Node childNode = getNode(n.ids[i]);\r
1088           if (!intersects(r, v, childNode)) {\r
1089             return false;\r
1090           }\r
1091         }\r
1092       }\r
1093     }\r
1094     return true;\r
1095   }\r
1096 \r
1097   /**\r
1098    * Used by delete(). Ensures that all nodes from the passed node\r
1099    * up to the root have the minimum number of entries.\r
1100    * \r
1101    * Note that the parent and parentEntry stacks are expected to\r
1102    * contain the nodeIds of all parents up to the root.\r
1103    */\r
1104   private void condenseTree(Node l) {\r
1105     // CT1 [Initialize] Set n=l. Set the list of eliminated\r
1106     // nodes to be empty.\r
1107     Node n = l;\r
1108     Node parent = null;\r
1109     int parentEntry = 0;\r
1110     \r
1111     TIntStack eliminatedNodeIds = new TIntStack();\r
1112   \r
1113     // CT2 [Find parent entry] If N is the root, go to CT6. Otherwise \r
1114     // let P be the parent of N, and let En be N's entry in P  \r
1115     while (n.level != treeHeight) {\r
1116       parent = getNode(parents.pop());\r
1117       parentEntry = parentsEntry.pop();\r
1118       \r
1119       // CT3 [Eliminiate under-full node] If N has too few entries,\r
1120       // delete En from P and add N to the list of eliminated nodes\r
1121       if (n.entryCount < minNodeEntries) {\r
1122         parent.deleteEntry(parentEntry);\r
1123         eliminatedNodeIds.push(n.nodeId);\r
1124       } else {\r
1125         // CT4 [Adjust covering rectangle] If N has not been eliminated,\r
1126         // adjust EnI to tightly contain all entries in N\r
1127         if (n.mbrMinX != parent.entriesMinX[parentEntry] ||\r
1128             n.mbrMinY != parent.entriesMinY[parentEntry] ||\r
1129             n.mbrMaxX != parent.entriesMaxX[parentEntry] ||\r
1130             n.mbrMaxY != parent.entriesMaxY[parentEntry]) {\r
1131           float deletedMinX = parent.entriesMinX[parentEntry];\r
1132           float deletedMinY = parent.entriesMinY[parentEntry];\r
1133           float deletedMaxX = parent.entriesMaxX[parentEntry];\r
1134           float deletedMaxY = parent.entriesMaxY[parentEntry];\r
1135           parent.entriesMinX[parentEntry] = n.mbrMinX;\r
1136           parent.entriesMinY[parentEntry] = n.mbrMinY;\r
1137           parent.entriesMaxX[parentEntry] = n.mbrMaxX;\r
1138           parent.entriesMaxY[parentEntry] = n.mbrMaxY;\r
1139           parent.recalculateMBRIfInfluencedBy(deletedMinX, deletedMinY, deletedMaxX, deletedMaxY);\r
1140         }\r
1141       }\r
1142       // CT5 [Move up one level in tree] Set N=P and repeat from CT2\r
1143       n = parent;\r
1144     }\r
1145     \r
1146     // CT6 [Reinsert orphaned entries] Reinsert all entries of nodes in set Q.\r
1147     // Entries from eliminated leaf nodes are reinserted in tree leaves as in \r
1148     // Insert(), but entries from higher level nodes must be placed higher in \r
1149     // the tree, so that leaves of their dependent subtrees will be on the same\r
1150     // level as leaves of the main tree\r
1151     while (eliminatedNodeIds.size() > 0) {\r
1152       Node e = getNode(eliminatedNodeIds.pop());\r
1153       for (int j = 0; j < e.entryCount; j++) {\r
1154         add(e.entriesMinX[j], e.entriesMinY[j], e.entriesMaxX[j], e.entriesMaxY[j], e.ids[j], e.level); \r
1155         e.ids[j] = -1;\r
1156       }\r
1157       e.entryCount = 0;\r
1158       deletedNodeIds.push(e.nodeId);\r
1159     }\r
1160   }\r
1161 \r
1162   /**\r
1163    *  Used by add(). Chooses a leaf to add the rectangle to.\r
1164    */\r
1165   private Node chooseNode(float minX, float minY, float maxX, float maxY, int level) {\r
1166     // CL1 [Initialize] Set N to be the root node\r
1167     Node n = getNode(rootNodeId);\r
1168     parents.reset();\r
1169     parentsEntry.reset();\r
1170      \r
1171     // CL2 [Leaf check] If N is a leaf, return N\r
1172     while (true) {\r
1173       if (n == null) {\r
1174         log.error("Could not get root node (" + rootNodeId + ")");  \r
1175       }\r
1176    \r
1177       if (n.level == level) {\r
1178         return n;\r
1179       }\r
1180       \r
1181       // CL3 [Choose subtree] If N is not at the desired level, let F be the entry in N \r
1182       // whose rectangle FI needs least enlargement to include EI. Resolve\r
1183       // ties by choosing the entry with the rectangle of smaller area.\r
1184       float leastEnlargement = Rectangle.enlargement(n.entriesMinX[0], n.entriesMinY[0], n.entriesMaxX[0], n.entriesMaxY[0],\r
1185                                                      minX, minY, maxX, maxY);\r
1186       int index = 0; // index of rectangle in subtree\r
1187       for (int i = 1; i < n.entryCount; i++) {\r
1188         float tempMinX = n.entriesMinX[i];\r
1189         float tempMinY = n.entriesMinY[i];\r
1190         float tempMaxX = n.entriesMaxX[i];\r
1191         float tempMaxY = n.entriesMaxY[i];\r
1192         float tempEnlargement = Rectangle.enlargement(tempMinX, tempMinY, tempMaxX, tempMaxY, \r
1193                                                       minX, minY, maxX, maxY);\r
1194         if ((tempEnlargement < leastEnlargement) ||\r
1195             ((tempEnlargement == leastEnlargement) && \r
1196              (Rectangle.area(tempMinX, tempMinY, tempMaxX, tempMaxY) < \r
1197               Rectangle.area(n.entriesMinX[index], n.entriesMinY[index], n.entriesMaxX[index], n.entriesMaxY[index])))) {\r
1198           index = i;\r
1199           leastEnlargement = tempEnlargement;\r
1200         }\r
1201       }\r
1202       \r
1203       parents.push(n.nodeId);\r
1204       parentsEntry.push(index);\r
1205     \r
1206       // CL4 [Descend until a leaf is reached] Set N to be the child node \r
1207       // pointed to by Fp and repeat from CL2\r
1208       n = getNode(n.ids[index]);\r
1209     }\r
1210   }\r
1211   \r
1212   /**\r
1213    * Ascend from a leaf node L to the root, adjusting covering rectangles and\r
1214    * propagating node splits as necessary.\r
1215    */\r
1216   private Node adjustTree(Node n, Node nn) {\r
1217     // AT1 [Initialize] Set N=L. If L was split previously, set NN to be \r
1218     // the resulting second node.\r
1219     \r
1220     // AT2 [Check if done] If N is the root, stop\r
1221     while (n.level != treeHeight) {\r
1222     \r
1223       // AT3 [Adjust covering rectangle in parent entry] Let P be the parent \r
1224       // node of N, and let En be N's entry in P. Adjust EnI so that it tightly\r
1225       // encloses all entry rectangles in N.\r
1226       Node parent = getNode(parents.pop());\r
1227       int entry = parentsEntry.pop(); \r
1228       \r
1229       if (parent.ids[entry] != n.nodeId) {\r
1230         log.error("Error: entry " + entry + " in node " + \r
1231              parent.nodeId + " should point to node " + \r
1232              n.nodeId + "; actually points to node " + parent.ids[entry]);\r
1233       }\r
1234    \r
1235       if (parent.entriesMinX[entry] != n.mbrMinX ||\r
1236           parent.entriesMinY[entry] != n.mbrMinY ||\r
1237           parent.entriesMaxX[entry] != n.mbrMaxX ||\r
1238           parent.entriesMaxY[entry] != n.mbrMaxY) {\r
1239    \r
1240         parent.entriesMinX[entry] = n.mbrMinX;\r
1241         parent.entriesMinY[entry] = n.mbrMinY;\r
1242         parent.entriesMaxX[entry] = n.mbrMaxX;\r
1243         parent.entriesMaxY[entry] = n.mbrMaxY;\r
1244 \r
1245         parent.recalculateMBR();\r
1246       }\r
1247       \r
1248       // AT4 [Propagate node split upward] If N has a partner NN resulting from \r
1249       // an earlier split, create a new entry Enn with Ennp pointing to NN and \r
1250       // Enni enclosing all rectangles in NN. Add Enn to P if there is room. \r
1251       // Otherwise, invoke splitNode to produce P and PP containing Enn and\r
1252       // all P's old entries.\r
1253       Node newNode = null;\r
1254       if (nn != null) {\r
1255         if (parent.entryCount < maxNodeEntries) {\r
1256           parent.addEntry(nn.mbrMinX, nn.mbrMinY, nn.mbrMaxX, nn.mbrMaxY, nn.nodeId);\r
1257         } else {\r
1258           newNode = splitNode(parent, nn.mbrMinX, nn.mbrMinY, nn.mbrMaxX, nn.mbrMaxY, nn.nodeId);\r
1259         }\r
1260       }\r
1261       \r
1262       // AT5 [Move up to next level] Set N = P and set NN = PP if a split \r
1263       // occurred. Repeat from AT2\r
1264       n = parent;\r
1265       nn = newNode;\r
1266       \r
1267       parent = null;\r
1268       newNode = null;\r
1269     }\r
1270     \r
1271     return nn;\r
1272   }\r
1273   \r
1274   \r
1275   /**\r
1276    * Check the consistency of the tree.\r
1277    * \r
1278    * @return false if an inconsistency is detected, true otherwise.\r
1279    */\r
1280   public boolean checkConsistency() {\r
1281     return checkConsistency(rootNodeId, treeHeight, null);\r
1282   }\r
1283   \r
1284   private boolean checkConsistency(int nodeId, int expectedLevel, Rectangle expectedMBR) {\r
1285     // go through the tree, and check that the internal data structures of \r
1286     // the tree are not corrupted.        \r
1287     Node n = getNode(nodeId);\r
1288     \r
1289     if (n == null) {\r
1290       log.error("Error: Could not read node " + nodeId);\r
1291       return false;\r
1292     }\r
1293     \r
1294     // if tree is empty, then there should be exactly one node, at level 1\r
1295     // TODO: also check the MBR is as for a new node\r
1296     if (nodeId == rootNodeId && size() == 0) {\r
1297       if (n.level != 1) {\r
1298         log.error("Error: tree is empty but root node is not at level 1");\r
1299         return false;\r
1300       }\r
1301     }\r
1302     \r
1303     if (n.level != expectedLevel) {\r
1304       log.error("Error: Node " + nodeId + ", expected level " + expectedLevel + ", actual level " + n.level);\r
1305       return false;\r
1306     }\r
1307     \r
1308     Rectangle calculatedMBR = calculateMBR(n);\r
1309     Rectangle actualMBR = new Rectangle();\r
1310     actualMBR.minX = n.mbrMinX;\r
1311     actualMBR.minY = n.mbrMinY;\r
1312     actualMBR.maxX = n.mbrMaxX;\r
1313     actualMBR.maxY = n.mbrMaxY;\r
1314     if (!actualMBR.equals(calculatedMBR)) {\r
1315       log.error("Error: Node " + nodeId + ", calculated MBR does not equal stored MBR");\r
1316       if (actualMBR.minX != n.mbrMinX) log.error("  actualMinX=" + actualMBR.minX + ", calc=" + calculatedMBR.minX);\r
1317       if (actualMBR.minY != n.mbrMinY) log.error("  actualMinY=" + actualMBR.minY + ", calc=" + calculatedMBR.minY);\r
1318       if (actualMBR.maxX != n.mbrMaxX) log.error("  actualMaxX=" + actualMBR.maxX + ", calc=" + calculatedMBR.maxX);\r
1319       if (actualMBR.maxY != n.mbrMaxY) log.error("  actualMaxY=" + actualMBR.maxY + ", calc=" + calculatedMBR.maxY);\r
1320       return false;\r
1321     }\r
1322     \r
1323     if (expectedMBR != null && !actualMBR.equals(expectedMBR)) {\r
1324       log.error("Error: Node " + nodeId + ", expected MBR (from parent) does not equal stored MBR");\r
1325       return false;\r
1326     }\r
1327     \r
1328     // Check for corruption where a parent entry is the same object as the child MBR\r
1329     if (expectedMBR != null && actualMBR.sameObject(expectedMBR)) {\r
1330       log.error("Error: Node " + nodeId + " MBR using same rectangle object as parent's entry");\r
1331       return false;\r
1332     }\r
1333     \r
1334     for (int i = 0; i < n.entryCount; i++) {\r
1335       if (n.ids[i] == -1) {\r
1336         log.error("Error: Node " + nodeId + ", Entry " + i + " is null");\r
1337         return false;\r
1338       }     \r
1339       \r
1340       if (n.level > 1) { // if not a leaf\r
1341         if (!checkConsistency(n.ids[i], n.level - 1, new Rectangle(n.entriesMinX[i], n.entriesMinY[i], n.entriesMaxX[i], n.entriesMaxY[i]))) {\r
1342           return false;\r
1343         }\r
1344       }   \r
1345     }\r
1346     return true;\r
1347   }\r
1348   \r
1349   /**\r
1350    * Given a node object, calculate the node MBR from it's entries.\r
1351    * Used in consistency checking\r
1352    */\r
1353   private Rectangle calculateMBR(Node n) {\r
1354     Rectangle mbr = new Rectangle();\r
1355    \r
1356     for (int i = 0; i < n.entryCount; i++) {\r
1357       if (n.entriesMinX[i] < mbr.minX) mbr.minX = n.entriesMinX[i];\r
1358       if (n.entriesMinY[i] < mbr.minY) mbr.minY = n.entriesMinY[i];\r
1359       if (n.entriesMaxX[i] > mbr.maxX) mbr.maxX = n.entriesMaxX[i];\r
1360       if (n.entriesMaxY[i] > mbr.maxY) mbr.maxY = n.entriesMaxY[i];\r
1361     }\r
1362     return mbr; \r
1363   }\r
1364 }\r