Skip to content

Commit

Permalink
13.01.2019
Browse files Browse the repository at this point in the history 
General Work
- Reformatted D* Lite function
- Removed unoptimized D* Lite
- README changes
  • Loading branch information
@Suficio
Suficio committed Jan 13, 2019
1 parent 1012d2a commit 01e2b58
Show file tree
Hide file tree
Showing 8 changed files with 387 additions and 459 deletions.
10 changes: 10 additions & 0 deletions Pathfinders/ASTAR.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -108,6 +108,8 @@ module.exports = function(bot, sp, ep)

const MainPromise = new Promise(function(resolve)
{
const hrastart = process.hrtime();

// Maintains the element with the best path
let closest = new State(sp.floored());
closest.g = 0; // Otherwise POSITIVE_INFINITY - POSITIVE_INFINITY returns NaN
Expand All @@ -123,7 +125,12 @@ module.exports = function(bot, sp, ep)
{
const u = O.pop();
if (u === end)
{
const hraend = process.hrtime(hrastart);
bot.chat('New A* calculated world state in: ' + hraend[0] + 's ' + hraend[1] / 1000000 + 'ms');

return resolve({ENUMStatus: bot.pathfinder.ENUMStatus.Complete, State: u});
}

const successors = bot.pathfinder.getSuccessors(u.p);
for (let n = 0, len = successors.length; n < len; n++)
Expand All @@ -146,6 +153,9 @@ module.exports = function(bot, sp, ep)
if (u.f - u.g < closest.f - closest.g) closest = u;
};

const hraend = process.hrtime(hrastart);
bot.chat('New A* calculated world state in: ' + hraend[0] + 's ' + hraend[1] / 1000000 + 'ms');

return resolve({ENUMStatus: bot.pathfinder.ENUMStatus.Incomplete, State: closest});
});

Expand Down
331 changes: 331 additions & 0 deletions Pathfinders/DLITE.js
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,331 @@
'use strict';
const Heap = require('fastpriorityqueue');

module.exports = function(bot, sp, ep)
{
// D* Lite as per S. Koenig, 2002
// Compute Shortest Path and UpdateVertex as described in http://idm-lab.org/bib/abstracts/papers/aaai02b.pdf
// Based on an optimized version of D* Lite as presented in Figure 4.

// The advantage of using D* Lite is it precomputes the global state between the start and end point, this allows for
// convenient changes of costs at runtime, which can be used for entity avoidance.

function DLITEReturnState(MainPromise)
{
const ReturnState = this;
let ResolveFunction = function() {return;};

this.on = function(Callback)
{
ResolveFunction = function(IntermediateObject)
{
ReturnState.ENUMStatus = IntermediateObject.ENUMStatus;
if (IntermediateObject.ENUMStatus === bot.pathfinder.ENUMStatus.Incomplete)
{
console.warn(
'WARNING Pathfinder: Did not find path in allowed MAX_EXPANSIONS,',
'returned path to closest valid end point'
);
}

Callback(ReturnState);
};

MainPromise.then(function(IntermediateObject)
{
ResolveFunction(IntermediateObject);
}).catch(function(e) {console.error('ERROR Pathfinder:', e);});
};

// Path functions
this.path = {};
function _peek()
{
// First part of Main() in D*Lite
if (S.start === S.goal) return undefined;
if (ReturnState.OBSOLETE) return undefined;

let minState;
let minCost = Number.POSITIVE_INFINITY;

// Get successors according to stored state.
const successors = bot.pathfinder.getSuccessors(S.start.p);
for (let n = 0, len = successors.length; n < len; n++)
{
const sp = ExistingState(successors[n]);
if (sp)
{
const cost = bot.pathfinder.COST(S.start.p, sp.p) + sp.g;
if (minCost > cost)
{
minCost = cost;
minState = sp;
}
}
}

return minState;
}
this.path.peek = function()
{
const temp = _peek();
if (temp !== undefined)
return temp.p;
else return undefined;
};
this.path.pop = function()
{
const temp = _peek();
if (temp !== undefined)
{
S.start = temp;
return temp.p;
}
else return undefined;
};
this.path.replan = function()
{
console.log('replan');
const v = ReturnState.OBSOLETE;
ReturnState.OBSOLETE = undefined;

k_m = k_m + bot.pathfinder.HEURISTIC(S.last.p, S.start.p);
S.last = S.start;

// 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.p);
for (let n = 0, len = predecessors.length; n < len; n++)
{
const u = ExistingState(predecessors[n]);
if (u)
{
const c_old = bot.pathfinder.COST(u.p, v.p);
if (floatEqual(u.rhs, c_old + v.g) && u !== S.goal)
{
u.rhs = Number.POSITIVE_INFINITY;

const successors = bot.pathfinder.getSuccessors(u.p);
for (let m = 0, len = successors.length; m < len; m++)
{
const sp = new State(successors[m]);
const cost = bot.pathfinder.COST(u.p, sp.p) + sp.g;
if (u.rhs > cost) u.rhs = cost;
}
}
}

UpdateVertex(u);
}

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
{
ReturnState.OBSOLETE = v;
U.remove(U.check(v));
S[v.p.x >>> 0][v.p.y >>> 0][v.p.z >>> 0] = undefined;
}
});
};

