Created
April 30, 2022 12:57
-
-
Save Elmuti/467827353e59d890decdafd77861c797 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/** | |
* 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 | |
*/ | |
private mnt_index = 1; | |
/** | |
* Current nodegraph | |
* @private | |
*/ | |
private nodeGraph = new Array<GraphNode>(); | |
/** | |
* Euclidean distance heuristic function for findPath() | |
* @param a | |
* @param b | |
* @private | |
*/ | |
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 | |
* @param brick | |
* @private | |
*/ | |
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 | |
* @param t | |
* @param n | |
* @private | |
*/ | |
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 | |
* @param id | |
*/ | |
searchByBrick(brick: BasePart): number | undefined { | |
this.nodeGraph.forEach( (node) => { | |
if (node.Brick === brick) { | |
return node; | |
} | |
}); | |
return undefined; | |
} | |
/** | |
* Returns node from given ID | |
* @param id | |
*/ | |
searchByID(id: number): BasePart | undefined { | |
this.nodeGraph.forEach( (node) => { | |
if (node.ID === id) { | |
return node; | |
} | |
}); | |
return undefined; | |
} | |
/** | |
* Gets the node nearest to position | |
* @param 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 | |
* @param 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 | |
* @param nodes | |
*/ | |
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 | |
* @param start | |
* @param 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); | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment