[Feature] Add PhysicsQuery raycast facade with octree fallback

Sources/UntoldEngine/Physics/PhysicsQuery.swift

@@ -0,0 +1,140 @@
+//
+//  PhysicsQuery.swift
+//  UntoldEngine
+//
+// Copyright (C) Untold Engine Studios
+//
+// This Source Code Form is subject to the terms of the Mozilla Public
+// License, v. 2.0. If a copy of the MPL was not distributed with this
+// file, You can obtain one at https://mozilla.org/MPL/2.0/.
+
+import Foundation
+import simd
+
+/// Scene queries against the physics world.
+///
+/// `raycast` routes to the active `PhysicsBackend` when it reports the
+/// `.raycast` capability, so results come from real collider geometry. With no
+/// backend installed — or a backend without the capability — it falls back to a
+/// best-effort query against the engine's octree of entity bounds.
+///
+/// Fallback semantics (documented contract, intentionally approximate):
+/// - Hits are entity **AABBs**, not collider shapes; `position`/`normal` are on
+///   the box surface (the normal is the hit face's axis direction). A ray that
+///   starts inside a box reports the hit at the ray origin with `distance` 0
+///   and the normal pointing back along the ray.
+/// - `filter.excludedEntities` is always honored. `filter.layerMask` is tested
+///   against `RigidBodyComponent.layer` interpreted as a layer *index* (bit
+///   `1 << layer` in the mask); entities without a `RigidBodyComponent` are
+///   treated as layer 0, and layers ≥ 32 cannot be expressed in the mask and
+///   always pass.
+/// - Only entities registered with `OctreeSystem` (those with render bounds)
+///   are considered.
+///
+/// Shapecast and overlap queries are deliberately not exposed yet; the
+/// capability bits exist so backends can declare them ahead of phase 2.
+public enum PhysicsQuery {
+    /// Longest segment the octree fallback searches. `PhysicsRay`'s
+    /// "unbounded" sentinel is `.greatestFiniteMagnitude` — which is finite —
+    /// so the cap is applied with `min`, not a finiteness test; 1e6 m is
+    /// comfortably beyond the octree's world bounds.
+    static let fallbackUnboundedDistance: Float = 1.0e6
+
+    public static func raycast(
+        _ ray: PhysicsRay,
+        filter: PhysicsQueryFilter = PhysicsQueryFilter()
+    ) -> PhysicsRayHit? {
+        if let backend = PhysicsBackendRegistry.shared.activeBackend(),
+           backend.capabilities.contains(.raycast)
+        {
+            return backend.raycast(ray, filter: filter)
+        }
+        return octreeRaycast(ray, filter: filter)
+    }
+
+    // MARK: - Octree fallback
+
+    static func octreeRaycast(
+        _ ray: PhysicsRay,
+        filter: PhysicsQueryFilter
+    ) -> PhysicsRayHit? {
+        let directionLength = simd_length(ray.direction)
+        guard directionLength > .ulpOfOne else { return nil }
+        let direction = ray.direction / directionLength
+
+        let maxDistance = min(ray.maxDistance, fallbackUnboundedDistance)
+        guard maxDistance > 0 else { return nil }
+
+        // Tree-pruned broad phase: candidates arrive sorted by their ray-AABB
+        // distance. For a ray starting outside a box that distance equals the
+        // hit distance below; for a ray starting inside it is the exit
+        // distance, an upper bound of the reported 0 — so in both cases it
+        // never undercuts the final distance, and the scan can stop as soon
+        // as the sorted distance passes the best hit found.
+        var best: PhysicsRayHit?
+        for (entity, sortedDistance) in OctreeSystem.shared.query(
+            rayOrigin: ray.origin,
+            rayDirection: direction,
+            maxDistance: maxDistance
+        ) {
+            if let currentBest = best, sortedDistance >= currentBest.distance { break }
+            guard passesFilter(entity, filter),
+                  let bounds = OctreeSystem.shared.getBounds(for: entity)
+            else { continue }
+
+            // Narrow phase recomputes the entry distance: the broad-phase
+            // value is the exit distance for inside-the-box origins, where
+            // the documented contract reports the hit at the origin instead.
+            var tmin: Float = 0
+            guard rayIntersectsAABB(
+                rayOrigin: ray.origin,
+                rayDir: direction,
+                boxMin: bounds.min,
+                boxMax: bounds.max,
+                tmin: &tmin
+            ) else { continue }
+
+            let distance = max(0.0, tmin)
+            guard distance <= maxDistance else { continue }
+            if let currentBest = best, currentBest.distance <= distance { continue }
+
+            let position = ray.origin + direction * distance
+            let normal = distance > 0
+                ? boxFaceNormal(at: position, bounds: bounds)
+                : -direction
+            best = PhysicsRayHit(
+                entity: entity,
+                position: position,
+                normal: normal,
+                distance: distance
+            )
+        }
+        return best
+    }
+
+    private static func passesFilter(_ entity: EntityID, _ filter: PhysicsQueryFilter) -> Bool {
+        if filter.excludedEntities.contains(entity) { return false }
+        guard filter.layerMask != .max else { return true }
+
+        let layer = scene.get(component: RigidBodyComponent.self, for: entity)?.layer ?? 0
+        guard layer < 32 else { return true }
+        return (filter.layerMask >> layer) & 1 != 0
+    }
+
+    /// Axis-aligned face normal of the box face nearest to a surface point.
+    private static func boxFaceNormal(at point: simd_float3, bounds: AABB) -> simd_float3 {
+        let halfExtents = simd_max((bounds.max - bounds.min) * 0.5, simd_float3(repeating: .ulpOfOne))
+        let local = (point - bounds.center) / halfExtents
+
+        var normal = simd_float3(local.x < 0 ? -1 : 1, 0, 0)
+        var strongest = abs(local.x)
+        if abs(local.y) > strongest {
+            strongest = abs(local.y)
+            normal = simd_float3(0, local.y < 0 ? -1 : 1, 0)
+        }
+        if abs(local.z) > strongest {
+            normal = simd_float3(0, 0, local.z < 0 ? -1 : 1)
+        }
+        return normal
+    }
+}

