GameplayKit Pathfinding

navmesh.swift

//
//  PKNavmesh.swift
//  PathKit
//
//  Created by Ryan Campbell on 9/12/16.
//  Copyright © 2016 Phalanx Studios. All rights reserved.
//

import SpriteKit
import GameplayKit

#if os(iOS)
    import Particleboard
#elseif os(OSX)
    import ParticleboardOS
#endif

/**
 Extension of GKMeshGraph that has built in utility methods for pathfinding. For example, if the desired point is inside of an obstacle, you can optionally find the
 closest point on the graph.
 */
public class PKNavmesh : GKMeshGraph<GKGraphNode2D> {
    
    public var meshObstacles = [PKMeshObstacle]()
    
    public var collider : UInt32 = 0
    
    public var buffer : Float = 0
    
    // MARK: Initializing a Navmesh
    
    public init(meshObstacles:[PKMeshObstacle], obstacles:[GKPolygonObstacle], scene:SKScene, bgLayer:SKNode, bufferRadius:Float, collider:UInt32, size:CGSize) {
        /// Add / subtract 250 from map size so that obstacle buffers off screen to trigger gameplaykit bug
        super.init(bufferRadius: bufferRadius, minCoordinate: float2(x:-250, y:-250), maxCoordinate: float2(x:Float(size.width) + 250, y:Float(size.height) + 250))
        self.buffer = bufferRadius
        self.meshObstacles = meshObstacles
        self.collider = collider
        self.triangulationMode = [.vertices]
        
        self.addObstacles(obstacles)
        
        self.triangulate()
        
        self.removeNodesWithNoSceneConnection(scene, layer: bgLayer)
        
        self.debugOverlay(scene: scene)
        ///self.debugTriangles(scene: scene)

    }
    
    
    override init(bufferRadius: Float, minCoordinate min: vector_float2, maxCoordinate max: vector_float2) {
        super.init(bufferRadius: bufferRadius, minCoordinate: min, maxCoordinate: max)
    }
    
    override init(bufferRadius: Float, minCoordinate min: vector_float2, maxCoordinate max: vector_float2, nodeClass:AnyClass) {
        super.init(bufferRadius: bufferRadius, minCoordinate: min, maxCoordinate: max, nodeClass:nodeClass)
    }
    
    required public init?(coder aDecoder: NSCoder) {
        fatalError("init(coder:) has not been implemented")
    }
    
    /// Standard helper to load a plist file
    class func loadPListData(_ file:String) -> NSDictionary {
        let lookup = "\(file)_obstacles"
        let path = Bundle.main.path(forResource: lookup, ofType: "plist")
        let pListData = NSDictionary(contentsOfFile:path!)
        return pListData!
    }
    
    /// The mesh graph is slightly bigger than the scene, so nodes can connect behind obstacles off scene. This allows units to walk behind obstacles
    /// if it is the shortest path. Removing off scene nodes causes hard error in GameplayKit. Instead, we'll remove any connections from one off scene node to another.
    /// This keeps the nodes in place, but units will no longer try to walk behind the scene
    func removeNodesWithNoSceneConnection(_ scene:SKScene, layer:SKNode) {
        var nodesOffScene = [GKGraphNode2D]()
        
        /// Loop each node
        for node in self.nodes as! [GKGraphNode2D] {
            
            /// If node is already off scene, we want to check it
            if(!layer.contains(CGPoint(node.position))) {
                /// Since it is offscene, remove connection to other off scene nodes
                for connection in node.connectedNodes {
                    let connectNode = connection as! GKGraphNode2D
                    if(!layer.contains(CGPoint(connectNode.position))) {
                        node.removeConnections(to: [connection], bidirectional: true)
                    }
                }

            }
        }
    }

    // MARK: Pathfinding
    
