File size: 22,429 Bytes
e6be672
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
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
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
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
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
# Copyright (C) 2026 Mattia Zatelli
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU Affero General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU Affero General Public License for more details.
#
# You should have received a copy of the GNU Affero General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.

"""
Geometric utilities for BIM element processing using IfcOpenShell, pythonocc, and trimesh

This module provides core geometric operations for:
- 3D distance calculations
- Bounding box operations
- Spatial relationships (containment, intersection, proximity)
- Clash detection
- Volume and area calculations
- Geometric validation
"""
import logging
import numpy as np
from typing import List, Tuple, Optional, Dict, Any, Union
from dataclasses import dataclass
from enum import Enum

import ifcopenshell
import ifcopenshell.geom
import trimesh
from OCC.Core.gp import gp_Pnt, gp_Vec
from OCC.Core.BRepBndLib import brepbndlib
from OCC.Core.Bnd import Bnd_Box
from OCC.Core.BRepExtrema import BRepExtrema_DistShapeShape
from OCC.Core.BRepAlgoAPI import BRepAlgoAPI_Common, BRepAlgoAPI_Section
from OCC.Core.BRepGProp import brepgprop
from OCC.Core.GProp import GProp_GProps
from OCC.Core.TopoDS import TopoDS_Shape
from OCC.Core.TopAbs import TopAbs_VERTEX, TopAbs_EDGE, TopAbs_FACE, TopAbs_SOLID
from OCC.Core.TopExp import TopExp_Explorer
from OCC.Core.BRep import BRep_Tool

logger = logging.getLogger(__name__)


class ClashType(str, Enum):
    """Types of clashes between elements"""
    HARD = "hard"  # Physical intersection
    SOFT = "soft"  # Clearance violation
    NONE = "none"  # No clash


@dataclass
class BoundingBox:
    """Axis-aligned bounding box"""
    min_x: float
    min_y: float
    min_z: float
    max_x: float
    max_y: float
    max_z: float

    @property
    def center(self) -> Tuple[float, float, float]:
        """Get center point of bounding box"""
        return (
            (self.min_x + self.max_x) / 2,
            (self.min_y + self.max_y) / 2,
            (self.min_z + self.max_z) / 2
        )

    @property
    def dimensions(self) -> Tuple[float, float, float]:
        """Get dimensions (width, depth, height)"""
        return (
            self.max_x - self.min_x,
            self.max_y - self.min_y,
            self.max_z - self.min_z
        )

    @property
    def volume(self) -> float:
        """Calculate bounding box volume"""
        dims = self.dimensions
        return dims[0] * dims[1] * dims[2]

    def contains_point(self, x: float, y: float, z: float) -> bool:
        """Check if point is inside bounding box"""
        return (
            self.min_x <= x <= self.max_x and
            self.min_y <= y <= self.max_y and
            self.min_z <= z <= self.max_z
        )

    def intersects(self, other: 'BoundingBox') -> bool:
        """Check if this bounding box intersects another"""
        return (
            self.min_x <= other.max_x and self.max_x >= other.min_x and
            self.min_y <= other.max_y and self.max_y >= other.min_y and
            self.min_z <= other.max_z and self.max_z >= other.min_z
        )

    def expand(self, distance: float) -> 'BoundingBox':
        """Expand bounding box by distance in all directions"""
        return BoundingBox(
            min_x=self.min_x - distance,
            min_y=self.min_y - distance,
            min_z=self.min_z - distance,
            max_x=self.max_x + distance,
            max_y=self.max_y + distance,
            max_z=self.max_z + distance
        )


@dataclass
class GeometricProperties:
    """Geometric properties of an element"""
    volume: float
    surface_area: float
    centroid: Tuple[float, float, float]
    bounding_box: BoundingBox


@dataclass
class ClashResult:
    """Result of clash detection between two elements"""
    element1_id: str
    element2_id: str
    clash_type: ClashType
    intersection_volume: float
    clearance_distance: float
    clash_point: Optional[Tuple[float, float, float]]
    description: str


