From 3901c8273be20d6b773036634bf81a6919447f21 Mon Sep 17 00:00:00 2001 From: CheezBarger Date: Thu, 12 Sep 2019 23:15:34 +0200 Subject: [PATCH] General Work - New traversal conditions - D* Lite fully supports replanning - Updated README with working advanced implementation --- DefaultConditions/predecessorconditions.json | 10 ++ DefaultConditions/successorconditions.json | 10 ++ Pathfinders/ASTAR.js | 1 + Pathfinders/DLITE.js | 146 +++++++++++++------ Pathfinders/README.md | 7 +- README.md | 26 ++-- index.js | 2 +- 7 files changed, 141 insertions(+), 61 deletions(-) diff --git a/DefaultConditions/predecessorconditions.json b/DefaultConditions/predecessorconditions.json index 81dfbe7..393cc73 100644 --- a/DefaultConditions/predecessorconditions.json +++ b/DefaultConditions/predecessorconditions.json @@ -162,5 +162,15 @@ ]} ]} + ]}, + {"type":"blockconditions","coordinates":[1,3,0],"conditions":[ + + {"type":"nc_condition","coordinates":[1,2,0],"condition":"block"}, + {"type":"nc_condition","coordinates":[0,3,0],"condition":"empty"}, + {"type":"nc_condition","coordinates":[0,4,0],"condition":"empty"}, + + {"type":"condition","coordinates":[0,2,0],"condition":"empty"}, + {"type":"condition","coordinates":[1,3,0],"condition":"empty"}, + {"type":"condition","coordinates":[1,4,0],"condition":"empty"} ]} ] \ No newline at end of file diff --git a/DefaultConditions/successorconditions.json b/DefaultConditions/successorconditions.json index 4ca2240..3f809aa 100644 --- a/DefaultConditions/successorconditions.json +++ b/DefaultConditions/successorconditions.json @@ -156,5 +156,15 @@ ]} ]} ]} + ]}, + {"type":"blockconditions","coordinates":[1,-3,0],"conditions":[ + + {"type":"nc_condition","coordinates":[1,-4,0],"condition":"block"}, + {"type":"nc_condition","coordinates":[1,-3,0],"condition":"empty"}, + {"type":"nc_condition","coordinates":[1,-2,0],"condition":"empty"}, + {"type":"nc_condition","coordinates":[1,-1,0],"condition":"empty"}, + + {"type":"condition","coordinates":[1,0,0],"condition":"empty"}, + {"type":"condition","coordinates":[1,1,0],"condition":"empty"} ]} ] \ No newline at end of file diff --git a/Pathfinders/ASTAR.js b/Pathfinders/ASTAR.js index 682286b..4bfcd3b 100644 --- a/Pathfinders/ASTAR.js +++ b/Pathfinders/ASTAR.js @@ -40,6 +40,7 @@ module.exports = function(bot, sp, ep) path.push(state.c); } returnState.path = path; + returnState.closestPoint = state.c; }) .catch(function(e) {console.error('ERROR Pathfinder:', e);}); } diff --git a/Pathfinders/DLITE.js b/Pathfinders/DLITE.js index 9cad634..7d3baa9 100644 --- a/Pathfinders/DLITE.js +++ b/Pathfinders/DLITE.js @@ -21,14 +21,14 @@ module.exports = function(bot, sp, ep) // First part of Main() in D*Lite if (S.start === S.goal) return undefined; - let minState = null; + let minState; let minCost = Number.POSITIVE_INFINITY; // Get successors according to stored state. const successors = bot.pathfinder.getSuccessors(S.start.c); for (let n = 0, len = successors.length; n < len; n++) { - const sp = ExistingState(successors[n]); + const sp = new State(successors[n]); if (sp) { const cost = bot.pathfinder.COST(S.start.c, sp.c) + sp.g; @@ -42,23 +42,35 @@ module.exports = function(bot, sp, ep) return minState; } + this.path.peek = function() { const temp = _peek(); if (temp !== undefined) return temp.c; - else return undefined; + else return; }; + + let globalMinCost = Number.POSITIVE_INFINITY; this.path.pop = function() { const temp = _peek(); if (temp !== undefined) { + const cost = bot.pathfinder.COST(S.start.c, temp.c) + temp.g; + if (cost >= globalMinCost) return; + + globalMinCost = cost; S.start = temp; + + k_m = k_m + bot.pathfinder.HEURISTIC(S.last.c, S.start.c); + S.last = S.start; + return temp.c; } - else return undefined; + else return; }; + this.path.replan = function(c) { return new Promise(function(resolve, reject) @@ -83,50 +95,97 @@ module.exports = function(bot, sp, ep) reject(e); }); }); - /* const v = ReturnState.OBSOLETE.pop(); - if (v) - { - k_m = k_m + bot.pathfinder.HEURISTIC(S.last.c, S.start.c); - S.last = S.start; + }; + + // Changes in world state are passed to this function + this.path.updateState = function(c) + { + // Check if state or any predecessors of the state are part of the pathfinder state + let existsInState = false; + const predecessors = bot.pathfinder.getPredecessors(c); - // Since the blockUpdate event is only validated when a previously navigable vertex is blocked - // we need to only accomodate for one case. [c_old < c(u,v)] - const predecessors = bot.pathfinder.getPredecessors(v.c); - for (let n = 0, len = predecessors.length; n < len; n++) + if (S.check(c)) + existsInState = true; + + else + { + for (let m = 0, len = predecessors.length; m < len; m++) { - const u = new State(predecessors[n]); - if (floatEqual(u.rhs, bot.pathfinder.COST(u.c, v.c) + v.g) && u !== S.goal) + if (S.check(predecessors[m])) { - u.rhs = Number.POSITIVE_INFINITY; - - const successors = bot.pathfinder.getSuccessors(u.c); - for (let m = 0, len = successors.length; m < len; m++) - { - const sp = new State(successors[m]); - const cost = bot.pathfinder.COST(u.c, sp.c) + sp.g; - if (u.rhs > cost) u.rhs = cost; - } + existsInState = true; + break; } + } + } + + if (!existsInState) + return; + + // Updated block is relevant to pathfinder state + // Check if state is blocked or can traverse + + // Usually a block above the state might be blocking it, this performs a check for a block right below + // it as either one or the other will be traversable. c0 is considered the relevant state position. - updateVertex(u); + let traversable = false; + let c0 = c; + const c1 = c.offset(0, -1, 0); + + outer: for (let m = 0, len = predecessors.length; m < len; m++) + { + const successors = bot.pathfinder.getSuccessors(predecessors[m]); + for (let n = 0, len = successors.length; n < len; n++) + { + if (c0.equals(successors[n])) + { + traversable = true; + break outer; + } + else if (c1.equals(successors[n])) + { + c0 = c1; + traversable = true; + break outer; + } } + } + + // Updates state - computeShortestPath().then(ResolveFunction); + if (traversable) + { + const v = new State(c0); + predecessors.unshift(v.c); // Introduce v to state + } + else + { + if (!S.check(c0)) + c0 = c1; } - else computeShortestPath().then(ResolveFunction);*/ - }; - // Handles changes in the world state - /* bot.on('blockUpdate', function(_, newBlock) - { - const v = ExistingState(newBlock.position); - if (v && compareKeys(v.k, S.start.k)) // Ensures we havent already passed that block + for (let n = 0, len = predecessors.length; n < len; n++) { - ReturnState.OBSOLETE.push(v); - U.remove(U.check(v)); - S[v.c.x >>> 0][v.c.y >>> 0][v.c.z >>> 0] = undefined; + const u = new State(predecessors[n]); + + if (u !== S.goal) + { + u.rhs = Number.POSITIVE_INFINITY; + + const successors = bot.pathfinder.getSuccessors(u.c); + for (let m = 0, len = successors.length; m < len; m++) + { + const sp = new State(successors[m]); + const cost = bot.pathfinder.COST(u.c, sp.c) + sp.g; + if (u.rhs > cost) u.rhs = cost; + } + } + + updateVertex(u); } - }); */ + + return returnState.path.replan(bot.entity.position.floored()); + }; }; // Global state functions @@ -156,6 +215,10 @@ module.exports = function(bot, sp, ep) } } return false; }; + S.remove = function(c) + { + this[c.x >>> 0][c.y >>> 0][c.z >>> 0] = undefined; + }; // Priority queue functions const U = new Heap(function(s1, s2) {return compareKeys(s1.k, s2.k);}); @@ -201,13 +264,6 @@ module.exports = function(bot, sp, ep) } } - function ExistingState(c) - { - if (S.check(c)) - return S[c.x >>> 0][c.y >>> 0][c.z >>> 0]; - else return undefined; - } - // Algorithm functions function calculateKey(s) { @@ -261,7 +317,7 @@ module.exports = function(bot, sp, ep) u.g = Number.POSITIVE_INFINITY; const predecessors = bot.pathfinder.getPredecessors(u.c); - predecessors.push(u.c); // ∪ {u} + predecessors.unshift(u.c); // ∪ {u} for (let n = 0, len = predecessors.length; n < len; n++) { const s = new State(predecessors[n]); diff --git a/Pathfinders/README.md b/Pathfinders/README.md index 9fde2ca..623e1ea 100644 --- a/Pathfinders/README.md +++ b/Pathfinders/README.md @@ -30,7 +30,7 @@ Object with the following properties: ### ASTARReturnState.ENUMState Set when algorithim completes path search, equal to one of `bot.pathfinder.ENUMStatus` depending on whether the path search was successfull or not. -### ASTARReturnState.ClosestPoint +### ASTARReturnState.closestPoint Set when algorithim the path search is incomplete, equal to the furthest position the bot could find a path to. ## D* Lite Pathfinding @@ -53,10 +53,7 @@ Object with the following properties: ### DLITEReturnState.ENUMState Set when algorithim completes path search, equal to one of `bot.pathfinder.ENUMStatus` depending on whether the path search was successful or not. -### DLITEReturnState.ClosestPoint -Set when algorithim completes path search, equal to the closest point from which a path from the end point to the start could be found. - -If `DLITEReturnState.ENUMState` is incomplete, it is recommended to use a different method to navigate the bot to the `DLITEReturnState.ClosestPoint` before attempting pathfinding using D* Lite again. +If `DLITEReturnState.ENUMState` is incomplete, it is recommended to use a different method to find the nearest valid point to navigate from. #### DLITEReturnState.path.pop() As the path is determined while the bot moves across it, pop must be used to determine the next location to move to. diff --git a/README.md b/README.md index ea73455..ee14e79 100644 --- a/README.md +++ b/README.md @@ -100,32 +100,38 @@ bot.on('chat', function(username, message) if (moveReturn === bot.move.ENUMStatus.Arrived) { if (endPoint.equals(position)) - bot.chat('I\'ve arrived!'); + resolve(position); - // Checks if bot hasnt moved since last replan. + // Checks if bot hasnt moved since last replan // This does not mean there is no path, the bot could have fallen off its know state and should rescan with a new state. else if (lastPoint && lastPoint.equals(position)) - bot.chat('I\'ve been blocked!'); + resolve(position); else { lastPoint = position; - returnState.path.replan(position).then(move); + bot.pathfind.to(position, endPoint, bot.pathfinder.ENUMPathfinder.DLITE).then(move); } } else { lastPoint = position; - returnState.path.replan(position).then(move); + bot.pathfind.to(position, endPoint, bot.pathfinder.ENUMPathfinder.DLITE).then(move); } }); } - bot.pathfinder.to( - bot.entity.position, - endPoint, - bot.pathfind.ENUMPathfinder.DLITE - ).then(move); + bot.pathfind + .to(bot.entity.position, endPoint) + .then(function(returnState) + { + if (returnState.ENUMState === bot.pathfinder.ENUMStatus.Incomplete) + endPoint = returnState.closestPoint; + + bot.pathfind + .to(bot.entity.position, endPoint, bot.pathfinder.ENUMPathfinder.DLITE) + .then(move); + }); } }); ``` diff --git a/index.js b/index.js index 4268204..75763b6 100644 --- a/index.js +++ b/index.js @@ -96,7 +96,7 @@ module.exports = function(bot) // Setup variables - bot.pathfinder.MAX_EXPANSIONS = 100000; // 10000 + bot.pathfinder.MAX_EXPANSIONS = 10000; // 10000 bot.pathfinder.HEURISTIC = function(p1, p2) {return p1.distanceTo(p2);}; bot.pathfinder.COST = bot.pathfinder.HEURISTIC;