diff --git a/Octree/Octree.playground/Sources/Octree.swift b/Octree/Octree.playground/Sources/Octree.swift index 72863fb7c..351335e36 100644 --- a/Octree/Octree.playground/Sources/Octree.swift +++ b/Octree/Octree.playground/Sources/Octree.swift @@ -1,79 +1,99 @@ +// Based on: +// Octree.swift +// Swift Algorithm Club +// +// Written for Swift Algorithm Club by Jaap Wijnen *Heavily inspired by +// Timur Galimov's Quadtree implementation and Apple's GKOctree implementation +// +// https://github.com/kodecocodes/swift-algorithm-club/blob/master/Octree/README.md +// +// Corrected, refined and improved by Marco Binder (Heidelberg, Germany), 2025 +// using ChatGPT as a coding buddy. BUG FIXES: bug removed in tryAdd, where it was +// implicitly expected that elements is never nil (which it was upon subdivision). +// REFINEMENTS: improved efficiency in Box .contains(simd3_double), including using +// an epsilon to prevent unneccessary subdivision upon rounding errors; several +// other steps refined. IMPROVEMENTS: tree now autocollapses upon removing elements, +// turning empty leafs into internal nodes again; accepts elements with box-regions +// instead of point positions, similar to the Apple GKOctree implementation; now +// respects minimumCellSize from initializer, saves it and subdivides only until +// reaching minimum size, after which it bulks elements in single leave. + import Foundation -import simd +import SIMD + -public struct Box: CustomStringConvertible { - public var boxMin: vector_double3 - public var boxMax: vector_double3 +public struct MBBox: CustomStringConvertible { + public var boxMin: SIMD3 + public var boxMax: SIMD3 - public init(boxMin: vector_double3, boxMax: vector_double3) { + public init(boxMin: SIMD3, boxMax: SIMD3) { self.boxMin = boxMin self.boxMax = boxMax } - public var boxSize: vector_double3 { + public var boxSize: SIMD3 { return boxMax - boxMin } - var halfBoxSize: vector_double3 { + var halfBoxSize: SIMD3 { return boxSize/2 } - var frontLeftTop: Box { - let boxMin = self.boxMin + vector_double3(0, halfBoxSize.y, halfBoxSize.z) - let boxMax = self.boxMax - vector_double3(halfBoxSize.x, 0, 0) - return Box(boxMin: boxMin, boxMax: boxMax) - } - var frontLeftBottom: Box { - let boxMin = self.boxMin + vector_double3(0, 0, halfBoxSize.z) - let boxMax = self.boxMax - vector_double3(halfBoxSize.x, halfBoxSize.y, 0) - return Box(boxMin: boxMin, boxMax: boxMax) - } - var frontRightTop: Box { - let boxMin = self.boxMin + vector_double3(halfBoxSize.x, halfBoxSize.y, halfBoxSize.z) - let boxMax = self.boxMax - vector_double3(0, 0, 0) - return Box(boxMin: boxMin, boxMax: boxMax) - } - var frontRightBottom: Box { - let boxMin = self.boxMin + vector_double3(halfBoxSize.x, 0, halfBoxSize.z) - let boxMax = self.boxMax - vector_double3(0, halfBoxSize.y, 0) - return Box(boxMin: boxMin, boxMax: boxMax) - } - var backLeftTop: Box { - let boxMin = self.boxMin + vector_double3(0, halfBoxSize.y, 0) - let boxMax = self.boxMax - vector_double3(halfBoxSize.x, 0, halfBoxSize.z) - return Box(boxMin: boxMin, boxMax: boxMax) - } - var backLeftBottom: Box { - let boxMin = self.boxMin + vector_double3(0, 0, 0) - let boxMax = self.boxMax - vector_double3(halfBoxSize.x, halfBoxSize.y, halfBoxSize.z) - return Box(boxMin: boxMin, boxMax: boxMax) - } - var backRightTop: Box { - let boxMin = self.boxMin + vector_double3(halfBoxSize.x, halfBoxSize.y, 0) - let boxMax = self.boxMax - vector_double3(0, 0, halfBoxSize.z) - return Box(boxMin: boxMin, boxMax: boxMax) - } - var backRightBottom: Box { - let boxMin = self.boxMin + vector_double3(halfBoxSize.x, 0, 0) - let boxMax = self.boxMax - vector_double3(0, halfBoxSize.y, halfBoxSize.z) - return Box(boxMin: boxMin, boxMax: boxMax) + var frontLeftTop: MBBox { + let boxMin = self.boxMin + SIMD3(0, halfBoxSize.y, halfBoxSize.z) + let boxMax = self.boxMax - SIMD3(halfBoxSize.x, 0, 0) + return MBBox(boxMin: boxMin, boxMax: boxMax) + } + var frontLeftBottom: MBBox { + let boxMin = self.boxMin + SIMD3(0, 0, halfBoxSize.z) + let boxMax = self.boxMax - SIMD3(halfBoxSize.x, halfBoxSize.y, 0) + return MBBox(boxMin: boxMin, boxMax: boxMax) + } + var frontRightTop: MBBox { + let boxMin = self.boxMin + SIMD3(halfBoxSize.x, halfBoxSize.y, halfBoxSize.z) + let boxMax = self.boxMax - SIMD3(0, 0, 0) + return MBBox(boxMin: boxMin, boxMax: boxMax) + } + var frontRightBottom: MBBox { + let boxMin = self.boxMin + SIMD3(halfBoxSize.x, 0, halfBoxSize.z) + let boxMax = self.boxMax - SIMD3(0, halfBoxSize.y, 0) + return MBBox(boxMin: boxMin, boxMax: boxMax) + } + var backLeftTop: MBBox { + let boxMin = self.boxMin + SIMD3(0, halfBoxSize.y, 0) + let boxMax = self.boxMax - SIMD3(halfBoxSize.x, 0, halfBoxSize.z) + return MBBox(boxMin: boxMin, boxMax: boxMax) + } + var backLeftBottom: MBBox { + let boxMin = self.boxMin + SIMD3(0, 0, 0) + let boxMax = self.boxMax - SIMD3(halfBoxSize.x, halfBoxSize.y, halfBoxSize.z) + return MBBox(boxMin: boxMin, boxMax: boxMax) + } + var backRightTop: MBBox { + let boxMin = self.boxMin + SIMD3(halfBoxSize.x, halfBoxSize.y, 0) + let boxMax = self.boxMax - SIMD3(0, 0, halfBoxSize.z) + return MBBox(boxMin: boxMin, boxMax: boxMax) + } + var backRightBottom: MBBox { + let boxMin = self.boxMin + SIMD3(halfBoxSize.x, 0, 0) + let boxMax = self.boxMax - SIMD3(0, halfBoxSize.y, halfBoxSize.z) + return MBBox(boxMin: boxMin, boxMax: boxMax) } - public func contains(_ point: vector_double3) -> Bool { - return (boxMin.x <= point.x && point.x <= boxMax.x) && (boxMin.y <= point.y && point.y <= boxMax.y) && (boxMin.z <= point.z && point.z <= boxMax.z) + /// Robust against floating-point rounding errors + public func contains(_ point: SIMD3, epsilon: Double = 1e-10) -> Bool { + return (boxMin.x - epsilon <= point.x && point.x <= boxMax.x + epsilon) && + (boxMin.y - epsilon <= point.y && point.y <= boxMax.y + epsilon) && + (boxMin.z - epsilon <= point.z && point.z <= boxMax.z + epsilon) } - - public func contains(_ box: Box) -> Bool { - return - self.boxMin.x <= box.boxMin.x && - self.boxMin.y <= box.boxMin.y && - self.boxMin.z <= box.boxMin.z && - self.boxMax.x >= box.boxMax.x && - self.boxMax.y >= box.boxMax.y && - self.boxMax.z >= box.boxMax.z + + /// Compatible legacy overload without epsilon + public func contains(_ point: SIMD3) -> Bool { + contains(point, epsilon: 1e-10) } - public func isContained(in box: Box) -> Bool { + + public func isContained(in box: MBBox) -> Bool { return self.boxMin.x >= box.boxMin.x && self.boxMin.y >= box.boxMin.y && @@ -83,26 +103,13 @@ public struct Box: CustomStringConvertible { self.boxMax.z <= box.boxMax.z } - /* This intersect function does not handle all possibilities such as two beams - of different diameter crossing each other half way. But it does cover all cases - needed for an octree as the bounding box has to contain the given intersect box */ - public func intersects(_ box: Box) -> Bool { - let corners = [ - vector_double3(boxMin.x, boxMax.y, boxMax.z), //frontLeftTop - vector_double3(boxMin.x, boxMin.y, boxMax.z), //frontLeftBottom - vector_double3(boxMax.x, boxMax.y, boxMax.z), //frontRightTop - vector_double3(boxMax.x, boxMin.y, boxMax.z), //frontRightBottom - vector_double3(boxMin.x, boxMax.y, boxMin.z), //backLeftTop - vector_double3(boxMin.x, boxMin.y, boxMin.z), //backLeftBottom - vector_double3(boxMax.x, boxMax.y, boxMin.z), //backRightTop - vector_double3(boxMax.x, boxMin.y, boxMin.z) //backRightBottom - ] - for corner in corners { - if box.contains(corner) { - return true - } - } - return false + public func intersects(_ other: MBBox) -> Bool { + return !(boxMax.x < other.boxMin.x || + boxMin.x > other.boxMax.x || + boxMax.y < other.boxMin.y || + boxMin.y > other.boxMax.y || + boxMax.z < other.boxMin.z || + boxMin.z > other.boxMax.z) } public var description: String { @@ -110,11 +117,22 @@ public struct Box: CustomStringConvertible { } } -public class OctreeNode: CustomStringConvertible { - let box: Box - var point: vector_double3! - var elements: [T]! +public class MBOctreeNode: CustomStringConvertible { + let box: MBBox + let minimumCellSize: Double + private var point: SIMD3? + private var elements: [T]? var type: NodeType = .leaf + /// Stores elements that occupy entire spatial regions + private var regionElements: [(element: T, region: MBBox)] = [] + /// Helper to access child nodes as an array + private var childrenNodes: [MBOctreeNode] { + guard case .internal(let c) = type else { return [] } + return [ + c.frontLeftTop, c.frontLeftBottom, c.frontRightTop, c.frontRightBottom, + c.backLeftTop, c.backLeftBottom, c.backRightTop, c.backRightBottom + ] + } enum NodeType { case leaf @@ -124,7 +142,7 @@ public class OctreeNode: CustomStringConvertible { public var description: String { switch type { case .leaf: - return "leaf node with \(box) elements: \(elements)" + return "leaf node with \(box) elements: \(String(describing: elements))" case .internal: return "internal node with \(box)" } @@ -149,24 +167,24 @@ public class OctreeNode: CustomStringConvertible { } struct Children: Sequence { - let frontLeftTop: OctreeNode - let frontLeftBottom: OctreeNode - let frontRightTop: OctreeNode - let frontRightBottom: OctreeNode - let backLeftTop: OctreeNode - let backLeftBottom: OctreeNode - let backRightTop: OctreeNode - let backRightBottom: OctreeNode + let frontLeftTop: MBOctreeNode + let frontLeftBottom: MBOctreeNode + let frontRightTop: MBOctreeNode + let frontRightBottom: MBOctreeNode + let backLeftTop: MBOctreeNode + let backLeftBottom: MBOctreeNode + let backRightTop: MBOctreeNode + let backRightBottom: MBOctreeNode - init(parentNode: OctreeNode) { - frontLeftTop = OctreeNode(box: parentNode.box.frontLeftTop) - frontLeftBottom = OctreeNode(box: parentNode.box.frontLeftBottom) - frontRightTop = OctreeNode(box: parentNode.box.frontRightTop) - frontRightBottom = OctreeNode(box: parentNode.box.frontRightBottom) - backLeftTop = OctreeNode(box: parentNode.box.backLeftTop) - backLeftBottom = OctreeNode(box: parentNode.box.backLeftBottom) - backRightTop = OctreeNode(box: parentNode.box.backRightTop) - backRightBottom = OctreeNode(box: parentNode.box.backRightBottom) + init(parentNode: MBOctreeNode) { + frontLeftTop = MBOctreeNode(box: parentNode.box.frontLeftTop, minimumCellSize: parentNode.minimumCellSize) + frontLeftBottom = MBOctreeNode(box: parentNode.box.frontLeftBottom, minimumCellSize: parentNode.minimumCellSize) + frontRightTop = MBOctreeNode(box: parentNode.box.frontRightTop, minimumCellSize: parentNode.minimumCellSize) + frontRightBottom = MBOctreeNode(box: parentNode.box.frontRightBottom, minimumCellSize: parentNode.minimumCellSize) + backLeftTop = MBOctreeNode(box: parentNode.box.backLeftTop, minimumCellSize: parentNode.minimumCellSize) + backLeftBottom = MBOctreeNode(box: parentNode.box.backLeftBottom, minimumCellSize: parentNode.minimumCellSize) + backRightTop = MBOctreeNode(box: parentNode.box.backRightTop, minimumCellSize: parentNode.minimumCellSize) + backRightBottom = MBOctreeNode(box: parentNode.box.backRightBottom, minimumCellSize: parentNode.minimumCellSize) } struct ChildrenIterator: IteratorProtocol { @@ -177,7 +195,7 @@ public class OctreeNode: CustomStringConvertible { self.children = children } - mutating func next() -> OctreeNode? { + mutating func next() -> MBOctreeNode? { defer { index += 1 } switch index { case 0: return children.frontLeftTop @@ -198,35 +216,46 @@ public class OctreeNode: CustomStringConvertible { } } - init(box: Box) { + init(box: MBBox, minimumCellSize: Double) { self.box = box + self.minimumCellSize = minimumCellSize } @discardableResult - func add(_ element: T, at point: vector_double3) -> OctreeNode { - return tryAdd(element, at: point)! + func add(_ element: T, at point: SIMD3) -> MBOctreeNode? { + return tryAdd(element, at: point) } - - private func tryAdd(_ element: T, at point: vector_double3) -> OctreeNode? { - if !box.contains(point) { - return nil - } + + /// Adds an element that occupies the entire region of space + @discardableResult + func add(_ element: T, in region: MBBox) -> MBOctreeNode? { + return tryAdd(element, in: region) + } + + private func tryAdd(_ element: T, at point: SIMD3) -> MBOctreeNode? { + if !box.contains(point, epsilon: 1e-10) { return nil } switch type { - case .internal(let children): + case .internal: // pass the point to one of the children - for child in children { + for child in childrenNodes { if let child = child.tryAdd(element, at: point) { return child } } - - fatalError("box.contains evaluted to true, but none of the children added the point") + // Fallback: kein Child angenommen – Loggen und überspringen + NSLog("Warning: point \(point) in \(box) but no child accepted it, skipping element \(element)") + return nil case .leaf: + let maxSize = max(box.boxSize.x, box.boxSize.y, box.boxSize.z) + if maxSize / 2.0 < minimumCellSize { + self.elements?.append(element) ?? { self.elements = [element] }() + return self + } if self.point != nil { // leaf already has an asigned point if self.point == point { - self.elements.append(element) + self.elements?.append(element) ?? { self.elements = [element] }() return self } else { return subdivide(adding: element, at: point) @@ -238,8 +267,38 @@ public class OctreeNode: CustomStringConvertible { } } } + + func tryAdd(_ element: T, in region: MBBox) -> MBOctreeNode? { + // nur einfügen, wenn die Region in diesen Knoten passt + guard region.isContained(in: box) else { return nil } + switch type { + case .internal: + // wenn ein Kind die Region komplett enthält, dort weiterleiten + for child in childrenNodes { + if region.isContained(in: child.box) { + return child.tryAdd(element, in: region) + } + } + // ansonsten hier speichern + regionElements.append((element: element, region: region)) + return self + case .leaf: + // Convert leaf to internal node to redistribute point-based elements + type = .internal(children: Children(parentNode: self)) + if let p = point, let els = elements { + for e in els { + _ = tryAdd(e, at: p) + } + } + // Reset leaf status + elements = nil + point = nil + // dann Region hier einfügen + return tryAdd(element, in: region) + } + } - func add(_ elements: [T], at point: vector_double3) { + func add(_ elements: [T], at point: SIMD3) { for element in elements { self.add(element, at: point) } @@ -247,24 +306,25 @@ public class OctreeNode: CustomStringConvertible { @discardableResult func remove(_ element: T) -> Bool { + // Region-based removal: remove from regionElements if present + if let idx = regionElements.firstIndex(where: { $0.element == element }) { + regionElements.remove(at: idx) + return true + } + switch type { case .leaf: - if let elements = self.elements { - // leaf contains one ore more elements - if let index = elements.index(of: element) { - // leaf contains the element we want to remove - self.elements.remove(at: index) - // if elements is now empty remove it - if self.elements.isEmpty { - self.elements = nil - } - return true - } + guard let index = self.elements?.firstIndex(of: element) else { return false } + self.elements?.remove(at: index) + if self.elements?.isEmpty == true { + self.elements = nil + self.point = nil } - return false - case .internal(let children): - for child in children { + return true + case .internal: + for child in childrenNodes { if child.remove(element) { + collapseIfEmpty() return true } } @@ -272,24 +332,32 @@ public class OctreeNode: CustomStringConvertible { } } - func elements(at point: vector_double3) -> [T]? { + func elements(at point: SIMD3) -> [T]? { + var result: [T] = [] switch type { case .leaf: if self.point == point { - return self.elements + if let els = self.elements { + result.append(contentsOf: els) + } } - case .internal(let children): - for child in children { + case .internal: + for child in childrenNodes { if child.box.contains(point) { - return child.elements(at: point) + if let els = child.elements(at: point) { + result.append(contentsOf: els) + } } } } - // tree does not contain given point - return nil + // Include region-based elements + for (elem, region) in regionElements where region.contains(point) { + result.append(elem) + } + return result.isEmpty ? nil : result } - func elements(in box: Box) -> [T]? { + func elements(in box: MBBox) -> [T]? { var values: [T] = [] switch type { case .leaf: @@ -300,59 +368,107 @@ public class OctreeNode: CustomStringConvertible { values += elements ?? [] } } - case .internal(let children): - for child in children { + case .internal: + // Reserve approximate capacity for efficiency + values.reserveCapacity(values.count + childrenNodes.count) + for child in childrenNodes { if child.box.isContained(in: box) { - // child is contained in box - // add all children of child - values += child.elements(in: child.box) ?? [] - } else if child.box.contains(box) || child.box.intersects(box) { - // child contains at least part of box + // Complete subtree case: collect all elements + values += child.collectAllElements() + } else if child.box.intersects(box) { + // Partial overlap case: recursive query values += child.elements(in: box) ?? [] } // child does not contain any part of given box } } + // Also include region-based elements + for (elem, region) in regionElements where region.intersects(box) { + values.append(elem) + } if values.isEmpty { return nil } return values } - private func subdivide(adding element: T, at point: vector_double3) -> OctreeNode? { - precondition(self.elements != nil, "Subdividing while leaf does not contain a element") - precondition(self.point != nil, "Subdividing while leaf does not contain a point") + private func subdivide(adding element: T, at point: SIMD3) -> MBOctreeNode? { + guard let oldElements = self.elements, let oldPoint = self.point else { + NSLog("⚠️ Inconsistent state: subdivide on empty leaf") + self.elements = [element] + self.point = point + return self + } + type = .internal(children: Children(parentNode: self)) + self.elements = nil + self.point = nil + self.add(oldElements, at: oldPoint) + return self.add(element, at: point) + } + + /// Collapses internal node to empty leaf if all children are empty leaves + private func collapseIfEmpty() { + guard case .internal(let children) = type else { return } + var allEmpty = true + for child in children { + if case .leaf = child.type { + if let els = child.elements, !els.isEmpty { + allEmpty = false + break + } + } else { + allEmpty = false + break + } + } + if allEmpty { + type = .leaf + elements = nil + point = nil + } + } + + /// Sammelt alle Elemente dieses Knotens und seiner Nachkommen + func collectAllElements() -> [T] { + var result: [T] = [] switch type { case .leaf: - type = .internal(children: Children(parentNode: self)) - // add element previously contained in leaf to children - self.add(self.elements, at: self.point) - self.elements = nil - self.point = nil - // add new element to children - return self.add(element, at: point) - case .internal: - preconditionFailure("Calling subdivide on an internal node") + if let els = elements { + result.append(contentsOf: els) + } + case .internal(let children): + for child in children { + result.append(contentsOf: child.collectAllElements()) + } } + return result } } -public class Octree: CustomStringConvertible { - var root: OctreeNode +public class MBOctree: CustomStringConvertible { + private let minimumCellSize: Double + var root: MBOctreeNode public var description: String { return "Octree\n" + root.recursiveDescription } - public init(boundingBox: Box, minimumCellSize: Double) { - root = OctreeNode(box: boundingBox) + public init(boundingBox: MBBox, minimumCellSize: Double) { + self.minimumCellSize = minimumCellSize + root = MBOctreeNode(box: boundingBox, minimumCellSize: minimumCellSize) } @discardableResult - public func add(_ element: T, at point: vector_double3) -> OctreeNode { + public func add(_ element: T, at point: SIMD3) -> MBOctreeNode? { return root.add(element, at: point) } + + /// Adds an element that occupies a region of space + @discardableResult + public func add(_ element: T, in region: MBBox) -> MBOctreeNode? { + return root.add(element, in: region) + } @discardableResult - public func remove(_ element: T, using node: OctreeNode) -> Bool { + public func remove(_ element: T, using node: MBOctreeNode) -> Bool { return node.remove(element) } @@ -361,12 +477,12 @@ public class Octree: CustomStringConvertible { return root.remove(element) } - public func elements(at point: vector_double3) -> [T]? { + public func elements(at point: SIMD3) -> [T]? { return root.elements(at: point) } - public func elements(in box: Box) -> [T]? { - precondition(root.box.contains(box), "box is outside of octree bounds") + public func elements(in box: MBBox) -> [T]? { + precondition(box.isContained(in: root.box), "box is outside of octree bounds") return root.elements(in: box) } }