    /// Add a start and end node to the graph. Find the path. Remove the start and end node. Trickery needed with buffer radius issues.
    /// NOTE: We need to allow movement inside buffer region because squad shuodl be able to end up anywhere. The buffer in our case is only so that the squad will take
    /// wide angles when rounding corners, but should not affect start and end point.
    public func findPath(_ start:CGPoint, end:CGPoint, ignoringObstacles:[GKPolygonObstacle] = [], radius:Float, scene:SKScene, size:Float = 32) -> (path:GKPath, nodes:[GKGraphNode2D])? {
        
        logged("Finding path from \(start) to \(end)", file: #file, level: .Debug, newline: false)
        
        let startNode = GKGraphNode2D(point:float2(start))
        let endNode = GKGraphNode2D(point:float2(end))

        /// Start Node
        /// If point is in buffer region, force connection to closest node
        if self.getBuffersContainingPoint(point: float2(start), bufferSize: self.buffer).count > 0 {
            logged("trying to force start node", file: #file, level: .Debug, newline:false)
            self.connectToLowestCostNodeAvoidingObstacles(node: startNode, scene:scene)
        }
        /// else, let the gknavmesh connect it to every node nearby
        else {
            logged("trying to add start node", file: #file, level: .Debug, newline:false)
            self.connectToGraph(node: startNode)
        }
        
        logged("Start node added", file: #file, level: .Debug, newline:false)
        
        /// End Node
        /// If point is in buffer region, force connection to closest node
        if self.getBuffersContainingPoint(point: float2(end), bufferSize: self.buffer).count > 0 {
            logged("trying to force end node", file: #file, level: .Debug, newline:false)
            self.connectToLowestCostNodeAvoidingObstacles(node: endNode, scene:scene)

        }
        /// else, let the gknavmesh connect it to every node nearby
        else {
            logged("trying to add end node", file: #file, level: .Debug, newline:false)
            self.connectToGraph(node: endNode)
        }
        
        logged("End node added", file: #file, level: .Debug, newline:false)
        
        /// If there is a connection with more than two points, return the path smoothed result
        if(!startNode.connectedNodes.isEmpty && !endNode.connectedNodes.isEmpty) {
            self.debugOverlay(scene: scene)
            let pathNodes = self.findPath(from: startNode, to: endNode) as! [GKGraphNode2D]
            if pathNodes.count < 2 {
                logged("Path not found :(", file: #file, level: .Error, newline:false)
                self.remove([startNode, endNode])
                return nil
            }
            else {
                let optimizedNodes = self.optimizeWaypoints(startNode: startNode, points: pathNodes, scene: scene)
                let path = GKPath(graphNodes: optimizedNodes, radius: radius)
                self.remove([startNode, endNode])
                logged("Path found!", file: #file, level: .Debug, newline:false)
                self.debugPath(pathNodes: pathNodes, optimizedNodes:optimizedNodes, scene: scene)
                return (path, optimizedNodes)
            }
        }
        
        self.remove([startNode, endNode])
        
        logged("Path not found -- start or end node have no connections", file: #file, level: .Error, newline:false)
        
        return nil
    }
    
    // MARK: Utility
    
    /// Determines if a point lies on an obstacle. If this doesn't perform well, we can loop through the MeshObstacles and call
    /// containsPoint() on each one. Expects world point
    public func pointIsValid(_ point:float2) -> Bool {
        for mesh in self.meshObstacles {
            if mesh.containsPoint(CGPoint(point)) {
                return false
            }
        }
        return true
    }
    
    /// Builds a list of all buffer regions of obstacles containing a point. Used for ignoreBufferRadius when adding a GKGraphNode to the mesh.
    func getBuffersContainingPoint(point:float2, bufferSize:Float) -> [GKPolygonObstacle] {
        var ret = [GKPolygonObstacle]()
        for mesh in self.meshObstacles {
            if mesh.containsPointInBuffer(point: CGPoint(point), bufferRadius: CGFloat(bufferSize)) {
                ret.append(mesh.obstacle)
            }
        }
        return ret
    }
    
    /// GameplayKit has bugs where clicking in the coordinates of one GKTriangle will connect to nodes from another triangle.
    /// This is a utility method to help override that.
    func triangleAtPoint(point:CGPoint) -> GKTriangle? {
        for i in 1..<self.triangleCount {
            let triangle = self.triangle(at: i)
            if triangle.containsPoint(point: point) {
                return triangle
            }
        }
        return nil
    }
    
    /// Once we have a triangle, we have the 3 corners. This utility method can help locate the actual GKGraphNode2D that lives at those corners.
    func nodeAtPoint(point:float2) -> GKGraphNode2D? {
        for node in self.nodes! {
            let node2d = node as! GKGraphNode2D
            if node2d.position == point {
                return node2d
            }
        }
        return nil
    }
    
    /// GameplayKit has bugs where clicking in the coordinates of one GKTriangle will connect to nodes from another triangle.
    /// This will manually connect a new node to the 3 corners of the triangle it sits in.
    func connectNodeToTriangle(node:GKGraphNode2D, triangle:GKTriangle) -> Bool {
        var nodesToConnect = [GKGraphNode2D]()
        
        /// See if a node lives at each triangle  corner position.
        if let point2d = self.nodeAtPoint(point: float2(x:triangle.points.0.x, y:triangle.points.0.y)) {
            nodesToConnect.append(point2d)
        }
        if let point2d = self.nodeAtPoint(point: float2(x:triangle.points.1.x, y:triangle.points.1.y)) {
            nodesToConnect.append(point2d)
        }
        if let point2d = self.nodeAtPoint(point: float2(x:triangle.points.2.x, y:triangle.points.2.y)) {
            nodesToConnect.append(point2d)
        }
        
        if nodesToConnect.count > 0 {
            self.add([node])
            node.addConnections(to: nodesToConnect, bidirectional: true)
            return true
        }
        else {
            return false
        }
    }
    
    /// GameplayKit has bugs where clicking in the coordinates of one GKTriangle will connect to nodes from another triangle.
    /// FInd the triangle a new node sits in and connect to the 3 corners.
    func connectToGraph(node:GKGraphNode2D) -> Bool {
        if let triangle = self.triangleAtPoint(point: CGPoint(float2(x:node.position.x, y:node.position.y))) {
            return self.connectNodeToTriangle(node: node, triangle: triangle)
        }
        else {
            return false
        }
    }
    
    /// If the point is inside a buffer region, we have to manually connect to the lowest cost node while avoiding obstacles.
    func connectToLowestCostNodeAvoidingObstacles(node:GKGraphNode2D, scene:SKScene) -> Bool {
        
        /// Add the node to the graph
        self.add([node])
        
        /// Find the lowest cost to nearby nodes. Store in array with best cost as first index.
        var bestCost : Float = -1
        var bestNodes = [GKGraphNode2D]()
        for point in self.nodes! {
            if point != node {
                let cost = node.estimatedCost(to: point)
                if cost < bestCost || bestCost == -1 {
                    bestCost = cost
                    bestNodes.insert(point as! GKGraphNode2D, at: 0)
                }
            }
        }
        
        /// Assuming we found nearby nodes, connect to the first node possible that does not pass through a physics environment object.
        if bestNodes.count > 0 {
            for best in bestNodes {
                if self.hasClearPathThroughEnvironment(start: node, end: best, scene: scene) {
                    node.addConnections(to: [best], bidirectional: true)
                    return true
                }

            }
        }

        return false
    }
    
    // MARK: Path Smoothing
    
    /// Given an array of waypoints, check line of sight on each connecting point to find the most
    /// efficient walking path
    func optimizeWaypoints(startNode:GKGraphNode2D, points:[GKGraphNode2D], scene:SKScene) -> [GKGraphNode2D] {
        
        var optimizedPath = [GKGraphNode2D]()
        
        var origin = startNode
        var checkpoint = origin
        var checkpointIndex = 0
        
        /// Keep looping until we've arrived at final point
        while optimizedPath.last != points.last {
            
            /// Checkpoint is last checked node ... have to see how far we can skip ahead
            for index in checkpointIndex ..< points.count {
                
                /// Set our new start and end based on where we are in the iteration
                let start = origin
                let end = points[index]
                
                logged("Checking \(start) to \(end)", file: #file, level: .Debug, newline: false)
                
                /// If there is no clear path, we have to add this node and then start testing next node
                if(self.lineSegmentsHitBuffer(start: start, end: end) ||
                   !self.hasClearPathThroughEnvironment(start: start, end: end, scene: scene)) {
                    
                    logged("No clear path. Adding waypoint at \(checkpoint)\n", file: #file, level: .Debug, newline: true)
                    
                    /// Add the checkpoint to our new path
                    ///if(checkpoint != origin) {
                        optimizedPath.append(checkpoint)
                    ///}
                    
                    /// Set the checkpoint as the new origin to scan from
                    origin = checkpoint
                    
                    /// Reset the loop index to that of the current checkpoint
                    checkpointIndex = index
                    
                    /// Reset the next checkpoint to look at to the reflected index
                    checkpoint = points[index]
                    
                    break
                }
                
                /// Otherwise, make check point the next node
                checkpoint = points[index]
                
                /// If we've reached the last point (destination), append it which will end the loop
                if(index == points.count - 1) {
                    optimizedPath.append(points[index])
                }
                
            }
            
        }

        /**
         Always add in the start point in case algorithm above doesn't think it is needed. Two examples of why:
         1. If the point is in the same triangle that it started in, the start point will never get added based on logic of loop above.
         2. Gameplaykit will walk to the closest point of the path, and then continue down the path. So, if no start point added, unit will walk to an unpredictable start point.
        */
        if optimizedPath.first != points[0] {
            optimizedPath.insert(points[0], at: 0)
        }

        return optimizedPath
    }
    
    /// Checks against objects registered as physics objects to see if they would be intersected by the line.
    /// Only checks Environemnt obstacles.
    func hasClearPathThroughEnvironment(start:GKGraphNode2D, end:GKGraphNode2D, scene:SKScene) -> Bool {
        var ret = true
        let startPos = CGPoint(x:CGFloat(start.position.x), y:CGFloat(start.position.y))
        let endPos = CGPoint(x:CGFloat(end.position.x), y:CGFloat(end.position.y))

        scene.physicsWorld.enumerateBodies(alongRayStart: startPos, end:endPos) {
            body, point, normal, stop in
            if body.categoryBitMask == self.collider {
                ret = false
                logged("Line segment hit physics environment", file: #file, level: .Debug, newline: false)
                return
             }
            
        }
        return ret
    }
    
    /// Checks against objects registered as physics objects to see if they would be intersected by the line.
    /// Checks all obstacles
    func hasClearPath(start:GKGraphNode2D, end:GKGraphNode2D, scene:SKScene) -> Bool {
        var ret = true
        let startPos = CGPoint(x:CGFloat(start.position.x), y:CGFloat(start.position.y))
        let endPos = CGPoint(x:CGFloat(end.position.x), y:CGFloat(end.position.y))
        scene.physicsWorld.enumerateBodies(alongRayStart: startPos, end:endPos) {
            body, point, normal, stop in
            ret = false
            
        }
        return ret
    }
    
    /// This is a quick and dirty check to sample 10 points along a line to see if they live inside of the buffer region of an obstacle.
    /// Goal is for this to be quicker than physics, and be accurate a majority of the time. A few outliers will not break pathfinding.
    /// Also fixes issue when rounding corners and physics environment not hit from center of agent, but buffer would be.
    func lineSegmentsHitBuffer(start:GKGraphNode2D, end:GKGraphNode2D) -> Bool {
        let startPos = CGPoint(x:CGFloat(start.position.x), y:CGFloat(start.position.y))
        let endPos = CGPoint(x:CGFloat(end.position.x), y:CGFloat(end.position.y))
        
        /// We want to inset the buffer a bit so that points along the edge of the buffer don't trigger this
        var testBuffer = self.buffer
        if testBuffer > 1 {
            testBuffer = testBuffer - 1
        }
        
        /// If start point or end point is in buffer (i.e: unit is in a corner / going to a corner) ignore this check
        let startBuffers = self.getBuffersContainingPoint(point: float2(startPos), bufferSize:testBuffer)
        let endBuffers = self.getBuffersContainingPoint(point: float2(endPos), bufferSize:testBuffer)
        /*if startBuffers.count > 0 || endBuffers.count > 0 {
            return false
        }*/
        
        /// Find out vector
        var vector = endPos - startPos

        /// Get the length so that we can divide into equal points along the line.
        let length = vector.length()
        if length > 0 {
            
            /// 7 equal points, and we'll ignore the start and end point
            let pointDistance = length / 10

            /// Normalize the vector, so now it's just a direction
            vector.normalize()
            
            /// Loop to generate points along the line
            for i in 1 ... 8 {
                
                /// Take vector times equal point distance to generate a point i increments along the line
                let testPoint = startPos + (vector * (pointDistance * CGFloat(i)))
                
                /// See if the point exists in any of the buffers.
                let buffers = self.getBuffersContainingPoint(point: float2(testPoint), bufferSize:testBuffer)
                if buffers.count != 0 {
                    
                    for buffer in buffers {
                        if startBuffers.contains(buffer) || endBuffers.contains(buffer) {
                            /// Ignore this buffer because unit may be stuck in a corner
                        }
                        else {
                            /// Intersecting a random buffer, which should trigger a waypoint drop
                            logged("Line segment hit buffer", file: #file, level: .Debug, newline: false)
                            return true
                        }
                    }
                    
                }
            }
        }
        return false
    }
    
    // MARK: Debug
    
    /// Will automatically turn on debugging information if Debug.Navigaiton.enabled is set to true.
    func debugOverlay(scene:SKScene) {
        if PKAtlas.DEBUG == true {
            if let debug = scene.childNode(withName: "PermanentDebugLayer") {
            
                debug.enumerateChildNodes(withName: "navmeshDebug") { node, stop in
                    node.removeFromParent()
                }
                
                for node in self.nodes as! [GKGraphNode2D] {
                    
                    
                    let point = node.position
                    
                    let shapeTexture = SKSpriteNode(texture: SKTexture(imageNamed: "yellow_circle"))
                    shapeTexture.zPosition = 1001
                    shapeTexture.position = debug.convert(CGPoint(point), from:scene)
                    shapeTexture.setScale(0.4)
                    shapeTexture.name = "navmeshDebug"
                    debug.addChild(shapeTexture)
                    
                    for destination in node.connectedNodes as! [GKGraphNode2D] {
                        let start = debug.convert(CGPoint(node.position), from: scene)
                        let end = debug.convert(CGPoint(destination.position), from: scene)
                        let points = [start, end]
                        
                        let shapeNode = SKShapeNode(points: UnsafeMutablePointer<CGPoint>(mutating: points), count: 2)
                        shapeNode.strokeColor = SKColor(white: 1.0, alpha: 0.5)
                        shapeNode.lineWidth = 5.0
                        shapeNode.zPosition = 1000
                        shapeNode.name = "navmeshDebug"
                        debug.addChild(shapeNode)
                    }
                    
                    if node.connectedNodes.isEmpty {
                        shapeTexture.color = SKColor.orange
                        shapeTexture.colorBlendFactor = 1
                        shapeTexture.alpha = 0.2
                    }
                    
                    
                    if(!scene.childNode(withName: "World")!.contains(CGPoint(node.position))) {
                        shapeTexture.color = SKColor.red
                        shapeTexture.colorBlendFactor = 1
                        shapeTexture.alpha = 1
                    }
                }
            }
        }
    }
    
    /// Will automatically turn on debugging information if Debug.Pathfinding.enabled is set to true.
    func debugPath(pathNodes:[GKGraphNode2D], optimizedNodes:[GKGraphNode2D], scene:SKScene) {
        if PKAtlas.DEBUG == true {
            if let debug = scene.childNode(withName: "PermanentDebugLayer") {
                
                // remove last debug informaiton
                debug.enumerateChildNodes(withName: "pathfinding") { node, stop in
                    node.removeFromParent()
                }
                
                for node in pathNodes {
                    var shapeTexture1 = SKSpriteNode(imageNamed: "red_cross")
                    shapeTexture1.name = "pathfinding"
                    shapeTexture1.xScale = 4
                    shapeTexture1.yScale = 4
                    shapeTexture1.zPosition = 1010
                    shapeTexture1.position = debug.convert(CGPoint(x:CGFloat(node.position.x), y:CGFloat(node.position.y)), from:scene)
                    debug.addChild(shapeTexture1)
                }
                
                for point in optimizedNodes {
                    var shapeTexture1 = SKSpriteNode(imageNamed: "green_cross")
                    shapeTexture1.name = "pathfinding"
                    shapeTexture1.xScale = 4
                    shapeTexture1.yScale = 4
                    shapeTexture1.zPosition = 1011
                    shapeTexture1.position = debug.convert(CGPoint(x:CGFloat(point.position.x), y:CGFloat(point.position.y)), from:scene)
                    debug.addChild(shapeTexture1)
                }

                
            }
        }
    }
    
    func debugTriangles(scene:SKScene) {
        if PKAtlas.DEBUG == true {
            if let debug = scene.childNode(withName: "PermanentDebugLayer") {
                print(self.triangleCount)
                print(triangle(at: 1))
                for i in 1..<self.triangleCount {
                    let triangle = self.triangle(at: i)
                    
                    let path = CGMutablePath()
                    let points = triangle.points
                    path.move(to: CGPoint(x:CGFloat(points.0.x), y:CGFloat(points.0.y)))
                    path.addLine(to: CGPoint(x:CGFloat(points.1.x), y:CGFloat(points.1.y)))
                    path.addLine(to: CGPoint(x:CGFloat(points.2.x), y:CGFloat(points.2.y)))
                    path.closeSubpath()
                    
                 
                    let outline = SKShapeNode(path: path)
                    outline.zPosition = 1001
                    outline.lineWidth = 1
                    outline.fillColor = SKColor.yellow
                    outline.alpha = 0.5
                    debug.addChild(outline)
                    
                }
            }
        }
    }
    
}