View Javadoc
1   package org.opentrafficsim.core.geometry;
2   
3   import java.awt.geom.Rectangle2D;
4   import java.io.Serializable;
5   import java.util.Collection;
6   import java.util.HashSet;
7   import java.util.Iterator;
8   import java.util.Set;
9   
10  import org.opentrafficsim.core.logger.SimLogger;
11  
12  import com.vividsolutions.jts.geom.Envelope;
13  
14  import nl.tudelft.simulation.language.Throw;
15  
16  /**
17   * Set of OTSShape objects and provides methods for fast selection of those objects that intersect an OTSShape. <br>
18   * An OTS2DSet internally stores the OTSShapes in a quad tree. At time of construction the minimum cell size is defined. Node
19   * expansion is never performed on nodes that are smaller than this limit. <br>
20   * Each node (even the non-leaf nodes) store a set of OTSShape. Non-leaf nodes locally store those shapes that completely cover
21   * the rectangular area of the node. Such shapes are <b>not</b> also stored in leaf nodes below that node. OTSShapes that
22   * partially cover a non-leaf node are stored in each of the leaf nodes below that node that those OTSShapes (partially) cover.
23   * Leaf nodes that cannot be expanded (because they are too small) also store all OTSShapes that partially cover the area of the
24   * node. <br>
25   * If removal of an OTSShape objects results in a leaf becoming empty, that leaf is removed from its parent (which may then
26   * itself become empty and removed in turn).
27   * <p>
28   * Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
29   * BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
30   * <p>
31   * @version $Revision$, $LastChangedDate$, by $Author$, initial version Jun 20, 2016 <br>
32   * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
33   * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
34   * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
35   */
36  public class OTS2DSet implements Set<OTSShape>, Serializable
37  {
38      /** */
39      private static final long serialVersionUID = 20170400L;
40  
41      /** Set of all shapes used for iterators, etc. */
42      private final Set<OTSShape> allShapes = new HashSet<OTSShape>();
43  
44      /** How fine will this quad tree divide. This one is copied to each sub-node which is somewhat inefficient. */
45      private final double minimumCellSize;
46  
47      /** Spatial storage for the OTSShapes. */
48      private QuadTreeNode quadTree;
49  
50      /**
51       * Construct an empty OTS2DSet for a rectangular region. Objects that do not intersect this region will never be stored in
52       * this OTS2DSet. (Trying to add such an OTSShape is <b>not</b> an error; the <code>add</code> method will return false,
53       * indicating that the set has not been modified.)
54       * @param boundingBox Rectangle2D; the region
55       * @param minimumCellSize double; resolution of the underlying quad tree
56       * @throws OTSGeometryException when the bounding box covers no surface
57       */
58      public OTS2DSet(final Rectangle2D boundingBox, final double minimumCellSize) throws OTSGeometryException
59      {
60          Throw.when(null == boundingBox, NullPointerException.class, "The boundingBox may not be null");
61          Throw.when(boundingBox.getWidth() <= 0 || boundingBox.getHeight() <= 0, OTSGeometryException.class,
62                  "The boundingBox must have nonzero surface (got %s", boundingBox);
63          Throw.when(minimumCellSize <= 0, OTSGeometryException.class, "The minimumCellSize must be > 0 (got %f)",
64                  minimumCellSize);
65          this.quadTree = new QuadTreeNode(boundingBox);
66          this.minimumCellSize = minimumCellSize;
67      }
68  
69      /** {@inheritDoc} */
70      @Override
71      public final int size()
72      {
73          return this.allShapes.size();
74      }
75  
76      /** {@inheritDoc} */
77      @Override
78      public final boolean isEmpty()
79      {
80          return this.allShapes.isEmpty();
81      }
82  
83      /** {@inheritDoc} */
84      @Override
85      public final boolean contains(final Object o)
86      {
87          return this.allShapes.contains(o);
88      }
89  
90      /** {@inheritDoc} */
91      @Override
92      public final Iterator<OTSShape> iterator()
93      {
94          return new QuadTreeIterator();
95      }
96  
97      /** {@inheritDoc} */
98      @Override
99      public final Object[] toArray()
100     {
101         return this.allShapes.toArray();
102     }
103 
104     /** {@inheritDoc} */
105     @Override
106     public final <T> T[] toArray(final T[] a)
107     {
108         return this.allShapes.toArray(a);
109     }
110 
111     /** {@inheritDoc} */
112     @Override
113     public final boolean add(final OTSShape e)
114     {
115         if (!this.quadTree.intersects(e))
116         {
117             return false;
118         }
119         if (this.allShapes.contains(e))
120         {
121             return false;
122         }
123         if (!this.quadTree.add(e))
124         {
125             SimLogger.always().error("add: ERROR object could not be added to the quad tree");
126         }
127         return this.allShapes.add(e);
128     }
129 
130     /** {@inheritDoc} */
131     @Override
132     public final boolean remove(final Object o)
133     {
134         if (!this.allShapes.remove(o))
135         {
136             return false;
137         }
138         if (!this.quadTree.remove((OTSShape) o))
139         {
140             SimLogger.always().error("remove: ERROR object could not be removed from the quad tree");
141         }
142         return true;
143     }
144 
145     /** {@inheritDoc} */
146     @Override
147     public final boolean containsAll(final Collection<?> c)
148     {
149         for (Object o : c)
150         {
151             if (!contains(o))
152             {
153                 return false;
154             }
155         }
156         return true;
157     }
158 
159     /** {@inheritDoc} */
160     @Override
161     public final boolean addAll(final Collection<? extends OTSShape> c)
162     {
163         boolean result = false;
164         for (OTSShape s : c)
165         {
166             if (add(s))
167             {
168                 result = true;
169             }
170         }
171         return result;
172     }
173 
174     /** {@inheritDoc} */
175     @Override
176     public final boolean retainAll(final Collection<?> c)
177     {
178         boolean result = false;
179         for (Iterator<OTSShape> it = iterator(); it.hasNext();)
180         {
181             OTSShape shape = it.next();
182             if (!c.contains(shape))
183             {
184                 it.remove();
185                 result = true;
186             }
187         }
188         return result;
189     }
190 
191     /** {@inheritDoc} */
192     @Override
193     public final boolean removeAll(final Collection<?> c)
194     {
195         boolean result = false;
196         for (Iterator<OTSShape> it = iterator(); it.hasNext();)
197         {
198             OTSShape shape = it.next();
199             if (c.contains(shape))
200             {
201                 it.remove();
202                 result = true;
203             }
204         }
205         return result;
206     }
207 
208     /** {@inheritDoc} */
209     @Override
210     public final void clear()
211     {
212         this.quadTree.clear();
213         this.allShapes.clear();
214     }
215 
216     /**
217      * Return the set of all shapes in this OTS2DSet that intersect the given rectangle.
218      * @param rectangle Rectangle2D; the rectangle
219      * @return Set&lt;OTSShape&gt;; the shapes that intersect the rectangle
220      */
221     public final Set<OTSShape> intersectingShapes(final Rectangle2D rectangle)
222     {
223         return this.quadTree.intersectingShapes(rectangle);
224     }
225 
226     /**
227      * Recursively print this OTS2DSet.
228      * @param recursionDepth int; maximum depth to recurse
229      * @return String
230      */
231     final String toString(final int recursionDepth)
232     {
233         return "OTS2DSet [contains " + size() + (1 == this.allShapes.size() ? "shape" : "shapes") + ", minimumCellSize="
234                 + this.minimumCellSize + ", quadTree=" + this.quadTree.toString(recursionDepth) + "]";
235     }
236 
237     /** {@inheritDoc} */
238     @Override
239     public final String toString()
240     {
241         return toString(0);
242     }
243 
244     /**
245      * Return all OTSShapes in this OTS2DSet that intersect a given OTSShape.
246      * @param shape OTSShape; the given OTSShape
247      * @return Set&lt;OTSShape&gt;; all OTSShapes in this OTS2DSet that intersect <code>shape</code>
248      */
249     public final Set<OTSShape> intersectingShapes(final OTSShape shape)
250     {
251         Envelope envelope = shape.getEnvelope();
252         Set<OTSShape> result = intersectingShapes(
253                 new Rectangle2D.Double(envelope.getMinX(), envelope.getMinY(), envelope.getWidth(), envelope.getHeight()));
254         for (Iterator<OTSShape> it = result.iterator(); it.hasNext();)
255         {
256             if (!it.next().intersects(shape))
257             {
258                 it.remove();
259             }
260         }
261         return result;
262     }
263 
264     /**
265      * Return an ASCII art rendering of this OTS2DSet.
266      * @param recursionDepth int; maximum recursion depth
267      * @return String; a somewhat human readable rendering of this OTS2DSet
268      */
269     public final String toStringGraphic(final int recursionDepth)
270     {
271         return this.quadTree.toStringGraphic(recursionDepth);
272     }
273 
274     /**
275      * Iterator for quad tree. Shall iterate over the local set of shapes and the (up to four) non-null leave nodes.
276      */
277     class QuadTreeIterator implements Iterator<OTSShape>, Serializable
278     {
279         /** */
280         private static final long serialVersionUID = 20170400L;
281 
282         /** Underlying iterator that traverses the allShapes Set. */
283         @SuppressWarnings("synthetic-access")
284         private final Iterator<OTSShape> theIterator = OTS2DSet.this.allShapes.iterator();
285 
286         /** Remember the last returned result so we can remove it when requested. */
287         private OTSShape lastResult = null;
288 
289         /** {@inheritDoc} */
290         @Override
291         public final boolean hasNext()
292         {
293             return this.theIterator.hasNext();
294         }
295 
296         /** {@inheritDoc} */
297         @Override
298         public final OTSShape next()
299         {
300             this.lastResult = this.theIterator.next();
301             return this.lastResult;
302         }
303 
304         /** {@inheritDoc} */
305         @SuppressWarnings("synthetic-access")
306         @Override
307         public final void remove()
308         {
309             this.theIterator.remove();
310             if (!OTS2DSet.this.quadTree.remove(this.lastResult))
311             {
312                 SimLogger.always().error("iterator.remove: ERROR: could not remove {} from the quad tree", this.lastResult);
313             }
314         }
315 
316         /** {@inheritDoc} */
317         @Override
318         public String toString()
319         {
320             return "QuadTreeIterator [theIterator=" + this.theIterator + ", lastResult=" + this.lastResult + "]";
321         }
322 
323     }
324 
325     /**
326      * Spatial-aware storage for a set of OTSShape objects.
327      */
328     class QuadTreeNode implements Serializable
329     {
330         /** */
331         private static final long serialVersionUID = 20170400L;
332 
333         /** The OTSShapes stored at this node. */
334         private Set<OTSShape> shapes = new HashSet<OTSShape>();
335 
336         /** The bounding box of this QuadTreeNode. */
337         private final Rectangle2D boundingBox;
338 
339         /** The bounding box of this QuadTreeNode as an OTSShape. */
340         private final OTSShape boundingShape;
341 
342         /**
343          * The four leaves of this node in the quad tree. An empty sub tree may be represented by null. If this field is
344          * initialized to null; this node may not expand by adding sub-nodes.
345          */
346         private final QuadTreeNode[] leaves;
347 
348         /**
349          * Construct a new QuadTreeNode.
350          * @param boundingBox Rectangle2D; the bounding box of the area of the new QuadTreeNode
351          */
352         @SuppressWarnings("synthetic-access")
353         QuadTreeNode(final Rectangle2D boundingBox)
354         {
355             this.boundingBox = boundingBox;
356             this.boundingShape = rectangleShape(boundingBox);
357             this.leaves = boundingBox.getWidth() > OTS2DSet.this.minimumCellSize
358                     || boundingBox.getHeight() > OTS2DSet.this.minimumCellSize ? new QuadTreeNode[4] : null;
359         }
360 
361         /**
362          * Return a Set containing all OTSShapes in this QuadTreeNode that intersect a rectangular area.
363          * @param rectangle Rectangle2D; the area
364          * @return Set&lt;OTSShape&gt;; the set
365          */
366         public Set<OTSShape> intersectingShapes(final Rectangle2D rectangle)
367         {
368             Set<OTSShape> result = new HashSet<OTSShape>();
369             if (!this.boundingBox.intersects(rectangle))
370             {
371                 return result;
372             }
373             if (null == this.leaves)
374             {
375                 return result;
376             }
377             for (QuadTreeNode leaf : this.leaves)
378             {
379                 if (null != leaf && leaf.intersects(rectangle))
380                 {
381                     result.addAll(leaf.intersectingShapes(rectangle));
382                 }
383             }
384             for (OTSShape shape : this.shapes)
385             {
386                 OTSShape rectangleShape = rectangleShape(rectangle);
387                 if (rectangleShape.intersects(shape))
388                 {
389                     result.add(shape);
390                 }
391             }
392             return result;
393         }
394 
395         /**
396          * Test if this QuadTreeNode intersects a rectangular area.
397          * @param rectangle Rectangle2D; the rectangular area
398          * @return boolean; true if the rectangular area intersects this QuadTreeNode; false otherwise
399          */
400         private boolean intersects(final Rectangle2D rectangle)
401         {
402             return this.boundingBox.intersects(rectangle);
403         }
404 
405         /**
406          * Remove all OTSShapes from this QuadTreeNode and cut off all leaves.
407          */
408         public void clear()
409         {
410             this.shapes.clear();
411             for (int index = 0; index < this.leaves.length; index++)
412             {
413                 this.leaves[index] = null;
414             }
415         }
416 
417         /**
418          * Remove an OTSShape from this QuadTreeNode.
419          * @param shape OTSShape; the shape that must be removed.
420          * @return boolean; true if this node (or a sub-node) was altered; false otherwise
421          */
422         public boolean remove(final OTSShape shape)
423         {
424             if (!this.boundingShape.intersects(shape))
425             {
426                 return false;
427             }
428             for (OTSShape s : this.shapes)
429             {
430                 if (shape.equals(s))
431                 {
432                     this.shapes.remove(shape);
433                     return true;
434                 }
435             }
436             boolean result = false;
437             for (int index = 0; index < this.leaves.length; index++)
438             {
439                 QuadTreeNode qtn = this.leaves[index];
440                 if (null != qtn)
441                 {
442                     if (qtn.remove(shape))
443                     {
444                         result = true;
445                         if (qtn.isEmpty())
446                         {
447                             this.leaves[index] = null; // Cut off empty leaf node
448                         }
449                     }
450                 }
451             }
452             return result;
453         }
454 
455         /**
456          * Check if this QuadTreeNode is empty.
457          * @return boolean; true if this QuadTreeNode is empty
458          */
459         private boolean isEmpty()
460         {
461             if (!this.shapes.isEmpty())
462             {
463                 return false;
464             }
465             if (null == this.leaves)
466             {
467                 return true;
468             }
469             for (QuadTreeNode qtn : this.leaves)
470             {
471                 if (null != qtn)
472                 {
473                     return false;
474                 }
475             }
476             return true;
477         }
478 
479         /**
480          * Test if the area of this QuadTree intersects an OTSShape.
481          * @param shape OTSShape; the shape
482          * @return boolean; true if the area of this QuadTree intersects the shape; false otherwise
483          */
484         public boolean intersects(final OTSShape shape)
485         {
486             return this.boundingShape.intersects(shape);
487         }
488 
489         /**
490          * Construct a OTSShape from a Rectangle2D.
491          * @param rectangle Rectangle3D; the rectangle
492          * @return OTSShape; a new OTSShape
493          */
494         private OTSShape rectangleShape(final Rectangle2D rectangle)
495         {
496             double left = rectangle.getMinX();
497             double bottom = rectangle.getMinY();
498             double right = rectangle.getMaxX();
499             double top = rectangle.getMaxY();
500             try
501             {
502                 return new OTSShape(new OTSPoint3D(left, bottom), new OTSPoint3D(right, bottom), new OTSPoint3D(right, top),
503                         new OTSPoint3D(left, top));
504             }
505             catch (OTSGeometryException exception)
506             {
507                 SimLogger.always().error(exception);
508                 return null;
509             }
510         }
511 
512         /**
513          * Add an OTSShape to this QuadTreeNode.
514          * @param shape OTSShape; the shape
515          * @return boolean; true if this QuadTreeNode changed as a result of this operation
516          */
517         public final boolean add(final OTSShape shape)
518         {
519             if (!this.boundingShape.intersects(shape))
520             {
521                 return false;
522             }
523             if ((null == this.leaves) || shape.contains(this.boundingBox))
524             {
525                 // shape belongs in the set of shapes of this node.
526                 return this.shapes.add(shape);
527             }
528             // This node may have leaves and shape does not entirely contain this node. Add shape to all applicable leaves.
529             boolean result = false;
530             for (int index = 0; index < this.leaves.length; index++)
531             {
532                 if (null == this.leaves[index])
533                 {
534                     double subWidth = this.boundingBox.getWidth() / 2;
535                     double subHeight = this.boundingBox.getHeight() / 2;
536                     if (0 == subWidth)
537                     {
538                         // loss of precision; degenerate into a binary tree
539                         subWidth = this.boundingBox.getWidth();
540                     }
541                     if (0 == subHeight)
542                     {
543                         // loss of precision; degenerate into a binary tree
544                         subHeight = this.boundingBox.getHeight();
545                     }
546                     double left = this.boundingBox.getMinX();
547                     if (0 != index / 2)
548                     {
549                         left += subWidth;
550                     }
551                     double bottom = this.boundingBox.getMinY();
552                     if (0 != index % 2)
553                     {
554                         bottom += subHeight;
555                     }
556                     Rectangle2D subBox = new Rectangle2D.Double(left, bottom, subWidth, subHeight);
557                     if (rectangleShape(subBox).intersects(shape))
558                     {
559                         // Expand this node by adding a sub node.
560                         this.leaves[index] = new QuadTreeNode(subBox);
561                         if (this.leaves[index].add(shape))
562                         {
563                             result = true;
564                         }
565                         else
566                         {
567                             throw new Error("Cannot happen: new QuadTreeNode refused to add shape that intersects it");
568                         }
569                     }
570                 }
571                 else
572                 {
573                     // Leaf node already exists. Let the leaf determine if shape should be stored (somewhere) in it.
574                     if (this.leaves[index].add(shape))
575                     {
576                         result = true;
577                     }
578                 }
579             }
580             return result;
581         }
582 
583         /**
584          * Helper function for toString.
585          * @param recursionDepth int; maximum number of levels to print recursively
586          * @param index int; index in leaves
587          * @return String
588          */
589         private String printLeaf(final int recursionDepth, final int index)
590         {
591             QuadTreeNode leaf = this.leaves[index];
592             if (null == leaf)
593             {
594                 return "null";
595             }
596             if (recursionDepth > 0)
597             {
598                 return leaf.toString(recursionDepth - 1);
599             }
600             int leafSize = leaf.shapes.size();
601             return leafSize + " shape" + (1 == leafSize ? "" : "s");
602         }
603 
604         /**
605          * Recursively print this QuadTreeNode.
606          * @param recursionDepth int; maximum depth to recurse
607          * @return String
608          */
609         final String toString(final int recursionDepth)
610         {
611             return "QuadTreeNode [" + this.shapes.size() + ", bounds=[LB: " + this.boundingBox.getMinX() + ","
612                     + this.boundingBox.getMinY() + ", RT: " + this.boundingBox.getMaxX() + "," + this.boundingBox.getMaxY()
613                     + "], " + subNodes(recursionDepth) + ", local " + this.shapes.size()
614                     + (1 == this.shapes.size() ? " shape" : " shapes") + "]";
615         }
616 
617         /**
618          * Print the leaves of this QuadTreeNode.
619          * @param recursionDepth int; maximum depth to recurse
620          * @return String
621          */
622         private String subNodes(final int recursionDepth)
623         {
624             if (null == this.leaves)
625             {
626                 return "cannot have leaves";
627             }
628             return "leaves=[LB: " + printLeaf(recursionDepth, 0) + ", RB: " + printLeaf(recursionDepth, 1) + ", LT: "
629                     + printLeaf(recursionDepth, 2) + ", RT: " + printLeaf(recursionDepth, 3) + "]";
630         }
631 
632         /** {@inheritDoc} */
633         @Override
634         public final String toString()
635         {
636             return toString(0);
637         }
638 
639         /**
640          * Return concatenation of a number of copies of a string.
641          * @param count int; number of copies to concatenate
642          * @param string String; the string to repeat
643          * @return String
644          */
645         private String repeat(final int count, final String string)
646         {
647             StringBuilder result = new StringBuilder();
648             for (int i = 0; i < count; i++)
649             {
650                 result.append(string);
651             }
652             return result.toString();
653         }
654 
655         /** Graphic to draw a vertical line. */
656         private static final String VLINE = "|";
657 
658         /** Graphic to draw a horizontal line. */
659         private static final String HLINE = "-";
660 
661         /** Graphic to draw a space. */
662         private static final String SPACE = " ";
663 
664         /** Number of digits to print. */
665         private static final int NUMBERSIZE = 6;
666 
667         /**
668          * Similar to toStringGraphic, but with QuadTreeNode argument which can be null. <br>
669          * This code is <b>not</b> optimized for performance; the repeated use of String.split is probably expensive.
670          * @param qtn QuadTreeNode; the QuadTreeNode to render. Can be null.
671          * @param recursionDepth int; levels to recurse
672          * @return String
673          */
674         private String subStringGraphic(final QuadTreeNode qtn, final int recursionDepth)
675         {
676             StringBuffer result = new StringBuffer();
677             if (0 == recursionDepth)
678             {
679                 if (null == qtn)
680                 {
681                     result.append(repeat(NUMBERSIZE, SPACE));
682                 }
683                 else
684                 {
685                     String numberBuf = String.format("%d", size());
686                     int spare = NUMBERSIZE - numberBuf.length();
687                     int filled = 0;
688                     while (filled < spare / 2)
689                     {
690                         result.append(SPACE);
691                         filled++;
692                     }
693                     result.append(numberBuf);
694                     while (filled < spare)
695                     {
696                         result.append(SPACE);
697                         filled++;
698                     }
699                     result.append("\n");
700                     return result.toString();
701                 }
702             }
703             else
704             {
705                 String[] left = subStringGraphic(null == qtn || null == qtn.leaves ? null : qtn.leaves[1], recursionDepth - 1)
706                         .split("\\n");
707                 String[] right = subStringGraphic(null == qtn || null == qtn.leaves ? null : qtn.leaves[3], recursionDepth - 1)
708                         .split("\\n");
709                 String horizontalLine = null;
710                 for (int i = 0; i < left.length; i++)
711                 {
712                     if (0 == i)
713                     {
714                         StringBuilder line = new StringBuilder();
715                         int width = left[0].length() + 1 + right[0].length();
716                         if (null == qtn)
717                         {
718                             line.append(repeat(width, SPACE));
719                         }
720                         else
721                         {
722                             String numberBuf = String.format("%d", qtn.shapes.size());
723                             int spare = width - numberBuf.length();
724                             line.append(repeat(spare / 2, HLINE));
725                             line.append(numberBuf);
726                             line.append(repeat(spare - spare / 2, HLINE));
727                         }
728                         horizontalLine = line.toString();
729                     }
730                     result.append(left[i]);
731                     result.append(null == qtn ? SPACE : VLINE);
732                     result.append(right[i]);
733                     result.append("\n");
734                 }
735                 result.append(horizontalLine);
736                 result.append("\n");
737                 left = subStringGraphic(null == qtn || null == qtn.leaves ? null : qtn.leaves[0], recursionDepth - 1)
738                         .split("\\n");
739                 right = subStringGraphic(null == qtn || null == qtn.leaves ? null : qtn.leaves[2], recursionDepth - 1)
740                         .split("\\n");
741                 for (int i = 0; i < left.length; i++)
742                 {
743                     result.append(left[i]);
744                     result.append(null == qtn ? SPACE : VLINE);
745                     result.append(right[i]);
746                     result.append("\n");
747                 }
748                 result.append("\n");
749             }
750             return result.toString();
751         }
752 
753         /**
754          * Return a String depicting this QuadTreeNode.
755          * @param recursionDepth int; levels to recurse
756          * @return String
757          */
758         public final String toStringGraphic(final int recursionDepth)
759         {
760             return subStringGraphic(this, recursionDepth);
761         }
762 
763     }
764 
765 }