// Global state functions
const S = [];
S.push = function(s)
{
const x = s.p.x >>> 0;
const y = s.p.y >>> 0;

if (!this[x])
this[x] = [];
if (!this[x][y])
this[x][y] = [];

this[x][y][s.p.z >>> 0] = s;
};
S.check = function(p)
{
const x = p.x >>> 0;
if (this[x])
{
const y = p.y >>> 0;
if (this[x][y])
{
if (this[x][y][p.z >>> 0])

return true;
}
} return false;
};

// Priority queue functions
const U = new Heap(function(s1, s2) {return CompareKeys(s1.k, s2.k);});
U.check = function(s)
{
for (let i = 0; i < this.size; i++)
if (this.array[i] === s) return i;
return undefined;
};
U.insert = function(s, k)
{
s.k = k;
this.add(s);
};
U.update = function(i, k)
{
// Priority queue handles the percolation automatically eitherway
this.array[i].k = k;
};

// Maintain familiarity with original heap implementation
U.remove = U._removeAt;
U.pop = U.poll;

// State functions
function State(p)
{
if (S.check(p))
return S[p.x >>> 0][p.y >>> 0][p.z >>> 0];
else
{
this.p = p;
this.g = Number.POSITIVE_INFINITY;
this.rhs = Number.POSITIVE_INFINITY;

this.k = [Number.POSITIVE_INFINITY, Number.POSITIVE_INFINITY];

S.push(this);
}
}

function ExistingState(p)
{
if (S.check(p))
return S[p.x >>> 0][p.y >>> 0][p.z >>> 0];
else return undefined;
}

// Algorithm functions
function CalculateKey(s)
{
return [Math.min(s.g, s.rhs) + bot.pathfinder.HEURISTIC(S.start.p, s.p) + k_m, Math.min(s.g, s.rhs)];
}

function UpdateVertex(u)
{
const exists = U.check(u); // Integer index
const equals = floatEqual(u.g, u.rhs);

if (!equals && exists !== undefined) U.update(exists, CalculateKey(u));
else if (!equals && exists === undefined) U.insert(u, CalculateKey(u));
else if (equals && exists !== undefined) U.remove(exists);
}

function ComputeShortestPath()
{
const CSPPromise = new Promise(function(resolve)
{
const hrstart = process.hrtime();

for (let i = 0; i < bot.pathfinder.MAX_EXPANSIONS && U.size !== 0 &&
(CompareKeys(U.peek().k, CalculateKey(S.start)) || S.start.rhs > S.start.g); i++)
{
const u = U.peek();
const k_old = u.k;
const k_new = CalculateKey(u);

// console.log(u);

if (CompareKeys(k_old, k_new))
U.update(u, k_new);

else if (u.g > u.rhs)
{
// console.log('second');
u.g = u.rhs;
U.pop(); // U.remove from first

const predecessors = bot.pathfinder.getPredecessors(u.p);
for (let n = 0, len = predecessors.length; n < len; n++)
{
const s = new State(predecessors[n]);

if (s !== S.goal) s.rhs = Math.min(s.rhs, bot.pathfinder.COST(s.p, u.p) + u.g);
UpdateVertex(s);
}
}
else
{
// console.log('third');
const g_old = u.g;
u.g = Number.POSITIVE_INFINITY;

const predecessors = bot.pathfinder.getPredecessors(u.p);
predecessors.push(u.p); // ∪ {u}
for (let n = 0, len = predecessors.length; n < len; n++)
{
const s = new State(predecessors[n]);
if (floatEqual(s, bot.pathfinder.COST(s.p, u.p) + g_old))
{
if (s !== R.S.goal)
{
s.rhs = Number.POSITIVE_INFINITY;

const successors = bot.pathfinder.getSuccessors(u.p);
for (let m = 0, len = successors.length; m < len; m++)
{
const sp = new State(successors[m]);
const cost = bot.pathfinder.COST(s.p, sp.p) + sp.g;
if (s.rhs > cost) s.rhs = cost;
}
}
}
UpdateVertex(s);
}
}
}
if (S.start.rhs === Number.POSITIVE_INFINITY)
resolve({ENUMStatus: bot.pathfinder.ENUMStatus.Incomplete});
else
resolve({ENUMStatus: bot.pathfinder.ENUMStatus.Complete});

const hrend = process.hrtime(hrstart);
bot.chat('D*LITE calculated world state in: ' + hrend[0] + 's ' + hrend[1] / 1000000 + 'ms');
});

return CSPPromise;
}

// Initialize

let k_m = 0;

S.start = new State(sp);
S.goal = new State(ep);
S.goal.rhs = 0;

S.last = S.start;

U.insert(
S.goal,
[bot.pathfinder.HEURISTIC(S.start.p, S.goal.p), 0]
);

return new DLITEReturnState(ComputeShortestPath());
};

function CompareKeys(k1, k2)
{
return (floatEqual(k1[0], k2[0]) && k1[1] < k2[1]) || k1[0] < k2[0];
}

function floatEqual(f1, f2)
{
if (f1 === Number.POSITIVE_INFINITY && f2 === Number.POSITIVE_INFINITY) return true;
return Math.abs(f1 - f2) < Number.EPSILON;
}
Loading

0 comments on commit 01e2b58

Please sign in to comment.