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 com.vividsolutions.jts.geom.Envelope;
11
12 import nl.tudelft.simulation.language.Throw;
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34 public class OTS2DSet implements Set<OTSShape>, Serializable
35 {
36
37 private static final long serialVersionUID = 20170400L;
38
39
40 private final Set<OTSShape> allShapes = new HashSet<OTSShape>();
41
42
43 private final double minimumCellSize;
44
45
46 private QuadTreeNode quadTree;
47
48
49
50
51
52
53
54
55
56 public OTS2DSet(final Rectangle2D boundingBox, final double minimumCellSize) throws OTSGeometryException
57 {
58 Throw.when(null == boundingBox, NullPointerException.class, "The boundingBox may not be null");
59 Throw.when(boundingBox.getWidth() <= 0 || boundingBox.getHeight() <= 0, OTSGeometryException.class,
60 "The boundingBox must have nonzero surface (got %s", boundingBox);
61 Throw.when(minimumCellSize <= 0, OTSGeometryException.class, "The minimumCellSize must be > 0 (got %f)",
62 minimumCellSize);
63 this.quadTree = new QuadTreeNode(boundingBox);
64 this.minimumCellSize = minimumCellSize;
65 }
66
67
68 @Override
69 public final int size()
70 {
71 return this.allShapes.size();
72 }
73
74
75 @Override
76 public final boolean isEmpty()
77 {
78 return this.allShapes.isEmpty();
79 }
80
81
82 @Override
83 public final boolean contains(final Object o)
84 {
85 return this.allShapes.contains(o);
86 }
87
88
89 @Override
90 public final Iterator<OTSShape> iterator()
91 {
92 return new QuadTreeIterator();
93 }
94
95
96 @Override
97 public final Object[] toArray()
98 {
99 return this.allShapes.toArray();
100 }
101
102
103 @Override
104 public final <T> T[] toArray(final T[] a)
105 {
106 return this.allShapes.toArray(a);
107 }
108
109
110 @Override
111 public final boolean add(final OTSShape e)
112 {
113 if (!this.quadTree.intersects(e))
114 {
115 return false;
116 }
117 if (this.allShapes.contains(e))
118 {
119 return false;
120 }
121 if (!this.quadTree.add(e))
122 {
123 System.err.println("add: ERROR object could not be added to the quad tree");
124 }
125 return this.allShapes.add(e);
126 }
127
128
129 @Override
130 public final boolean remove(final Object o)
131 {
132 if (!this.allShapes.remove(o))
133 {
134 return false;
135 }
136 if (!this.quadTree.remove((OTSShape) o))
137 {
138 System.err.println("remove: ERROR object could not be removed from the quad tree");
139 }
140 return true;
141 }
142
143
144 @Override
145 public final boolean containsAll(final Collection<?> c)
146 {
147 for (Object o : c)
148 {
149 if (!contains(o))
150 {
151 return false;
152 }
153 }
154 return true;
155 }
156
157
158 @Override
159 public final boolean addAll(final Collection<? extends OTSShape> c)
160 {
161 boolean result = false;
162 for (OTSShape s : c)
163 {
164 if (add(s))
165 {
166 result = true;
167 }
168 }
169 return result;
170 }
171
172
173 @Override
174 public final boolean retainAll(final Collection<?> c)
175 {
176 boolean result = false;
177 for (Iterator<OTSShape> it = iterator(); it.hasNext();)
178 {
179 OTSShape shape = it.next();
180 if (!c.contains(shape))
181 {
182 it.remove();
183 result = true;
184 }
185 }
186 return result;
187 }
188
189
190 @Override
191 public final boolean removeAll(final Collection<?> c)
192 {
193 boolean result = false;
194 for (Iterator<OTSShape> it = iterator(); it.hasNext();)
195 {
196 OTSShape shape = it.next();
197 if (c.contains(shape))
198 {
199 it.remove();
200 result = true;
201 }
202 }
203 return result;
204 }
205
206
207 @Override
208 public final void clear()
209 {
210 this.quadTree.clear();
211 this.allShapes.clear();
212 }
213
214
215
216
217
218
219 public final Set<OTSShape> intersectingShapes(final Rectangle2D rectangle)
220 {
221 return this.quadTree.intersectingShapes(rectangle);
222 }
223
224
225
226
227
228
229 final String toString(final int recursionDepth)
230 {
231 return "OTS2DSet [contains " + size() + (1 == this.allShapes.size() ? "shape" : "shapes") + ", minimumCellSize="
232 + this.minimumCellSize + ", quadTree=" + this.quadTree.toString(recursionDepth) + "]";
233 }
234
235
236 @Override
237 public final String toString()
238 {
239 return toString(0);
240 }
241
242
243
244
245
246
247 public final Set<OTSShape> intersectingShapes(final OTSShape shape)
248 {
249 Envelope envelope = shape.getEnvelope();
250 Set<OTSShape> result = intersectingShapes(
251 new Rectangle2D.Double(envelope.getMinX(), envelope.getMinY(), envelope.getWidth(), envelope.getHeight()));
252 for (Iterator<OTSShape> it = result.iterator(); it.hasNext();)
253 {
254 if (!it.next().intersects(shape))
255 {
256 it.remove();
257 }
258 }
259 return result;
260 }
261
262
263
264
265
266
267 public final String toStringGraphic(final int recursionDepth)
268 {
269 return this.quadTree.toStringGraphic(recursionDepth);
270 }
271
272
273
274
275 class QuadTreeIterator implements Iterator<OTSShape>, Serializable
276 {
277
278 private static final long serialVersionUID = 20170400L;
279
280
281 @SuppressWarnings("synthetic-access")
282 private final Iterator<OTSShape> theIterator = OTS2DSet.this.allShapes.iterator();
283
284
285 private OTSShape lastResult = null;
286
287
288 @Override
289 public final boolean hasNext()
290 {
291 return this.theIterator.hasNext();
292 }
293
294
295 @Override
296 public final OTSShape next()
297 {
298 this.lastResult = this.theIterator.next();
299 return this.lastResult;
300 }
301
302
303 @SuppressWarnings("synthetic-access")
304 @Override
305 public final void remove()
306 {
307 this.theIterator.remove();
308 if (!OTS2DSet.this.quadTree.remove(this.lastResult))
309 {
310 System.err.println("iterator.remove: ERROR: could not remove " + this.lastResult + " from the quad tree");
311 }
312 }
313
314
315 @Override
316 public String toString()
317 {
318 return "QuadTreeIterator [theIterator=" + this.theIterator + ", lastResult=" + this.lastResult + "]";
319 }
320
321 }
322
323
324
325
326 class QuadTreeNode implements Serializable
327 {
328
329 private static final long serialVersionUID = 20170400L;
330
331
332 private Set<OTSShape> shapes = new HashSet<OTSShape>();
333
334
335 private final Rectangle2D boundingBox;
336
337
338 private final OTSShape boundingShape;
339
340
341
342
343
344 private final QuadTreeNode[] leaves;
345
346
347
348
349
350 @SuppressWarnings("synthetic-access")
351 QuadTreeNode(final Rectangle2D boundingBox)
352 {
353 this.boundingBox = boundingBox;
354 this.boundingShape = rectangleShape(boundingBox);
355 this.leaves = boundingBox.getWidth() > OTS2DSet.this.minimumCellSize
356 || boundingBox.getHeight() > OTS2DSet.this.minimumCellSize ? new QuadTreeNode[4] : null;
357 }
358
359
360
361
362
363
364 public Set<OTSShape> intersectingShapes(final Rectangle2D rectangle)
365 {
366 Set<OTSShape> result = new HashSet<OTSShape>();
367 if (!this.boundingBox.intersects(rectangle))
368 {
369 return result;
370 }
371 if (null == this.leaves)
372 {
373 return result;
374 }
375 for (QuadTreeNode leaf : this.leaves)
376 {
377 if (null != leaf && leaf.intersects(rectangle))
378 {
379 result.addAll(leaf.intersectingShapes(rectangle));
380 }
381 }
382 for (OTSShape shape : this.shapes)
383 {
384 OTSShape rectangleShape = rectangleShape(rectangle);
385 if (rectangleShape.intersects(shape))
386 {
387 result.add(shape);
388 }
389 }
390 return result;
391 }
392
393
394
395
396
397
398 private boolean intersects(final Rectangle2D rectangle)
399 {
400 return this.boundingBox.intersects(rectangle);
401 }
402
403
404
405
406 public void clear()
407 {
408 this.shapes.clear();
409 for (int index = 0; index < this.leaves.length; index++)
410 {
411 this.leaves[index] = null;
412 }
413 }
414
415
416
417
418
419
420 public boolean remove(final OTSShape shape)
421 {
422 if (!this.boundingShape.intersects(shape))
423 {
424 return false;
425 }
426 for (OTSShape s : this.shapes)
427 {
428 if (shape.equals(s))
429 {
430 this.shapes.remove(shape);
431 return true;
432 }
433 }
434 boolean result = false;
435 for (int index = 0; index < this.leaves.length; index++)
436 {
437 QuadTreeNode qtn = this.leaves[index];
438 if (null != qtn)
439 {
440 if (qtn.remove(shape))
441 {
442 result = true;
443 if (qtn.isEmpty())
444 {
445 this.leaves[index] = null;
446 }
447 }
448 }
449 }
450 return result;
451 }
452
453
454
455
456
457 private boolean isEmpty()
458 {
459 if (!this.shapes.isEmpty())
460 {
461 return false;
462 }
463 if (null == this.leaves)
464 {
465 return true;
466 }
467 for (QuadTreeNode qtn : this.leaves)
468 {
469 if (null != qtn)
470 {
471 return false;
472 }
473 }
474 return true;
475 }
476
477
478
479
480
481
482 public boolean intersects(final OTSShape shape)
483 {
484 return this.boundingShape.intersects(shape);
485 }
486
487
488
489
490
491
492 private OTSShape rectangleShape(final Rectangle2D rectangle)
493 {
494 double left = rectangle.getMinX();
495 double bottom = rectangle.getMinY();
496 double right = rectangle.getMaxX();
497 double top = rectangle.getMaxY();
498 try
499 {
500 return new OTSShape(new OTSPoint3D(left, bottom), new OTSPoint3D(right, bottom), new OTSPoint3D(right, top),
501 new OTSPoint3D(left, top));
502 }
503 catch (OTSGeometryException exception)
504 {
505 exception.printStackTrace();
506 return null;
507 }
508 }
509
510
511
512
513
514
515 public final boolean add(final OTSShape shape)
516 {
517 if (!this.boundingShape.intersects(shape))
518 {
519 return false;
520 }
521 if ((null == this.leaves) || shape.contains(this.boundingBox))
522 {
523
524 return this.shapes.add(shape);
525 }
526
527 boolean result = false;
528 for (int index = 0; index < this.leaves.length; index++)
529 {
530 if (null == this.leaves[index])
531 {
532 double subWidth = this.boundingBox.getWidth() / 2;
533 double subHeight = this.boundingBox.getHeight() / 2;
534 if (0 == subWidth)
535 {
536
537 subWidth = this.boundingBox.getWidth();
538 }
539 if (0 == subHeight)
540 {
541
542 subHeight = this.boundingBox.getHeight();
543 }
544 double left = this.boundingBox.getMinX();
545 if (0 != index / 2)
546 {
547 left += subWidth;
548 }
549 double bottom = this.boundingBox.getMinY();
550 if (0 != index % 2)
551 {
552 bottom += subHeight;
553 }
554 Rectangle2D subBox = new Rectangle2D.Double(left, bottom, subWidth, subHeight);
555 if (rectangleShape(subBox).intersects(shape))
556 {
557
558 this.leaves[index] = new QuadTreeNode(subBox);
559 if (this.leaves[index].add(shape))
560 {
561 result = true;
562 }
563 else
564 {
565 throw new Error("Cannot happen: new QuadTreeNode refused to add shape that intersects it");
566 }
567 }
568 }
569 else
570 {
571
572 if (this.leaves[index].add(shape))
573 {
574 result = true;
575 }
576 }
577 }
578 return result;
579 }
580
581
582
583
584
585
586
587 private String printLeaf(final int recursionDepth, final int index)
588 {
589 QuadTreeNode leaf = this.leaves[index];
590 if (null == leaf)
591 {
592 return "null";
593 }
594 if (recursionDepth > 0)
595 {
596 return leaf.toString(recursionDepth - 1);
597 }
598 int leafSize = leaf.shapes.size();
599 return leafSize + " shape" + (1 == leafSize ? "" : "s");
600 }
601
602
603
604
605
606
607 final String toString(final int recursionDepth)
608 {
609 return "QuadTreeNode [" + this.shapes.size() + ", bounds=[LB: " + this.boundingBox.getMinX() + ","
610 + this.boundingBox.getMinY() + ", RT: " + this.boundingBox.getMaxX() + "," + this.boundingBox.getMaxY()
611 + "], " + subNodes(recursionDepth) + ", local " + this.shapes.size()
612 + (1 == this.shapes.size() ? " shape" : " shapes") + "]";
613 }
614
615
616
617
618
619
620 private String subNodes(final int recursionDepth)
621 {
622 if (null == this.leaves)
623 {
624 return "cannot have leaves";
625 }
626 return "leaves=[LB: " + printLeaf(recursionDepth, 0) + ", RB: " + printLeaf(recursionDepth, 1) + ", LT: "
627 + printLeaf(recursionDepth, 2) + ", RT: " + printLeaf(recursionDepth, 3) + "]";
628 }
629
630
631 @Override
632 public final String toString()
633 {
634 return toString(0);
635 }
636
637
638
639
640
641
642
643 private String repeat(final int count, final String string)
644 {
645 StringBuilder result = new StringBuilder();
646 for (int i = 0; i < count; i++)
647 {
648 result.append(string);
649 }
650 return result.toString();
651 }
652
653
654 private static final String VLINE = "|";
655
656
657 private static final String HLINE = "-";
658
659
660 private static final String SPACE = " ";
661
662
663 private static final int NUMBERSIZE = 6;
664
665
666
667
668
669
670
671
672 private String subStringGraphic(final QuadTreeNode qtn, final int recursionDepth)
673 {
674 StringBuffer result = new StringBuffer();
675 if (0 == recursionDepth)
676 {
677 if (null == qtn)
678 {
679 result.append(repeat(NUMBERSIZE, SPACE));
680 }
681 else
682 {
683 String numberBuf = String.format("%d", size());
684 int spare = NUMBERSIZE - numberBuf.length();
685 int filled = 0;
686 while (filled < spare / 2)
687 {
688 result.append(SPACE);
689 filled++;
690 }
691 result.append(numberBuf);
692 while (filled < spare)
693 {
694 result.append(SPACE);
695 filled++;
696 }
697 result.append("\n");
698 return result.toString();
699 }
700 }
701 else
702 {
703 String[] left = subStringGraphic(null == qtn || null == qtn.leaves ? null : qtn.leaves[1], recursionDepth - 1)
704 .split("\\n");
705 String[] right = subStringGraphic(null == qtn || null == qtn.leaves ? null : qtn.leaves[3], recursionDepth - 1)
706 .split("\\n");
707 String horizontalLine = null;
708 for (int i = 0; i < left.length; i++)
709 {
710 if (0 == i)
711 {
712 StringBuilder line = new StringBuilder();
713 int width = left[0].length() + 1 + right[0].length();
714 if (null == qtn)
715 {
716 line.append(repeat(width, SPACE));
717 }
718 else
719 {
720 String numberBuf = String.format("%d", qtn.shapes.size());
721 int spare = width - numberBuf.length();
722 line.append(repeat(spare / 2, HLINE));
723 line.append(numberBuf);
724 line.append(repeat(spare - spare / 2, HLINE));
725 }
726 horizontalLine = line.toString();
727 }
728 result.append(left[i]);
729 result.append(null == qtn ? SPACE : VLINE);
730 result.append(right[i]);
731 result.append("\n");
732 }
733 result.append(horizontalLine);
734 result.append("\n");
735 left = subStringGraphic(null == qtn || null == qtn.leaves ? null : qtn.leaves[0], recursionDepth - 1)
736 .split("\\n");
737 right = subStringGraphic(null == qtn || null == qtn.leaves ? null : qtn.leaves[2], recursionDepth - 1)
738 .split("\\n");
739 for (int i = 0; i < left.length; i++)
740 {
741 result.append(left[i]);
742 result.append(null == qtn ? SPACE : VLINE);
743 result.append(right[i]);
744 result.append("\n");
745 }
746 result.append("\n");
747 }
748 return result.toString();
749 }
750
751
752
753
754
755
756 public final String toStringGraphic(final int recursionDepth)
757 {
758 return subStringGraphic(this, recursionDepth);
759 }
760
761 }
762
763 }