class GeometryProcessor:
    """Main class for geometric operations on IFC elements"""

    def __init__(self, settings: Optional[ifcopenshell.geom.settings] = None):
        """
        Initialize geometry processor

        Args:
            settings: IfcOpenShell geometry settings. If None, uses default settings.
        """
        if settings is None:
            settings = ifcopenshell.geom.settings()
            settings.set(settings.USE_PYTHON_OPENCASCADE, True)
            settings.set(settings.WELD_VERTICES, True)
            settings.set(settings.USE_WORLD_COORDS, True)

        self.settings = settings
        self._shape_cache: Dict[str, TopoDS_Shape] = {}

    def get_shape_from_ifc(
        self,
        ifc_file: ifcopenshell.file,
        element_guid: str,
        use_cache: bool = True
    ) -> Optional[TopoDS_Shape]:
        """
        Extract OpenCASCADE shape from IFC element

        Args:
            ifc_file: IFC file object
            element_guid: Global ID of the element
            use_cache: Whether to use cached shapes

        Returns:
            TopoDS_Shape or None if geometry extraction fails
        """
        if use_cache and element_guid in self._shape_cache:
            return self._shape_cache[element_guid]

        try:
            element = ifc_file.by_guid(element_guid)
            shape = ifcopenshell.geom.create_shape(self.settings, element)

            if shape is None:
                logger.warning(f"Failed to create shape for element {element_guid}")
                return None

            # Get the OpenCASCADE shape
            occ_shape = shape.geometry.brep_data

            if use_cache:
                self._shape_cache[element_guid] = occ_shape

            return occ_shape

        except Exception as e:
            logger.error(f"Error extracting shape for {element_guid}: {e}")
            return None

    def get_bounding_box(self, shape: TopoDS_Shape) -> BoundingBox:
        """
        Calculate bounding box for a shape

        Args:
            shape: OpenCASCADE shape

        Returns:
            BoundingBox object
        """
        bbox = Bnd_Box()
        brepbndlib.Add(shape, bbox)

        xmin, ymin, zmin, xmax, ymax, zmax = bbox.Get()

        return BoundingBox(
            min_x=xmin, min_y=ymin, min_z=zmin,
            max_x=xmax, max_y=ymax, max_z=zmax
        )

    def get_centroid(self, shape: TopoDS_Shape) -> Tuple[float, float, float]:
        """
        Calculate centroid (center of mass) of a shape

        Args:
            shape: OpenCASCADE shape

        Returns:
            Tuple of (x, y, z) coordinates
        """
        props = GProp_GProps()
        brepgprop.VolumeProperties(shape, props)
        center = props.CentreOfMass()

        return (center.X(), center.Y(), center.Z())

    def get_geometric_properties(self, shape: TopoDS_Shape) -> GeometricProperties:
        """
        Calculate comprehensive geometric properties

        Args:
            shape: OpenCASCADE shape

        Returns:
            GeometricProperties object
        """
        # Volume and surface properties
        volume_props = GProp_GProps()
        surface_props = GProp_GProps()

        brepgprop.VolumeProperties(shape, volume_props)
        brepgprop.SurfaceProperties(shape, surface_props)

        volume = volume_props.Mass()
        surface_area = surface_props.Mass()
        centroid = volume_props.CentreOfMass()

        # Bounding box
        bbox = self.get_bounding_box(shape)

        return GeometricProperties(
            volume=volume,
            surface_area=surface_area,
            centroid=(centroid.X(), centroid.Y(), centroid.Z()),
            bounding_box=bbox
        )

    def calculate_distance(
        self,
        shape1: TopoDS_Shape,
        shape2: TopoDS_Shape
    ) -> Tuple[float, Optional[Tuple[float, float, float]], Optional[Tuple[float, float, float]]]:
        """
        Calculate minimum distance between two shapes

        Args:
            shape1: First shape
            shape2: Second shape

        Returns:
            Tuple of (distance, point_on_shape1, point_on_shape2)
        """
        dist_calc = BRepExtrema_DistShapeShape(shape1, shape2)
        dist_calc.Perform()

        if not dist_calc.IsDone():
            logger.warning("Distance calculation failed")
            return (float('inf'), None, None)

        distance = dist_calc.Value()

        # Get closest points
        point1 = dist_calc.PointOnShape1(1)
        point2 = dist_calc.PointOnShape2(1)

        return (
            distance,
            (point1.X(), point1.Y(), point1.Z()),
            (point2.X(), point2.Y(), point2.Z())
        )

    def detect_intersection(
        self,
        shape1: TopoDS_Shape,
        shape2: TopoDS_Shape,
        tolerance: float = 1e-6
    ) -> Tuple[bool, float]:
        """
        Detect if two shapes intersect and calculate intersection volume

        Args:
            shape1: First shape
            shape2: Second shape
            tolerance: Geometric tolerance

        Returns:
            Tuple of (intersects: bool, intersection_volume: float)
        """
        try:
            # Use Boolean common operation to find intersection
            common = BRepAlgoAPI_Common(shape1, shape2)
            common.Build()

            if not common.IsDone():
                return (False, 0.0)

            intersection_shape = common.Shape()

            # Check if intersection shape is valid
            if intersection_shape.IsNull():
                return (False, 0.0)

            # Calculate volume of intersection
            props = GProp_GProps()
            brepgprop.VolumeProperties(intersection_shape, props)
            volume = props.Mass()

            intersects = volume > tolerance

            return (intersects, volume)

        except Exception as e:
            logger.error(f"Error detecting intersection: {e}")
            return (False, 0.0)

    def check_containment(
        self,
        container_shape: TopoDS_Shape,
        contained_shape: TopoDS_Shape,
        tolerance: float = 1e-3
    ) -> bool:
        """
        Check if one shape is contained within another

        Args:
            container_shape: Potential container shape
            contained_shape: Shape to check containment
            tolerance: Tolerance for containment check

        Returns:
            True if contained_shape is inside container_shape
        """
        # Strategy: Check if all vertices of contained_shape are inside container
        # This is an approximation - for precise containment, more sophisticated checks needed

        # Get centroid of contained shape
        centroid = self.get_centroid(contained_shape)

        # Check distance from centroid to container
        distance, _, _ = self.calculate_distance(
            container_shape,
            self._point_to_vertex(centroid)
        )

        # If distance is very small, centroid is inside or on boundary
        if distance <= tolerance:
            # Additional check: ensure no significant parts extend outside
            # Get bounding boxes
            container_bbox = self.get_bounding_box(container_shape)
            contained_bbox = self.get_bounding_box(contained_shape)

            # Check if contained bbox is within container bbox
            return (
                contained_bbox.min_x >= container_bbox.min_x - tolerance and
                contained_bbox.max_x <= container_bbox.max_x + tolerance and
                contained_bbox.min_y >= container_bbox.min_y - tolerance and
                contained_bbox.max_y <= container_bbox.max_y + tolerance and
                contained_bbox.min_z >= container_bbox.min_z - tolerance and
                contained_bbox.max_z <= container_bbox.max_z + tolerance
            )

        return False

    def detect_clash(
        self,
        shape1: TopoDS_Shape,
        shape2: TopoDS_Shape,
        clearance_distance: float = 0.05,
        element1_id: str = "element1",
        element2_id: str = "element2"
    ) -> ClashResult:
        """
        Comprehensive clash detection between two shapes

        Args:
            shape1: First shape
            shape2: Second shape
            clearance_distance: Required clearance in meters (default 5cm)
            element1_id: ID of first element
            element2_id: ID of second element

        Returns:
            ClashResult object
        """
        # First check bounding boxes for quick rejection
        bbox1 = self.get_bounding_box(shape1)
        bbox2 = self.get_bounding_box(shape2)

        if not bbox1.intersects(bbox2.expand(clearance_distance)):
            # No potential clash
            distance, pt1, pt2 = self.calculate_distance(shape1, shape2)
            return ClashResult(
                element1_id=element1_id,
                element2_id=element2_id,
                clash_type=ClashType.NONE,
                intersection_volume=0.0,
                clearance_distance=distance,
                clash_point=None,
                description="No clash detected - elements are far apart"
            )

        # Check for hard clash (intersection)
        intersects, volume = self.detect_intersection(shape1, shape2)

        if intersects and volume > 1e-6:  # 1 cubic millimeter threshold
            # Hard clash detected
            centroid1 = self.get_centroid(shape1)
            return ClashResult(
                element1_id=element1_id,
                element2_id=element2_id,
                clash_type=ClashType.HARD,
                intersection_volume=volume,
                clearance_distance=0.0,
                clash_point=centroid1,
                description=f"Hard clash: intersection volume {volume:.6f} m³"
            )

        # Check for soft clash (clearance violation)
        distance, pt1, pt2 = self.calculate_distance(shape1, shape2)

        if distance < clearance_distance:
            # Soft clash detected
            return ClashResult(
                element1_id=element1_id,
                element2_id=element2_id,
                clash_type=ClashType.SOFT,
                intersection_volume=0.0,
                clearance_distance=distance,
                clash_point=pt1,
                description=f"Soft clash: clearance {distance:.4f}m < required {clearance_distance}m"
            )

        # No clash
        return ClashResult(
            element1_id=element1_id,
            element2_id=element2_id,
            clash_type=ClashType.NONE,
            intersection_volume=0.0,
            clearance_distance=distance,
            clash_point=None,
            description=f"No clash: clearance {distance:.4f}m"
        )

    def validate_geometry(self, shape: TopoDS_Shape) -> Dict[str, Any]:
        """
        Validate geometry for common issues

        Args:
            shape: Shape to validate

        Returns:
            Dictionary with validation results
        """
        issues = []
        is_valid = True

        try:
            # Check if shape is null
            if shape.IsNull():
                issues.append("Shape is null")
                is_valid = False
                return {"valid": False, "issues": issues, "warnings": []}

            # Check for degenerate geometry
            bbox = self.get_bounding_box(shape)
            dims = bbox.dimensions

            if min(dims) < 1e-6:
                issues.append(f"Degenerate geometry: dimension too small {min(dims)}")
                is_valid = False

            # Check for self-intersections using section operation
            # This is expensive, so we do a quick check first
            try:
                section = BRepAlgoAPI_Section(shape, shape)
                section.Build()

                if section.IsDone() and not section.Shape().IsNull():
                    # There might be self-intersections
                    # Additional analysis would be needed for confirmation
                    issues.append("Potential self-intersection detected")
                    is_valid = False

            except Exception as e:
                logger.debug(f"Self-intersection check failed: {e}")

            # Check volume properties
            try:
                props = GProp_GProps()
                brepgprop.VolumeProperties(shape, props)
                volume = props.Mass()

                if volume <= 0:
                    issues.append(f"Invalid volume: {volume}")
                    is_valid = False

            except Exception as e:
                issues.append(f"Failed to calculate volume: {e}")
                is_valid = False

            # Check for invalid faces/edges
            warnings = []
            face_count = 0
            edge_count = 0
            vertex_count = 0

            # Count topological elements
            face_explorer = TopExp_Explorer(shape, TopAbs_FACE)
            while face_explorer.More():
                face_count += 1
                face_explorer.Next()

            edge_explorer = TopExp_Explorer(shape, TopAbs_EDGE)
            while edge_explorer.More():
                edge_count += 1
                edge_explorer.Next()

            vertex_explorer = TopExp_Explorer(shape, TopAbs_VERTEX)
            while vertex_explorer.More():
                vertex_count += 1
                vertex_explorer.Next()

            if face_count == 0:
                warnings.append("Shape has no faces")

            if edge_count == 0:
                warnings.append("Shape has no edges")

            if vertex_count < 3:
                issues.append(f"Insufficient vertices: {vertex_count}")
                is_valid = False

            return {
                "valid": is_valid,
                "issues": issues,
                "warnings": warnings,
                "statistics": {
                    "faces": face_count,
                    "edges": edge_count,
                    "vertices": vertex_count,
                    "volume": volume if 'volume' in locals() else 0.0,
                    "bounding_box": {
                        "dimensions": dims,
                        "volume": bbox.volume
                    }
                }
            }

        except Exception as e:
            logger.error(f"Geometry validation failed: {e}")
            return {
                "valid": False,
                "issues": [f"Validation error: {str(e)}"],
                "warnings": [],
                "statistics": {}
            }

    def _point_to_vertex(self, point: Tuple[float, float, float]) -> TopoDS_Shape:
        """
        Convert a point to a TopoDS vertex shape

        Args:
            point: Tuple of (x, y, z) coordinates

        Returns:
            TopoDS vertex shape
        """
        from OCC.Core.BRepBuilderAPI import BRepBuilderAPI_MakeVertex
        pnt = gp_Pnt(point[0], point[1], point[2])
        return BRepBuilderAPI_MakeVertex(pnt).Shape()


