343 lines
12 KiB
JavaScript
343 lines
12 KiB
JavaScript
import { BC_TERRAIN_SETTINGS } from "../GlobalVariables/GlobalVariables";
|
|
import { ChunkStorageTypes } from "../WorldGeneration/WorldChunk/WorldGenChunk";
|
|
import { getTileAt } from "../WorldGeneration/WorldGen";
|
|
import { PointInt2D } from "./Math.utils";
|
|
import { SceneObject } from "../SceneObjects/SceneObject";
|
|
import { getNavigationGridTile } from "../World/NavigationGrid/NavigationGrid";
|
|
import { TerrainTile } from "../WorldGeneration/WorldObjects/TerrainTile/TerrainTile";
|
|
|
|
class PathFinderNode {
|
|
position;
|
|
fScore = 1e16;
|
|
gScore = 1e16;
|
|
dScore = 1;
|
|
// hScore = 1e16;
|
|
id;
|
|
/**
|
|
* @type {TerrainTile}
|
|
*/
|
|
ref;
|
|
|
|
/**
|
|
*
|
|
* @param {PointInt2D} position
|
|
* @param {Number} gScore
|
|
*/
|
|
constructor(position, gScore = 1e16, fScore = 1e16, dScore) {
|
|
this.position = position;
|
|
this.id = position.getX() + "_" + position.getY();
|
|
this.gScore = gScore;
|
|
this.fScore = fScore;
|
|
this.dScore = dScore;
|
|
}
|
|
}
|
|
|
|
export class NavigationPath {
|
|
path;
|
|
|
|
/**
|
|
* @param {Array<PointInt2D>} path
|
|
*/
|
|
constructor(path = []) {
|
|
this.path = path;
|
|
}
|
|
}
|
|
|
|
export class NavigationResult {
|
|
/**
|
|
* @type {Boolean}
|
|
*/
|
|
error;
|
|
/**
|
|
* @type {String}
|
|
*/
|
|
errorText;
|
|
/**
|
|
* @type {NavigationPath | undefined}
|
|
*/
|
|
result;
|
|
/**
|
|
* 0 - point is unreachable
|
|
* 1 - path found
|
|
* 2 - error
|
|
* @type {Number}
|
|
*/
|
|
state;
|
|
|
|
constructor(error, errorText, result, state) {
|
|
this.error = error;
|
|
this.errorText = errorText;
|
|
this.result = result;
|
|
this.state = state;
|
|
}
|
|
}
|
|
|
|
export class PathFinder {
|
|
/**
|
|
* @type Array<PathFinderNode>
|
|
*/
|
|
_openSet = new Array();
|
|
_closedSet = new Array();
|
|
|
|
_goal = new PointInt2D();
|
|
_start = new PointInt2D();
|
|
|
|
#MAX_PATH_ITERATIONS = 10000;
|
|
#iterationsCounter = 0;
|
|
|
|
// findPathIfExist(start, goal)
|
|
// {
|
|
// let resultFromGoal = this.search(goal, start);
|
|
// if(resultFromGoal.state !== 1)
|
|
// {
|
|
// return resultFromGoal;
|
|
// }
|
|
// else
|
|
// {
|
|
// resultFromGoal.result.path.reverse();
|
|
// return resultFromGoal;
|
|
// }
|
|
// }
|
|
|
|
/**
|
|
*
|
|
* @param {PointInt2D} start
|
|
* @param {PointInt2D} goal
|
|
*/
|
|
search(start, goal) {
|
|
this._openSet = [];
|
|
this._closedSet = [];
|
|
this.#iterationsCounter = 0;
|
|
start.divideBy(BC_TERRAIN_SETTINGS.totalSize);
|
|
start.multiplyBy(BC_TERRAIN_SETTINGS.totalSize);
|
|
this._start = start;
|
|
|
|
goal.divideBy(BC_TERRAIN_SETTINGS.totalSize);
|
|
goal.multiplyBy(BC_TERRAIN_SETTINGS.totalSize);
|
|
this._goal = goal;
|
|
// getNavigationGridTile(goal.getX(), goal.getY()).optionalTerrainTileRef.drawObject.tint = 0x0000FF;
|
|
|
|
this._openSet.push(new PathFinderNode(this._start, 0, this._heuristic(this._start, 0)));
|
|
|
|
//code
|
|
/**
|
|
* @type PathFinderNode
|
|
*/
|
|
let currentNode;
|
|
// /**
|
|
// * @type Map<String, Number>
|
|
// */
|
|
// let gScores = new Map();
|
|
// /**
|
|
// * @type Map<String, Number>
|
|
// */
|
|
// let fScores = new Map();
|
|
|
|
/**
|
|
* @type Map<String, PathFinderNode>
|
|
*/
|
|
let cameFrom = new Map();
|
|
|
|
let minFScoreNodeIndex = 0;
|
|
//START LOOP WOOOOAAAW
|
|
while (this._openSet.length > 0) {
|
|
this.#iterationsCounter++;
|
|
if(this.#iterationsCounter >= this.#MAX_PATH_ITERATIONS)
|
|
{
|
|
return new NavigationResult(true, "reached limit", new NavigationPath(), 2);
|
|
}
|
|
//find node with min f score
|
|
//better to rewrite it to priority queue
|
|
minFScoreNodeIndex = 0;
|
|
for (let i = 0; i < this._openSet.length; i++) {
|
|
if (this._openSet[i].fScore < this._openSet[minFScoreNodeIndex].fScore) minFScoreNodeIndex = i;
|
|
}
|
|
//wow! node is found and set!!
|
|
currentNode = this._openSet[minFScoreNodeIndex];
|
|
// if (currentNode.ref) currentNode.ref.drawObject.tint = 0xff0000;
|
|
|
|
if (PointInt2D.isEqual(currentNode.position, this._goal)) {
|
|
//wow!!! we have found an end of the path!!! this is so cool!!!
|
|
return new NavigationResult(false, "", new NavigationPath(this._reconstructPath(cameFrom, currentNode)), 1); //return something weird stuff
|
|
}
|
|
|
|
//and now we must delete this node... what a sad situation...
|
|
this._openSet.splice(minFScoreNodeIndex, 1);
|
|
//but wait! we add it to closed set! nice!!!
|
|
this._closedSet.push(currentNode);
|
|
|
|
/**
|
|
* @type Array<PathFinderNode>
|
|
*/
|
|
let currentNeighbors = this._getNeighbors(currentNode.position);
|
|
for (const neighbor of currentNeighbors) {
|
|
if (this._existsInClosedSet(neighbor)) continue;
|
|
let tentativeGScore = currentNode.gScore + this._manhattanDistance(currentNode.position, neighbor.position);
|
|
if (tentativeGScore < neighbor.gScore && !this._existsInOpenSet(neighbor)) {
|
|
cameFrom.set(neighbor.id, currentNode);
|
|
neighbor.gScore = tentativeGScore;
|
|
neighbor.fScore = tentativeGScore + this._heuristic(neighbor.position, neighbor.dScore);
|
|
if (!this._existsInOpenSet(neighbor)) {
|
|
this._openSet.push(neighbor);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return new NavigationResult(false, "", new NavigationPath(), 0);
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @param {Map<String, PathFinderNode>} cameFrom
|
|
* @param {PathFinderNode} current
|
|
*/
|
|
_reconstructPath(cameFrom, current) {
|
|
let totalPath = [current.position];
|
|
// console.log(cameFrom);
|
|
let keys = [...cameFrom.keys()];
|
|
while (keys.includes(current.id)) {
|
|
if (current.id === cameFrom.get(current.id).id) break;
|
|
current = cameFrom.get(current.id);
|
|
totalPath.push(current.position);
|
|
}
|
|
return totalPath;
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @param {PointInt2D} root
|
|
*/
|
|
_getNeighbors(root) {
|
|
/**
|
|
* @type Array<PathFinderNode>
|
|
*/
|
|
let neighbors = new Array();
|
|
//north
|
|
let currentTile = getNavigationGridTile(root.getX(), root.getY() + BC_TERRAIN_SETTINGS.totalSize);
|
|
let node;
|
|
if (currentTile && !currentTile.isObstacle) {
|
|
// currentTile.optionalTerrainTileRef.drawObject.tint = 0x0000ff;
|
|
node = new PathFinderNode(currentTile.position, 1e16, 1e16, currentTile.movementCost);
|
|
node.ref = currentTile.optionalTerrainTileRef;
|
|
neighbors.push(node);
|
|
}
|
|
//north-e
|
|
// currentTile = getNavigationGridTile(root.getX() + BC_TERRAIN_SETTINGS.totalSize, root.getY() + BC_TERRAIN_SETTINGS.totalSize);
|
|
// if (currentTile && !currentTile.isObstacle) {
|
|
// // currentTile.optionalTerrainTileRef.drawObject.tint = 0x0000ff;
|
|
// node = new PathFinderNode(currentTile.position, 1e16, 1e16, currentTile.movementCost);
|
|
// node.ref = currentTile.optionalTerrainTileRef;
|
|
// neighbors.push(node);
|
|
// }
|
|
//north-w
|
|
// currentTile = getNavigationGridTile(root.getX() - BC_TERRAIN_SETTINGS.totalSize, root.getY() + BC_TERRAIN_SETTINGS.totalSize);
|
|
// if (currentTile && !currentTile.isObstacle) {
|
|
// // currentTile.optionalTerrainTileRef.drawObject.tint = 0x0000ff;
|
|
// node = new PathFinderNode(currentTile.position, 1e16, 1e16, currentTile.movementCost);
|
|
// node.ref = currentTile.optionalTerrainTileRef;
|
|
// neighbors.push(node);
|
|
// }
|
|
//south
|
|
currentTile = getNavigationGridTile(root.getX(), root.getY() - BC_TERRAIN_SETTINGS.totalSize);
|
|
if (currentTile && !currentTile.isObstacle) {
|
|
// currentTile.optionalTerrainTileRef.drawObject.tint = 0x0000ff;
|
|
node = new PathFinderNode(currentTile.position, 1e16, 1e16, currentTile.movementCost);
|
|
node.ref = currentTile.optionalTerrainTileRef;
|
|
neighbors.push(node);
|
|
}
|
|
//south-e
|
|
// currentTile = getNavigationGridTile(root.getX() + BC_TERRAIN_SETTINGS.totalSize, root.getY() - BC_TERRAIN_SETTINGS.totalSize);
|
|
// if (currentTile && !currentTile.isObstacle) {
|
|
// // currentTile.optionalTerrainTileRef.drawObject.tint = 0x0000ff;
|
|
// node = new PathFinderNode(currentTile.position, 1e16, 1e16, currentTile.movementCost);
|
|
// node.ref = currentTile.optionalTerrainTileRef;
|
|
// neighbors.push(node);
|
|
// }
|
|
//south-w
|
|
// currentTile = getNavigationGridTile(root.getX() - BC_TERRAIN_SETTINGS.totalSize, root.getY() - BC_TERRAIN_SETTINGS.totalSize);
|
|
// if (currentTile && !currentTile.isObstacle) {
|
|
// // currentTile.optionalTerrainTileRef.drawObject.tint = 0x0000ff;
|
|
// node = new PathFinderNode(currentTile.position, 1e16, 1e16, currentTile.movementCost);
|
|
// node.ref = currentTile.optionalTerrainTileRef;
|
|
// neighbors.push(node);
|
|
// }
|
|
//east
|
|
currentTile = getNavigationGridTile(root.getX() + BC_TERRAIN_SETTINGS.totalSize, root.getY());
|
|
if (currentTile && !currentTile.isObstacle) {
|
|
// currentTile.optionalTerrainTileRef.drawObject.tint = 0x0000ff;
|
|
node = new PathFinderNode(currentTile.position, 1e16, 1e16, currentTile.movementCost);
|
|
node.ref = currentTile.optionalTerrainTileRef;
|
|
neighbors.push(node);
|
|
}
|
|
//west
|
|
currentTile = getNavigationGridTile(root.getX() - BC_TERRAIN_SETTINGS.totalSize, root.getY());
|
|
if (currentTile && !currentTile.isObstacle) {
|
|
// currentTile.optionalTerrainTileRef.drawObject.tint = 0x0000ff;
|
|
node = new PathFinderNode(currentTile.position, 1e16, 1e16, currentTile.movementCost);
|
|
node.ref = currentTile.optionalTerrainTileRef;
|
|
neighbors.push(node);
|
|
}
|
|
return neighbors;
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @param {PointInt2D} start
|
|
* @param {PointInt2D} goal
|
|
* @returns {Number}
|
|
*/
|
|
_manhattanDistance(start, goal) {
|
|
return Math.abs(start.getX() - goal.getX()) + Math.abs(start.getY() - goal.getY());
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @param {PointInt2D} start
|
|
* @param {PointInt2D} goal
|
|
* @returns {Number}
|
|
*/
|
|
_euclideanDistance(start, goal) {
|
|
let dx = Math.abs(start.getX() - goal.getX());
|
|
let dy = Math.abs(start.getY() - goal.getY());
|
|
return Math.sqrt(dx * dx + dy * dy);
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @param {PointInt2D} current
|
|
* @param {Number} d
|
|
*/
|
|
_heuristic(current, d) {
|
|
let x1 = current.getX() - this._goal.getX();
|
|
let y1 = current.getY() - this._goal.getY();
|
|
let x2 = this._start.getX() - this._goal.getX();
|
|
let y2 = this._start.getY() - this._goal.getY();
|
|
let cross = Math.abs(x1 * y2 - x2 * y1);
|
|
// console.log("cross", cross);
|
|
return this._manhattanDistance(current, this._goal) * d + cross * 0.00001;
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @param {PathFinderNode} node
|
|
*/
|
|
_existsInClosedSet(node) {
|
|
for (const i of this._closedSet) {
|
|
if (i.id === node.id) return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @param {PathFinderNode} node
|
|
*/
|
|
_existsInOpenSet(node) {
|
|
for (const i of this._openSet) {
|
|
if (i.id === node.id) return true;
|
|
}
|
|
return false;
|
|
}
|
|
}
|