pathfinder.ts

/*
		File: pathfinder.ts
        Author: Elmeri Ferm
        Description: A-Star based pathfinding library. (Converted to TS from Lua on 30.04.2022)

        Usage:
        const pathFinder = new PathFinder(Workspace.WaitForChild("Nodes"));
        const start = pathFinder.getNearestNode(Workspace.WaitForChild("Start").Position)
        const goal = pathFinder.getNearestNode(Workspace.WaitForChild("Goal").Position)
        const path = pathFinder.findPath(start, goal)
 */


/**
 * An Object representing the connection between GraphNodes
 */
export class GraphConnection {
    ID: number;
    G: number;

    constructor(id: number, g: number) {
        this.ID = id; this.G = g;
    }
}

/**
 * A Node representing a location on the nodegraph
 */
export class GraphNode {
    ID: number;
    Brick: BasePart;
    Connections = new Array<GraphConnection>();

    constructor(id: number, brick: BasePart) {
        this.ID = id; this.Brick = brick;
    }
}


export class Pathfinder {
    /**
     * Current node index
     */
    private mnt_index = 1;

    /**
     * Current nodegraph
     */
    private nodeGraph = new Array<GraphNode>();

    /**
     * Euclidean distance heuristic function for findPath()
     */
    private heuristic(a: number, b: number): number {
        const p1 = this.nodeGraph[a].Brick.Position;
        const p2 = this.nodeGraph[b].Brick.Position;
        return p1.sub(p2).Magnitude;
    }

    /**
     * Creates a GraphNode from a given brick
     */
    private nodeObjectFromBrick(brick: BasePart): number {
        let node_index = this.mnt_index;
        this.nodeGraph.push(new GraphNode(this.mnt_index, brick));

        brick.GetChildren().forEach( (child) => {
            if (child.IsA("ObjectValue") && child.Name === "connection") {
                let brick2 = child.Value as BasePart;
                if (brick2 === undefined) {
                    error(string.format("Cannot parse nodegraph, Connection value nil at node %s", tostring(this.searchByBrick(brick))))
                }
                let ID = this.searchByBrick(brick2)
                if (ID === undefined) {
                    this.mnt_index++;
                    ID = this.nodeObjectFromBrick(brick2);
                }
                this.nodeGraph[node_index].Connections.push(
                    new GraphConnection(ID, this.nodeGraph[ID].Brick.Position.sub(brick.Position).Magnitude)
                );
            }


        });
        return node_index;
    }

    /**
     * Path builder for findPath
     */
    private getPath(t: Array<number>, n: number): Array<number> {
        if (t[n] === undefined) {
            return new Array<number>(n);
        } else {
            let t2 = this.getPath(t, t[n]);
            t2.push(n);
            return t2;
        }
    }

    /**
     * Get the current nodegraph
     */
    getGraph() {
        return this.nodeGraph;
    }

    /**
     * Returns node from given BasePart
     */
    searchByBrick(brick: BasePart): number | undefined {
        this.nodeGraph.forEach( (node) => {
            if (node.Brick === brick) {
                return node;
            }
        });
        return undefined;
    }

    /**
     * Returns node from given ID
     */
    searchByID(id: number): BasePart | undefined {
        this.nodeGraph.forEach( (node) => {
            if (node.ID === id) {
                return node;
            }
        });
        return undefined;
    }

    /**
     * Gets the node nearest to position
     */
    getNearestNode(position: Vector3): GraphNode {
        let nearest = this.nodeGraph[0];
        for (let n = 1; n < this.nodeGraph.size(); n++) {
            const oldDist = nearest.Brick.Position.sub(position).Magnitude;
            const newDist = position.sub(this.nodeGraph[n].Brick.Position).Magnitude;
            if (newDist < oldDist) {
                nearest = this.nodeGraph[n];
            }
        }
        return nearest;
    }

    /**
     * Gets the node farthest from position
     */
    getFarthestNode(position: Vector3): GraphNode {
        let farthest = this.nodeGraph[0];
        for (let n = 1; n < this.nodeGraph.size(); n++) {
            const oldDist = farthest.Brick.Position.sub(position).Magnitude;
            const newDist = position.sub(this.nodeGraph[n].Brick.Position).Magnitude;
            if (newDist > oldDist) {
                farthest = this.nodeGraph[n];
            }
        }
        return farthest;
    }

    /**
     * Builds the graph from given node folder/model for this pathfinder
     */
    collectNodes(nodes: Instance): Array<GraphNode>  {
        this.nodeGraph = new Array<GraphNode>();
        this.mnt_index = 1;

        nodes.GetChildren().forEach( (node) => {
            if (node.IsA("BasePart")) {
                this.nodeObjectFromBrick(node);
                this.mnt_index++;
            }
        });
        return this.nodeGraph;
    }

    /**
     * Finds the shortest path between start and goal
     */
    findPath(start: GraphNode, goal: GraphNode): Array<GraphNode> {
        const startID = start.ID;
        const endID = goal.ID;
        let closed = new Array<number>();
        let open = [startID];
        let previous = new Array<number>();
        let g_score = new Array<number>();
        let f_score = new Array<number>();

        g_score[startID] = 0;
        f_score[startID] = this.heuristic(startID, endID);

        while (open.size() > 0) {
            let current: undefined | number;
            let current_i: undefined | number;
            for (let i = 0; i < open.size(); i++) {
                const j = open[i];
                if (current === undefined) {
                    current = j;
                    current_i = i;
                } else {
                    if (j !== undefined) {
                        if (f_score[j] < f_score[current]) {
                            current = j;
                            current_i = i;
                        }
                    }
                }
            }

            if (current === endID) {
                const path = this.getPath(previous, endID);
                const ret = new Array<GraphNode>();
                for (let i = 0; i < path.size(); i++) {
                    const id = path[i];
                    ret.push(this.nodeGraph[id])
                }
                return ret;
            }
            if (current_i !== undefined) {
                open.remove(current_i);
            }

            if (current !== undefined) {
                closed.push(current);
                for (const j of this.nodeGraph[current].Connections) {
                    let in_closed = false;
                    for (const l of closed) {
                        if (l === j.ID) {
                            in_closed = true;
                            break;
                        }
                    }
                    if (!in_closed)  {
                        let tentative_score = g_score[current] + j.G;
                        let in_open = false;
                        for (const l of open) {
                            if (l === j.ID) {
                                in_open = true;
                                break;
                            }
                        }
                        if (!in_open || tentative_score < g_score[j.ID]) {
                            previous[j.ID] = current;
                            g_score[j.ID] = tentative_score;
                            f_score[j.ID] = g_score[j.ID] + this.heuristic(j.ID, endID);
                            if (!in_open) {
                                open.push(j.ID);
                            }
                        }
                    }
                }
            }
        }
        return new Array<GraphNode>();
    }

    constructor(nodeFolder: Folder | Model) {
        this.collectNodes(nodeFolder);
    }
}