1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.apache.commons.geometry.euclidean.twod;
18
19 import java.util.ArrayList;
20 import java.util.Collections;
21 import java.util.List;
22 import java.util.stream.Collectors;
23 import java.util.stream.Stream;
24 import java.util.stream.StreamSupport;
25
26 import org.apache.commons.geometry.core.partitioning.Hyperplane;
27 import org.apache.commons.geometry.core.partitioning.Split;
28 import org.apache.commons.geometry.core.partitioning.bsp.AbstractBSPTree;
29 import org.apache.commons.geometry.core.partitioning.bsp.AbstractPartitionedRegionBuilder;
30 import org.apache.commons.geometry.core.partitioning.bsp.AbstractRegionBSPTree;
31 import org.apache.commons.geometry.core.partitioning.bsp.BSPTreeVisitor;
32 import org.apache.commons.geometry.core.partitioning.bsp.RegionCutBoundary;
33 import org.apache.commons.geometry.euclidean.twod.path.InteriorAngleLinePathConnector;
34 import org.apache.commons.geometry.euclidean.twod.path.LinePath;
35 import org.apache.commons.numbers.core.Precision;
36
37
38
39
40 public final class RegionBSPTree2D extends AbstractRegionBSPTree<Vector2D, RegionBSPTree2D.RegionNode2D>
41 implements BoundarySource2D {
42
43
44 private List<LinePath> boundaryPaths;
45
46
47
48 public RegionBSPTree2D() {
49 this(false);
50 }
51
52
53
54
55
56
57 public RegionBSPTree2D(final boolean full) {
58 super(full);
59 }
60
61
62
63
64
65 public RegionBSPTree2D copy() {
66 final RegionBSPTree2D result = RegionBSPTree2D.empty();
67 result.copy(this);
68
69 return result;
70 }
71
72
73 @Override
74 public Iterable<LineConvexSubset> boundaries() {
75 return createBoundaryIterable(LineConvexSubset.class::cast);
76 }
77
78
79 @Override
80 public Stream<LineConvexSubset> boundaryStream() {
81 return StreamSupport.stream(boundaries().spliterator(), false);
82 }
83
84
85 @Override
86 public List<LineConvexSubset> getBoundaries() {
87 return createBoundaryList(LineConvexSubset.class::cast);
88 }
89
90
91
92
93
94
95 public List<LinePath> getBoundaryPaths() {
96 if (boundaryPaths == null) {
97 boundaryPaths = Collections.unmodifiableList(computeBoundaryPaths());
98 }
99 return boundaryPaths;
100 }
101
102
103
104
105
106 public void add(final ConvexArea area) {
107 union(area.toTree());
108 }
109
110
111
112
113
114
115
116 public List<ConvexArea> toConvex() {
117 final List<ConvexArea> result = new ArrayList<>();
118
119 toConvexRecursive(getRoot(), ConvexArea.full(), result);
120
121 return result;
122 }
123
124
125
126
127
128
129
130
131 private void toConvexRecursive(final RegionNode2D node, final ConvexArea nodeArea,
132 final List<? super ConvexArea> result) {
133 if (node.isLeaf()) {
134
135 if (node.isInside()) {
136 result.add(nodeArea);
137 }
138 } else {
139
140 final Split<ConvexArea> split = nodeArea.split(node.getCutHyperplane());
141
142 toConvexRecursive(node.getMinus(), split.getMinus(), result);
143 toConvexRecursive(node.getPlus(), split.getPlus(), result);
144 }
145 }
146
147
148 @Override
149 public Split<RegionBSPTree2D> split(final Hyperplane<Vector2D> splitter) {
150 return split(splitter, RegionBSPTree2D.empty(), RegionBSPTree2D.empty());
151 }
152
153
154 @Override
155 public Vector2D project(final Vector2D pt) {
156
157
158 final BoundaryProjector2D projector = new BoundaryProjector2D(pt);
159 accept(projector);
160
161 return projector.getProjected();
162 }
163
164
165
166 @Override
167 public RegionBSPTree2D toTree() {
168 return this;
169 }
170
171
172 @Override
173 public List<LinecastPoint2D> linecast(final LineConvexSubset subset) {
174 final LinecastVisitor visitor = new LinecastVisitor(subset, false);
175 accept(visitor);
176
177 return visitor.getResults();
178 }
179
180
181 @Override
182 public LinecastPoint2D linecastFirst(final LineConvexSubset subset) {
183 final LinecastVisitor visitor = new LinecastVisitor(subset, true);
184 accept(visitor);
185
186 return visitor.getFirstResult();
187 }
188
189
190
191
192 private List<LinePath> computeBoundaryPaths() {
193 final InteriorAngleLinePathConnector connector = new InteriorAngleLinePathConnector.Minimize();
194 connector.connect(boundaries());
195
196 return connector.connectAll().stream()
197 .map(LinePath::simplify).collect(Collectors.toList());
198 }
199
200
201 @Override
202 protected RegionSizeProperties<Vector2D> computeRegionSizeProperties() {
203
204 if (isFull()) {
205 return new RegionSizeProperties<>(Double.POSITIVE_INFINITY, null);
206 } else if (isEmpty()) {
207 return new RegionSizeProperties<>(0, null);
208 }
209
210
211 double quadrilateralAreaSum = 0.0;
212
213 double scaledSumX = 0.0;
214 double scaledSumY = 0.0;
215
216 Vector2D startPoint;
217 Vector2D endPoint;
218 double signedArea;
219
220 for (final LineConvexSubset boundary : boundaries()) {
221
222 if (boundary.isInfinite()) {
223
224
225 quadrilateralAreaSum = Double.POSITIVE_INFINITY;
226
227 break;
228 }
229
230 startPoint = boundary.getStartPoint();
231 endPoint = boundary.getEndPoint();
232
233
234 signedArea = startPoint.signedArea(endPoint);
235
236 quadrilateralAreaSum += signedArea;
237
238
239 scaledSumX += signedArea * (startPoint.getX() + endPoint.getX());
240 scaledSumY += signedArea * (startPoint.getY() + endPoint.getY());
241 }
242
243 double size = Double.POSITIVE_INFINITY;
244 Vector2D centroid = null;
245
246
247
248
249 if (quadrilateralAreaSum >= 0 && Double.isFinite(quadrilateralAreaSum)) {
250 size = 0.5 * quadrilateralAreaSum;
251
252 if (quadrilateralAreaSum > 0) {
253 centroid = Vector2D.of(scaledSumX, scaledSumY).multiply(1.0 / (3.0 * quadrilateralAreaSum));
254 }
255 }
256
257 return new RegionSizeProperties<>(size, centroid);
258 }
259
260
261 @Override
262 protected void invalidate() {
263 super.invalidate();
264
265 boundaryPaths = null;
266 }
267
268
269 @Override
270 protected RegionNode2D createNode() {
271 return new RegionNode2D(this);
272 }
273
274
275
276
277 public static RegionBSPTree2D full() {
278 return new RegionBSPTree2D(true);
279 }
280
281
282
283
284 public static RegionBSPTree2D empty() {
285 return new RegionBSPTree2D(false);
286 }
287
288
289
290
291
292
293
294 public static RegionBSPTree2D from(final Iterable<? extends LineConvexSubset> boundaries) {
295 return from(boundaries, false);
296 }
297
298
299
300
301
302
303
304
305 public static RegionBSPTree2D from(final Iterable<? extends LineConvexSubset> boundaries, final boolean full) {
306 final RegionBSPTree2D tree = new RegionBSPTree2D(full);
307 tree.insert(boundaries);
308
309 return tree;
310 }
311
312
313
314
315
316 public static PartitionedRegionBuilder2D partitionedRegionBuilder() {
317 return new PartitionedRegionBuilder2D();
318 }
319
320
321
322 public static final class RegionNode2D extends AbstractRegionBSPTree.AbstractRegionNode<Vector2D, RegionNode2D> {
323
324
325
326 private RegionNode2D(final AbstractBSPTree<Vector2D, RegionNode2D> tree) {
327 super(tree);
328 }
329
330
331
332
333
334
335 public ConvexArea getNodeRegion() {
336 ConvexArea area = ConvexArea.full();
337
338 RegionNode2D child = this;
339 RegionNode2D parent;
340
341 while ((parent = child.getParent()) != null) {
342 final Split<ConvexArea> split = area.split(parent.getCutHyperplane());
343
344 area = child.isMinus() ? split.getMinus() : split.getPlus();
345
346 child = parent;
347 }
348
349 return area;
350 }
351
352
353 @Override
354 protected RegionNode2D getSelf() {
355 return this;
356 }
357 }
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392 public static final class PartitionedRegionBuilder2D
393 extends AbstractPartitionedRegionBuilder<Vector2D, RegionNode2D> {
394
395
396
397 private PartitionedRegionBuilder2D() {
398 super(RegionBSPTree2D.empty());
399 }
400
401
402
403
404
405
406 public PartitionedRegionBuilder2D insertPartition(final Line partition) {
407 return insertPartition(partition.span());
408 }
409
410
411
412
413
414
415 public PartitionedRegionBuilder2D insertPartition(final LineConvexSubset partition) {
416 insertPartitionInternal(partition);
417
418 return this;
419 }
420
421
422
423
424
425
426
427
428
429
430 public PartitionedRegionBuilder2D insertAxisAlignedPartitions(final Vector2D center,
431 final Precision.DoubleEquivalence precision) {
432
433 insertPartition(Lines.fromPointAndDirection(center, Vector2D.Unit.PLUS_X, precision));
434 insertPartition(Lines.fromPointAndDirection(center, Vector2D.Unit.PLUS_Y, precision));
435
436 return this;
437 }
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454 public PartitionedRegionBuilder2D insertAxisAlignedGrid(final Bounds2D bounds, final int level,
455 final Precision.DoubleEquivalence precision) {
456
457 insertAxisAlignedGridRecursive(bounds.getMin(), bounds.getMax(), level, precision);
458
459 return this;
460 }
461
462
463
464
465
466
467
468 private void insertAxisAlignedGridRecursive(final Vector2D min, final Vector2D max, final int level,
469 final Precision.DoubleEquivalence precision) {
470 if (level > 0) {
471 final Vector2D center = min.lerp(max, 0.5);
472
473 insertAxisAlignedPartitions(center, precision);
474
475 final int nextLevel = level - 1;
476 insertAxisAlignedGridRecursive(min, center, nextLevel, precision);
477 insertAxisAlignedGridRecursive(center, max, nextLevel, precision);
478 }
479 }
480
481
482
483
484
485 public PartitionedRegionBuilder2D insertBoundary(final LineConvexSubset boundary) {
486 insertBoundaryInternal(boundary);
487
488 return this;
489 }
490
491
492
493
494
495 public PartitionedRegionBuilder2D insertBoundaries(final Iterable<? extends LineConvexSubset> boundaries) {
496 for (final LineConvexSubset boundary : boundaries) {
497 insertBoundaryInternal(boundary);
498 }
499
500 return this;
501 }
502
503
504
505
506
507 public PartitionedRegionBuilder2D insertBoundaries(final BoundarySource2D boundarySrc) {
508 try (Stream<LineConvexSubset> stream = boundarySrc.boundaryStream()) {
509 stream.forEach(this::insertBoundaryInternal);
510 }
511
512 return this;
513 }
514
515
516
517
518 public RegionBSPTree2D build() {
519 return (RegionBSPTree2D) buildInternal();
520 }
521 }
522
523
524
525 private static final class BoundaryProjector2D extends BoundaryProjector<Vector2D, RegionNode2D> {
526
527
528
529 BoundaryProjector2D(final Vector2D point) {
530 super(point);
531 }
532
533
534 @Override
535 protected Vector2D disambiguateClosestPoint(final Vector2D target, final Vector2D a, final Vector2D b) {
536
537 final int cmp = Vector2D.COORDINATE_ASCENDING_ORDER.compare(a, b);
538 return cmp < 0 ? a : b;
539 }
540 }
541
542
543
544 private static final class LinecastVisitor implements BSPTreeVisitor<Vector2D, RegionNode2D> {
545
546
547 private final LineConvexSubset linecastSubset;
548
549
550
551
552 private final boolean firstOnly;
553
554
555 private double minAbscissa = Double.POSITIVE_INFINITY;
556
557
558 private final List<LinecastPoint2D> results = new ArrayList<>();
559
560
561
562
563
564
565 LinecastVisitor(final LineConvexSubset linecastSubset, final boolean firstOnly) {
566 this.linecastSubset = linecastSubset;
567 this.firstOnly = firstOnly;
568 }
569
570
571
572
573 public LinecastPoint2D getFirstResult() {
574 final List<LinecastPoint2D> sortedResults = getResults();
575
576 return sortedResults.isEmpty() ?
577 null :
578 sortedResults.get(0);
579 }
580
581
582
583
584
585 public List<LinecastPoint2D> getResults() {
586 LinecastPoint2D.sortAndFilter(results);
587
588 return results;
589 }
590
591
592 @Override
593 public Order visitOrder(final RegionNode2D internalNode) {
594 final Line cut = (Line) internalNode.getCutHyperplane();
595 final Line line = linecastSubset.getLine();
596
597 final boolean plusIsNear = line.getDirection().dot(cut.getOffsetDirection()) < 0;
598
599 return plusIsNear ?
600 Order.PLUS_NODE_MINUS :
601 Order.MINUS_NODE_PLUS;
602 }
603
604
605 @Override
606 public Result visit(final RegionNode2D node) {
607 if (node.isInternal()) {
608
609 final Line line = linecastSubset.getLine();
610 final Vector2D pt = ((Line) node.getCutHyperplane()).intersection(line);
611
612 if (pt != null) {
613 if (firstOnly && !results.isEmpty() &&
614 line.getPrecision().compare(minAbscissa, line.abscissa(pt)) < 0) {
615
616
617 return Result.TERMINATE;
618 } else if (linecastSubset.contains(pt)) {
619
620
621 final LinecastPoint2D potentialResult = computeLinecastPoint(pt, node);
622 if (potentialResult != null) {
623 results.add(potentialResult);
624
625
626 minAbscissa = Math.min(minAbscissa, potentialResult.getAbscissa());
627 }
628 }
629 }
630 }
631
632 return Result.CONTINUE;
633 }
634
635
636
637
638
639
640
641
642 private LinecastPoint2D computeLinecastPoint(final Vector2D pt, final RegionNode2D node) {
643 final Line cut = (Line) node.getCutHyperplane();
644 final RegionCutBoundary<Vector2D> boundary = node.getCutBoundary();
645
646 boolean onBoundary = false;
647 boolean negateNormal = false;
648
649 if (boundary.containsInsideFacing(pt)) {
650
651 onBoundary = true;
652 negateNormal = true;
653 } else if (boundary.containsOutsideFacing(pt)) {
654
655 onBoundary = true;
656 }
657
658 if (onBoundary) {
659 Vector2D normal = cut.getOffsetDirection();
660 if (negateNormal) {
661 normal = normal.negate();
662 }
663
664 return new LinecastPoint2D(pt, normal, linecastSubset.getLine());
665 }
666
667 return null;
668 }
669 }
670 }