├── .gitignore ├── Package.swift ├── README.md ├── Sources └── Graph3D │ ├── Graph3D.swift │ ├── RouteVertex.swift │ └── SIMD3+Extensions.swift └── Tests ├── Graph3DTests ├── Graph3DTests.swift └── XCTestManifests.swift └── LinuxMain.swift /.gitignore: -------------------------------------------------------------------------------- 1 | # Xcode 2 | # 3 | # gitignore contributors: remember to update Global/Xcode.gitignore, Objective-C.gitignore & Swift.gitignore 4 | 5 | ## Build generated 6 | build/ 7 | DerivedData/ 8 | 9 | ## Various settings 10 | *.pbxuser 11 | !default.pbxuser 12 | *.mode1v3 13 | !default.mode1v3 14 | *.mode2v3 15 | !default.mode2v3 16 | *.perspectivev3 17 | !default.perspectivev3 18 | xcuserdata/ 19 | 20 | ## Other 21 | *.moved-aside 22 | *.xccheckout 23 | *.xcscmblueprint 24 | 25 | ## Obj-C/Swift specific 26 | *.hmap 27 | *.ipa 28 | *.dSYM.zip 29 | *.dSYM 30 | 31 | ## Playgrounds 32 | timeline.xctimeline 33 | playground.xcworkspace 34 | 35 | # Swift Package Manager 36 | # 37 | # Add this line if you want to avoid checking in source code from Swift Package Manager dependencies. 38 | # Packages/ 39 | # Package.pins 40 | # Package.resolved 41 | .build/ 42 | 43 | # CocoaPods 44 | # 45 | # We recommend against adding the Pods directory to your .gitignore. However 46 | # you should judge for yourself, the pros and cons are mentioned at: 47 | # https://guides.cocoapods.org/using/using-cocoapods.html#should-i-check-the-pods-directory-into-source-control 48 | # 49 | Pods/ 50 | Podfile.lock 51 | *.xcworkspace 52 | 53 | # Carthage 54 | # 55 | # Add this line if you want to avoid checking in source code from Carthage dependencies. 56 | # Carthage/Checkouts 57 | 58 | Carthage/Build 59 | 60 | # fastlane 61 | # 62 | # It is recommended to not store the screenshots in the git repo. Instead, use fastlane to re-generate the 63 | # screenshots whenever they are needed. 64 | # For more information about the recommended setup visit: 65 | # https://docs.fastlane.tools/best-practices/source-control/#source-control 66 | 67 | fastlane/report.xml 68 | fastlane/Preview.html 69 | fastlane/screenshots/**/*.png 70 | fastlane/test_output 71 | -------------------------------------------------------------------------------- /Package.swift: -------------------------------------------------------------------------------- 1 | // swift-tools-version:5.0 2 | // The swift-tools-version declares the minimum version of Swift required to build this package. 3 | 4 | import PackageDescription 5 | 6 | let package = Package( 7 | name: "Graph3D", 8 | platforms: [ 9 | .iOS(.v10), 10 | .macOS(.v10_13) 11 | ], 12 | products: [ 13 | // Products define the executables and libraries produced by a package, and make them visible to other packages. 14 | .library( 15 | name: "Graph3D", 16 | targets: ["Graph3D"]), 17 | ], 18 | dependencies: [ 19 | // Dependencies declare other packages that this package depends on. 20 | // .package(url: /* package url */, from: "1.0.0"), 21 | ], 22 | targets: [ 23 | // Targets are the basic building blocks of a package. A target can define a module or a test suite. 24 | // Targets can depend on other targets in this package, and on products in packages which this package depends on. 25 | .target( 26 | name: "Graph3D", 27 | dependencies: []), 28 | .testTarget( 29 | name: "Graph3DTests", 30 | dependencies: ["Graph3D"]), 31 | ] 32 | ) 33 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Graph3D 2 | 3 | A package building upon iOS Framework `GameplayKit`'s class `GKGraph` to create a routing graph specific for navigating 3D positions. 4 | 5 | I have used this previously for Augmented Reality projects, and thought it could be useful for other developers. 6 | 7 | Check out [the tests](https://github.com/maxxfrazer/Swift-Graph3D/blob/master/Tests/Graph3DTests/Graph3DTests.swift) for examples how to use this package. 8 | 9 | Please feel free to test it out and contribute to this repository if there are some improvements you'd like to see. 10 | -------------------------------------------------------------------------------- /Sources/Graph3D/Graph3D.swift: -------------------------------------------------------------------------------- 1 | // 2 | // Graph3D.swift 3 | // 4 | // 5 | // Created by Max Cobb on 7/21/19. 6 | // 7 | 8 | import GameplayKit 9 | 10 | public enum RouterError: Error { 11 | case noVertices 12 | case badVertex 13 | case noEdges 14 | case badEdge 15 | case emptyListOfDestinations 16 | case edgeOutOfBounds 17 | case noShortestPath 18 | case failedToParse 19 | case couldNotRoute 20 | case noRouting 21 | } 22 | 23 | extension Array where Element: RouteVertex { 24 | internal func length() -> Float { 25 | let arr = self as [RouteVertex] 26 | return arr.enumerated().map({ (arg) -> Float in 27 | let (index, value) = arg 28 | return index > 0 ? value.distance(to: arr[index - 1]) : 0 29 | }).reduce(0, +) 30 | } 31 | } 32 | extension Array where Element: SIMD { 33 | internal func length() -> Float { 34 | let arr = self as! [SIMD3] 35 | return arr.enumerated().map({ (arg) -> Float in 36 | let (index, value) = arg 37 | return index > 0 ? value.distance(to: arr[index - 1]) : 0 38 | }).reduce(0, +) 39 | } 40 | } 41 | 42 | public class Graph3D: GKGraph { 43 | var edges = [[Int]]() 44 | var vertices = [[Float]]() 45 | internal var vertexNodes = [RouteVertex]() 46 | 47 | public convenience init(vertices: [[Double]], edges: [[Int]]) { 48 | self.init( 49 | vertices: vertices.map{$0.map { Float($0)}}, 50 | edges: edges 51 | ) 52 | } 53 | 54 | public init(vertices: [[Float]], edges: [[Int]]) { 55 | super.init() 56 | self.edges = edges 57 | self.vertices = vertices 58 | let nodes = vertices.map { vertice in RouteVertex(position: vertice) } 59 | edges.forEach { edge in 60 | nodes[edge[0]].addConnection(to: nodes[edge[1]]) 61 | } 62 | self.vertexNodes = nodes 63 | } 64 | 65 | 66 | public func findNearestNode(to position: SIMD3, between nodes: [RouteVertex]) -> RouteVertex { 67 | let newNode = RouteVertex(position) 68 | return nodes.reduce(nodes[0]) { (best, next) -> RouteVertex in 69 | return newNode.cost(to: next) < newNode.cost(to: best) ? next : best 70 | } 71 | } 72 | 73 | public func findNearestNode(to position: SIMD3) -> RouteVertex { 74 | return self.findNearestNode(to: position, between: self.vertexNodes) 75 | } 76 | 77 | 78 | /// Find the closest point on the graph to a given point 79 | public func findPointOnEdge(from startPoint: SIMD3) -> (SIMD3, [RouteVertex]) { 80 | var closestPoint = self.vertexNodes[edges[0][0]].getVectorPos() 81 | var closestDistance = startPoint.distance(to: closestPoint) 82 | var matchingNodes = [self.vertexNodes[edges[0][0]], self.vertexNodes[edges[0][1]]] 83 | for point in edges { 84 | let node1 = self.vertexNodes[point[0]] 85 | let node2 = self.vertexNodes[point[1]] 86 | let p1 = node1.getVectorPos() 87 | let p2 = node2.getVectorPos() 88 | if startPoint == p1 { 89 | return (p1, [node1]) 90 | } else if startPoint == p2 { 91 | return (p2, [node2]) 92 | } 93 | 94 | let newPoint = startPoint.onLine(start: p1, end: p2) 95 | 96 | let newDistance = newPoint.distance(to: startPoint) 97 | if closestDistance > newDistance { 98 | closestPoint = newPoint 99 | closestDistance = newDistance 100 | matchingNodes = [node1, node2] 101 | } 102 | } 103 | return (closestPoint, matchingNodes) 104 | } 105 | 106 | internal func doRouting(from worldPosition: SIMD3, to targetPosition: SIMD3) -> [SIMD3] 107 | { 108 | let (edgeStart, endsOfEdgeStart) = self.findPointOnEdge(from: worldPosition) 109 | let (edgeEnd, endsOfEdgeEnd) = self.findPointOnEdge(from: targetPosition) 110 | var allPaths = [[RouteVertex]]() 111 | let startEdgesSet = Set(endsOfEdgeStart) 112 | let endEdgesSet = Set(endsOfEdgeEnd) 113 | let endsOfEdgesIntersection = startEdgesSet.intersection(endEdgesSet) 114 | if endsOfEdgesIntersection.count > 0 { 115 | var myResult: [SIMD3] = [edgeStart, edgeEnd] 116 | if endsOfEdgesIntersection.count == 1, let firstIntersection = endsOfEdgesIntersection.first { 117 | myResult.insert(firstIntersection.getVectorPos(), at: 1) 118 | } 119 | if let firstResult = myResult.first, firstResult != worldPosition && firstResult.distance(to: worldPosition) > 0.5 { 120 | myResult.insert(SIMD3(worldPosition.x, firstResult.y, worldPosition.z), at: 0) 121 | } 122 | return myResult 123 | } 124 | for endStart in endsOfEdgeStart { 125 | for endEnd in endsOfEdgeEnd { 126 | allPaths.append(self.findPath(from: endStart, to: endEnd) as! [RouteVertex]) 127 | } 128 | } 129 | 130 | var newPath = allPaths.reduce(nil) { (bestPath, nextPath) -> [SIMD3] in 131 | var nextval = nextPath.length() 132 | nextval += edgeStart.distance(to: nextPath[0].getVectorPos()) 133 | nextval += edgeEnd.distance(to: nextPath.last!.getVectorPos()) 134 | if let bestPath = bestPath, nextval >= bestPath.length() { 135 | return bestPath 136 | } 137 | var newBest = nextPath.map { $0.getVectorPos() } 138 | if newBest.first! != edgeStart { 139 | newBest.insert(edgeStart, at: 0) 140 | } 141 | if newBest.last! != edgeEnd { 142 | newBest.append(edgeEnd) 143 | } 144 | return newBest 145 | } 146 | if let pathFirst = newPath?.first, pathFirst != worldPosition, pathFirst.distance(to: worldPosition) > 0.5 { 147 | newPath?.insert(SIMD3(worldPosition.x, pathFirst.y, worldPosition.z), at: 0) 148 | } 149 | return newPath! 150 | } 151 | required public init?(coder aDecoder: NSCoder) { 152 | fatalError("init(coder:) has not been implemented") 153 | } 154 | } 155 | 156 | public func parseRouter(data: Data) throws -> Graph3D { 157 | do { 158 | if let parsedJson = try JSONSerialization.jsonObject(with: data) as? [String: Any] { 159 | if let jsonBlock = parsedJson["routing"] as? [String: Any] { 160 | return try parseRouter(jsonBlock: jsonBlock) 161 | } else { 162 | throw RouterError.noRouting 163 | } 164 | } else { 165 | throw RouterError.failedToParse 166 | } 167 | } catch { 168 | print("could not serialize json \(error)") 169 | throw RouterError.failedToParse 170 | } 171 | } 172 | 173 | 174 | internal func parseRouter(jsonBlock: [String: Any]) throws -> Graph3D { 175 | if let vertices = jsonBlock["vertices"] as? [[Double]], let edges = jsonBlock["edges"] as? [[Int]] { 176 | if vertices.count < 2 { 177 | throw vertices.isEmpty ? RouterError.noVertices : RouterError.noEdges 178 | } 179 | for v in vertices { 180 | if v.count != 3 { 181 | throw RouterError.badVertex 182 | } 183 | } 184 | for e in edges { 185 | if e.count != 2 { 186 | throw RouterError.badEdge 187 | } else if e[0] >= vertices.count || e[1] >= vertices.count { 188 | throw RouterError.edgeOutOfBounds 189 | } 190 | } 191 | 192 | let router = Graph3D(vertices: vertices, edges: edges) 193 | return router 194 | } else { 195 | throw RouterError.failedToParse 196 | } 197 | } 198 | -------------------------------------------------------------------------------- /Sources/Graph3D/RouteVertex.swift: -------------------------------------------------------------------------------- 1 | // 2 | // RouteVertex.swift 3 | // 4 | // 5 | // Created by Max Cobb on 7/21/19. 6 | // 7 | 8 | import GameplayKit.GKGraph 9 | 10 | public class RouteVertex: GKGraphNode3D { 11 | var travelCost: [GKGraphNode: Float] = [:] 12 | 13 | var simdPos: SIMD3 { 14 | return SIMD3(self.position) 15 | } 16 | 17 | internal convenience init(position: [Float]) { 18 | self.init(point: vector_float3(position)) 19 | } 20 | 21 | internal convenience init(_ position: SIMD3) { 22 | self.init(point: vector_float3(position)) 23 | } 24 | 25 | internal override init(point: vector_float3) { 26 | super.init(point: point) 27 | } 28 | 29 | required init?(coder aDecoder: NSCoder) { 30 | fatalError("init(coder:) has not been implemented") 31 | } 32 | 33 | func getVectorPos() -> SIMD3 { 34 | return SIMD3(self.position) 35 | } 36 | 37 | func addConnection(to node: RouteVertex) { 38 | self.addConnections(to: [node], bidirectional: true) 39 | let weight = self.distance(to: node) 40 | travelCost[node] = weight 41 | node.travelCost[self] = weight 42 | } 43 | func distance(to: RouteVertex) -> Float { 44 | let pos = SIMD3(self.position) 45 | return pos.distance(to: SIMD3(to.position)) 46 | } 47 | func distanceSq(to: RouteVertex) -> Float { 48 | return self.simdPos.distanceSq(to: to.simdPos) 49 | } 50 | 51 | func cost(to node: RouteVertex) -> Float { 52 | return self.distance(to: node) 53 | } 54 | 55 | override public func cost(to node: GKGraphNode) -> Float { 56 | if let node = node as? RouteVertex { 57 | return self.distance(to: node) 58 | } else { 59 | return super.cost(to: node) 60 | } 61 | } 62 | } 63 | -------------------------------------------------------------------------------- /Sources/Graph3D/SIMD3+Extensions.swift: -------------------------------------------------------------------------------- 1 | // 2 | // SIMD3+Extensions.swift 3 | // 4 | // 5 | // Created by Max Cobb on 7/21/19. 6 | // 7 | 8 | import Foundation 9 | import simd 10 | 11 | internal extension SIMD3 where SIMD3.Scalar: FloatingPoint { 12 | func lengthSq() -> Scalar { 13 | var lengthSq = self.x * self.x 14 | lengthSq += self.y * self.y 15 | lengthSq += self.z * self.z 16 | return lengthSq 17 | } 18 | func length() -> Scalar { 19 | return sqrt(self.lengthSq()) 20 | } 21 | /** 22 | * Calculates the dot product between two SIMD3. 23 | */ 24 | func dot(vector: SIMD3) -> Scalar { 25 | return x * vector.x + y * vector.y + z * vector.z 26 | } 27 | 28 | func distanceSq(to point: SIMD3) -> Scalar { 29 | return (self - point).lengthSq() 30 | } 31 | 32 | func distance(to point: SIMD3) -> Scalar { 33 | return sqrt(distanceSq(to: point)) 34 | } 35 | func onLine(start: SIMD3, end: SIMD3) -> SIMD3 { 36 | let lineDist = (start - end).lengthSq() 37 | let selfStart = self - start 38 | let endStart = end - start 39 | let dotProd = selfStart.dot(vector: endStart) 40 | var t = dotProd / lineDist 41 | if t < 0 { 42 | t = 0 43 | } else if t > 1 { 44 | t = 1 45 | } 46 | return end * t + start * (1 - t) 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /Tests/Graph3DTests/Graph3DTests.swift: -------------------------------------------------------------------------------- 1 | import XCTest 2 | @testable import Graph3D 3 | 4 | final class Graph3DTests: XCTestCase { 5 | 6 | var basicGraph: Graph3D { 7 | let vertices: [[Float]] = [[1,1,1], [2,2,2], [3,3,3]] 8 | return Graph3D(vertices: vertices, edges: [[0, 1], [1,2]]) 9 | } 10 | var flatGridGraph: Graph3D { 11 | let vertices: [[Float]] = [ 12 | [0,0,1], [1,0,1], [2,0,1], [3,0,1], 13 | [0,1,1], [1,1,1], [2,1,1], [3,1,1], 14 | [0,2,1], [1,2,1], [2,2,1], [3,2,1], 15 | [0,3,1], [1,3,1], [2,3,1], [3,3,1], 16 | [0,4,1], [1,4,1], [2,4,1], [3,4,1], 17 | [0,5,1], [1,5,1], [2,5,1], [3,5,1], 18 | ] 19 | let edges: [[Int]] = [ 20 | [0,1], [0,4], [1,2], [1,5], [2,3], [2,6], 21 | [3,7], [4,5], [4,8], [5,6], [5,9], [6,7], 22 | [6,10], [7,11], [8,9], [8,12], [9,10], [9,13], 23 | [10,11], [10,14], [11,15], [12,13], [12,16], 24 | [13,14], [13,17], [14,15], [14,18], [15,19], 25 | [16,17], [16,20], [17,18], [17,21], [18,19], 26 | [18,22], [19,23], [20,21], [21,22], [22,23] 27 | ] 28 | return Graph3D(vertices: vertices, edges: edges) 29 | } 30 | 31 | func testNearestNode() { 32 | let myGraph = self.basicGraph 33 | let nearestNode = myGraph.findNearestNode(to: [2,3,2]) 34 | XCTAssertEqual(nearestNode.position, [2,2,2]) 35 | } 36 | 37 | func testManhattanDistance() { 38 | let myGraph = self.flatGridGraph 39 | let path = myGraph.doRouting(from: [0,0,1], to: [3,5,1]) 40 | XCTAssertEqual(9, path.count, 41 | "top left to bottom right returned \(path.count), should be 9" 42 | ) 43 | } 44 | 45 | static var allTests = [ 46 | ("testNearestNode", testNearestNode), 47 | ("testManhattanDistance", testManhattanDistance), 48 | ] 49 | } 50 | -------------------------------------------------------------------------------- /Tests/Graph3DTests/XCTestManifests.swift: -------------------------------------------------------------------------------- 1 | import XCTest 2 | 3 | #if !canImport(ObjectiveC) 4 | public func allTests() -> [XCTestCaseEntry] { 5 | return [ 6 | testCase(Graph3DTests.allTests), 7 | ] 8 | } 9 | #endif 10 | -------------------------------------------------------------------------------- /Tests/LinuxMain.swift: -------------------------------------------------------------------------------- 1 | import XCTest 2 | 3 | import Graph3DTests 4 | 5 | var tests = [XCTestCaseEntry]() 6 | tests += Graph3DTests.allTests() 7 | XCTMain(tests) 8 | --------------------------------------------------------------------------------