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