class SpatialIndex:
    """R-tree spatial index for efficient proximity queries"""

    def __init__(self):
        """Initialize spatial index"""
        self.elements: Dict[str, BoundingBox] = {}
        self._rebuild_needed = False

    def add_element(self, element_id: str, bbox: BoundingBox):
        """Add element to spatial index"""
        self.elements[element_id] = bbox
        self._rebuild_needed = True

    def find_in_proximity(
        self,
        point: Tuple[float, float, float],
        max_distance: float
    ) -> List[str]:
        """
        Find all elements within max_distance of point

        Args:
            point: Point coordinates (x, y, z)
            max_distance: Maximum distance

        Returns:
            List of element IDs
        """
        x, y, z = point
        candidates = []

        for element_id, bbox in self.elements.items():
            # Quick bounding box check
            center = bbox.center
            dx = abs(center[0] - x)
            dy = abs(center[1] - y)
            dz = abs(center[2] - z)

            # Approximate distance using bounding box center
            approx_distance = (dx**2 + dy**2 + dz**2) ** 0.5

            # Add some tolerance based on bbox size
            bbox_radius = max(bbox.dimensions) / 2

            if approx_distance - bbox_radius <= max_distance:
                candidates.append(element_id)

        return candidates

    def find_intersecting(self, bbox: BoundingBox) -> List[str]:
        """
        Find all elements whose bounding boxes intersect given bbox

        Args:
            bbox: Query bounding box

        Returns:
            List of element IDs
        """
        intersecting = []

        for element_id, element_bbox in self.elements.items():
            if bbox.intersects(element_bbox):
                intersecting.append(element_id)

        return intersecting


def calculate_point_distance(
    point1: Tuple[float, float, float],
    point2: Tuple[float, float, float]
) -> float:
    """
    Calculate Euclidean distance between two points

    Args:
        point1: First point (x, y, z)
        point2: Second point (x, y, z)

    Returns:
        Distance in meters
    """
    dx = point2[0] - point1[0]
    dy = point2[1] - point1[1]
    dz = point2[2] - point1[2]

    return (dx**2 + dy**2 + dz**2) ** 0.5


def point_in_bbox(
    point: Tuple[float, float, float],
    bbox: BoundingBox
) -> bool:
    """
    Check if point is inside bounding box

    Args:
        point: Point coordinates (x, y, z)
        bbox: Bounding box

    Returns:
        True if point is inside bbox
    """
    return bbox.contains_point(point[0], point[1], point[2])


def bboxes_intersect(bbox1: BoundingBox, bbox2: BoundingBox) -> bool:
    """
    Check if two bounding boxes intersect

    Args:
        bbox1: First bounding box
        bbox2: Second bounding box

    Returns:
        True if bounding boxes intersect
    """
    return bbox1.intersects(bbox2)