Tests/UntoldEngineTests/PhysicsQueryTests.swift

@@ -0,0 +1,224 @@
+//
+//  PhysicsQueryTests.swift
+//  UntoldEngineTests
+//
+// Copyright (C) Untold Engine Studios
+//
+// This Source Code Form is subject to the terms of the Mozilla Public
+// License, v. 2.0. If a copy of the MPL was not distributed with this
+// file, You can obtain one at https://mozilla.org/MPL/2.0/.
+
+import simd
+@testable import UntoldEngine
+import XCTest
+
+private final class RaycastPhysicsBackend: PhysicsBackend, @unchecked Sendable {
+    let id: String
+    let capabilities: PhysicsCapabilities
+
+    var cannedHit: PhysicsRayHit?
+    private(set) var raycastCallCount = 0
+
+    init(id: String = "com.example.raycastphysics", capabilities: PhysicsCapabilities) {
+        self.id = id
+        self.capabilities = capabilities
+    }
+
+    func configure(_: PhysicsWorldConfiguration) {}
+
+    func step(deltaTime _: Float) {}
+
+    func raycast(_: PhysicsRay, filter _: PhysicsQueryFilter) -> PhysicsRayHit? {
+        raycastCallCount += 1
+        return cannedHit
+    }
+}
+
+private struct RaycastBackendPlugin: PhysicsBackendPlugin {
+    let manifest: PhysicsBackendPluginManifest
+    let backend: RaycastPhysicsBackend
+
+    init(pluginID: String = "com.example.raycastphysics", capabilities: PhysicsCapabilities) {
+        manifest = PhysicsBackendPluginManifest(
+            id: pluginID,
+            displayName: "Raycast Physics",
+            version: PhysicsBackendVersion(major: 1, minor: 0, patch: 0),
+            requiredAPIVersion: .current
+        )
+        backend = RaycastPhysicsBackend(id: pluginID, capabilities: capabilities)
+    }
+
+    func makeBackend() -> any PhysicsBackend {
+        backend
+    }
+}
+
+@MainActor
+final class PhysicsQueryTests: XCTestCase {
+    override func setUp() async throws {
+        resetEngineTestState()
+    }
+
+    override func tearDown() {
+        PhysicsBackendRegistry.shared.resetForTesting()
+        super.tearDown()
+    }
+
+    /// Creates an entity with world-space AABB `center ± halfExtents`,
+    /// registered with the octree the same way renderable entities are.
+    private func makeObstacle(
+        center: simd_float3,
+        halfExtents: simd_float3 = simd_float3(repeating: 0.5)
+    ) -> EntityID {
+        let entityId = createEntity()
+        scene.get(component: LocalTransformComponent.self, for: entityId)?.boundingBox =
+            (min: -halfExtents, max: halfExtents)
+        if let worldTransform = scene.get(component: WorldTransformComponent.self, for: entityId) {
+            var space = matrix_identity_float4x4
+            space.columns.3 = simd_float4(center.x, center.y, center.z, 1.0)
+            worldTransform.space = space
+        }
+        OctreeSystem.shared.registerEntity(entityId)
+        return entityId
+    }
+
+    // MARK: - Octree fallback
+
+    func testFallbackReturnsNearestHitWithSurfaceData() {
+        let near = makeObstacle(center: simd_float3(0.0, 0.0, -5.0))
+        _ = makeObstacle(center: simd_float3(0.0, 0.0, -10.0))
+
+        let hit = PhysicsQuery.raycast(
+            PhysicsRay(origin: .zero, direction: simd_float3(0.0, 0.0, -1.0))
+        )
+
+        XCTAssertEqual(hit?.entity, near)
+        XCTAssertEqual(hit?.distance ?? 0, 4.5, accuracy: 1.0e-4)
+        XCTAssertEqual(hit?.position.z ?? 0, -4.5, accuracy: 1.0e-4)
+        XCTAssertEqual(hit?.normal, simd_float3(0.0, 0.0, 1.0))
+    }
+
+    func testFallbackNormalizesDirectionAndReportsWorldDistance() {
+        _ = makeObstacle(center: simd_float3(0.0, 0.0, -5.0))
+
+        let hit = PhysicsQuery.raycast(
+            PhysicsRay(origin: .zero, direction: simd_float3(0.0, 0.0, -8.0))
+        )
+
+        XCTAssertEqual(hit?.distance ?? 0, 4.5, accuracy: 1.0e-4)
+    }
+
+    func testFallbackHonorsMaxDistance() {
+        _ = makeObstacle(center: simd_float3(0.0, 0.0, -5.0))
+
+        let hit = PhysicsQuery.raycast(
+            PhysicsRay(origin: .zero, direction: simd_float3(0.0, 0.0, -1.0), maxDistance: 3.0)
+        )
+
+        XCTAssertNil(hit)
+    }
+
+    func testFallbackHonorsExcludedEntities() {
+        let near = makeObstacle(center: simd_float3(0.0, 0.0, -5.0))
+        let far = makeObstacle(center: simd_float3(0.0, 0.0, -10.0))
+
+        let hit = PhysicsQuery.raycast(
+            PhysicsRay(origin: .zero, direction: simd_float3(0.0, 0.0, -1.0)),
+            filter: PhysicsQueryFilter(excludedEntities: [near])
+        )
+
+        XCTAssertEqual(hit?.entity, far)
+        XCTAssertEqual(hit?.distance ?? 0, 9.5, accuracy: 1.0e-4)
+    }
+
+    func testFallbackAppliesLayerMaskToBodiesAndPassesBodylessEntities() {
+        let near = makeObstacle(center: simd_float3(0.0, 0.0, -5.0))
+        registerComponent(entityId: near, componentType: RigidBodyComponent.self)
+        scene.get(component: RigidBodyComponent.self, for: near)?.layer = 1
+        let far = makeObstacle(center: simd_float3(0.0, 0.0, -10.0))
+
+        // Mask selects only layer 0: the layer-1 body is skipped, the bodyless
+        // far entity (treated as layer 0) is hit.
+        let maskedHit = PhysicsQuery.raycast(
+            PhysicsRay(origin: .zero, direction: simd_float3(0.0, 0.0, -1.0)),
+            filter: PhysicsQueryFilter(layerMask: 1 << 0)
+        )
+        XCTAssertEqual(maskedHit?.entity, far)
+
+        // The default all-layers mask hits the near body.
+        let openHit = PhysicsQuery.raycast(
+            PhysicsRay(origin: .zero, direction: simd_float3(0.0, 0.0, -1.0))
+        )
+        XCTAssertEqual(openHit?.entity, near)
+    }
+
+    func testFallbackRayStartingInsideBoxHitsAtOrigin() {
+        let box = makeObstacle(center: simd_float3(0.0, 0.0, -0.2), halfExtents: simd_float3(repeating: 1.0))
+
+        let direction = simd_float3(0.0, 0.0, -1.0)
+        let hit = PhysicsQuery.raycast(PhysicsRay(origin: .zero, direction: direction))
+
+        XCTAssertEqual(hit?.entity, box)
+        XCTAssertEqual(hit?.distance, 0.0)
+        XCTAssertEqual(hit?.position, .zero)
+        XCTAssertEqual(hit?.normal, -direction)
+    }
+
+    func testFallbackMissReturnsNil() {
+        _ = makeObstacle(center: simd_float3(0.0, 10.0, -5.0))
+
+        let hit = PhysicsQuery.raycast(
+            PhysicsRay(origin: .zero, direction: simd_float3(0.0, 0.0, -1.0))
+        )
+
+        XCTAssertNil(hit)
+    }
+
+    // MARK: - Backend routing
+
+    func testBackendWithRaycastCapabilityIsAuthoritative() {
+        _ = makeObstacle(center: simd_float3(0.0, 0.0, -5.0))
+
+        let plugin = RaycastBackendPlugin(capabilities: [.raycast])
+        XCTAssertEqual(PhysicsBackendRegistry.shared.install(plugin), .installed)
+        plugin.backend.cannedHit = PhysicsRayHit(
+            entity: 999,
+            position: simd_float3(0.0, 0.0, -2.0),
+            normal: simd_float3(0.0, 0.0, 1.0),
+            distance: 2.0
+        )
+
+        let hit = PhysicsQuery.raycast(
+            PhysicsRay(origin: .zero, direction: simd_float3(0.0, 0.0, -1.0))
+        )
+
+        XCTAssertEqual(plugin.backend.raycastCallCount, 1)
+        XCTAssertEqual(hit?.entity, 999, "A capable backend's answer wins over the octree")
+
+        // A capable backend's miss is also authoritative — no octree fallback.
+        plugin.backend.cannedHit = nil
+        XCTAssertNil(PhysicsQuery.raycast(
+            PhysicsRay(origin: .zero, direction: simd_float3(0.0, 0.0, -1.0))
+        ))
+    }
+
+    func testBackendWithoutRaycastCapabilityFallsBackToOctree() {
+        let obstacle = makeObstacle(center: simd_float3(0.0, 0.0, -5.0))
+
+        let plugin = RaycastBackendPlugin(capabilities: [])
+        XCTAssertEqual(PhysicsBackendRegistry.shared.install(plugin), .installed)
+        plugin.backend.cannedHit = PhysicsRayHit(
+            entity: 999,
+            position: .zero,
+            normal: simd_float3(0.0, 1.0, 0.0),
+            distance: 1.0
+        )
+
+        let hit = PhysicsQuery.raycast(
+            PhysicsRay(origin: .zero, direction: simd_float3(0.0, 0.0, -1.0))
+        )
+
+        XCTAssertEqual(plugin.backend.raycastCallCount, 0, "Backends without .raycast are never asked")
+        XCTAssertEqual(hit?.entity, obstacle)
+    }
+}