/* Dstar.cpp
* James Neufeld (neufeld@cs.ualberta.ca)
*/
#include "Dstar.h"
#include <stdio.h>
#ifdef USE_OPEN_GL
#ifdef MACOS
#include <OpenGL/gl.h>
#else
#include <GL/gl.h>
#endif
#endif
/* void Dstar::Dstar()
* --------------------------
* Constructor sets constants.
*/
Dstar::Dstar() {
maxSteps = 80000; // node expansions before we give up
C1 = 1; // cost of an unseen cell
}
/* float Dstar::keyHashCode(state u)
* --------------------------
* Returns the key hash code for the state u, this is used to compare
* a state that have been updated
*/
float Dstar::keyHashCode(state u) {
return (float)(u.k.first + 1193*u.k.second);
}
/* bool Dstar::isValid(state u)
* --------------------------
* Returns true if state u is on the open list or not by checking if
* it is in the hash table.
*/
bool Dstar::isValid(state u) {
ds_oh::iterator cur = openHash.find(u);
if (cur == openHash.end()) return false;
if (!close(keyHashCode(u), cur->second)) return false;
return true;
}
/* void Dstar::getPath()
* --------------------------
* Returns the path created by replan()
*/
list<state> Dstar::getPath() {
return path;
}
/* bool Dstar::occupied(state u)
* --------------------------
* returns true if the cell is occupied (non-traversable), false
* otherwise. non-traversable are marked with a cost < 0.
*/
bool Dstar::occupied(state u) {
ds_ch::iterator cur = cellHash.find(u);
if (cur == cellHash.end()) return false;
return (cur->second.cost < 0);
}
/* void Dstar::init(int sX, int sY, int gX, int gY)
* --------------------------
* Init dstar with start and goal coordinates, rest is as per
* [S. Koenig, 2002]
*/
void Dstar::init(int sX, int sY, int gX, int gY) {
cellHash.clear();
path.clear();
openHash.clear();
while(!openList.empty()) openList.pop();
k_m = 0;
s_start.x = sX;
s_start.y = sY;
s_goal.x = gX;
s_goal.y = gY;
cellInfo tmp;
tmp.g = tmp.rhs = 0;
tmp.cost = C1;
cellHash[s_goal] = tmp;
tmp.g = tmp.rhs = heuristic(s_start,s_goal);
tmp.cost = C1;
cellHash[s_start] = tmp;
s_start = calculateKey(s_start);
s_last = s_start;
}
/* void Dstar::makeNewCell(state u)
* --------------------------
* Checks if a cell is in the hash table, if not it adds it in.
*/
void Dstar::makeNewCell(state u) {
if (cellHash.find(u) != cellHash.end()) return;
cellInfo tmp;
tmp.g = tmp.rhs = heuristic(u,s_goal);
tmp.cost = C1;
cellHash[u] = tmp;
}
/* double Dstar::getG(state u)
* --------------------------
* Returns the G value for state u.
*/
double Dstar::getG(state u) {
if (cellHash.find(u) == cellHash.end())
return heuristic(u,s_goal);
return cellHash[u].g;
}
/* double Dstar::getRHS(state u)
* --------------------------
* Returns the rhs value for state u.
*/
double Dstar::getRHS(state u) {
if (u == s_goal) return 0;
if (cellHash.find(u) == cellHash.end())
return heuristic(u,s_goal);
return cellHash[u].rhs;
}
/* void Dstar::setG(state u, double g)
* --------------------------
* Sets the G value for state u
*/
void Dstar::setG(state u, double g) {
makeNewCell(u);
cellHash[u].g = g;
}
/* void Dstar::setRHS(state u, double rhs)
* --------------------------
* Sets the rhs value for state u
*/
double Dstar::setRHS(state u, double rhs) {
makeNewCell(u);
cellHash[u].rhs = rhs;
}
/* double Dstar::eightCondist(state a, state b)
* --------------------------
* Returns the 8-way distance between state a and state b.
*/
double Dstar::eightCondist(state a, state b) {
double temp;
double min = abs(a.x - b.x);
double max = abs(a.y - b.y);
if (min > max) {
double temp = min;
min = max;
max = temp;
}
return ((M_SQRT2-1.0)*min + max);
}
/* int Dstar::computeShortestPath()
* --------------------------
* As per [S. Koenig, 2002] except for 2 main modifications:
* 1. We stop planning after a number of steps, 'maxsteps' we do this
* because this algorithm can plan forever if the start is
* surrounded by obstacles.
* 2. We lazily remove states from the open list so we never have to
* iterate through it.
*/
int Dstar::computeShortestPath() {
list<state> s;
list<state>::iterator i;
if (openList.empty()) return 1;
int k=0;
while ((!openList.empty()) &&
(openList.top() < (s_start = calculateKey(s_start))) ||
(getRHS(s_start) != getG(s_start))) {
if (k++ > maxSteps) {
fprintf(stderr, "At maxsteps\n");
return -1;
}
state u;
bool test = (getRHS(s_start) != getG(s_start));
// lazy remove
while(1) {
if (openList.empty()) return 1;
u = openList.top();
openList.pop();
if (!isValid(u)) continue;
if (!(u < s_start) && (!test)) return 2;
break;
}
ds_oh::iterator cur = openHash.find(u);
openHash.erase(cur);
state k_old = u;
if (k_old < calculateKey(u)) { // u is out of date
insert(u);
} else if (getG(u) > getRHS(u)) { // needs update (got better)
setG(u,getRHS(u));
getPred(u,s);
for (i=s.begin();i != s.end(); i++) {
updateVertex(*i);
}
} else { // g <= rhs, state has got worse
setG(u,INFINITY);
getPred(u,s);
for (i=s.begin();i != s.end(); i++) {
updateVertex(*i);
}
updateVertex(u);
}
}
return 0;
}
/* bool Dstar::close(double x, double y)
* --------------------------
* Returns true if x and y are within 10E-5, false otherwise
*/
bool Dstar::close(double x, double y) {
if (isinf(x) && isinf(y)) return true;
return (fabs(x-y) < 0.00001);
}
/* void Dstar::updateVertex(state u)
* --------------------------
* As per [S. Koenig, 2002]
*/
void Dstar::updateVertex(state u) {
list<state> s;
list<state>::iterator i;
if (u != s_goal) {
getSucc(u,s);
double tmp = INFINITY;
double tmp2;
for (i=s.begin();i != s.end(); i++) {
tmp2 = getG(*i) + cost(u,*i);
if (tmp2 < tmp) tmp = tmp2;
}
if (!close(getRHS(u),tmp)) setRHS(u,tmp);
}
if (!close(getG(u),getRHS(u))) insert(u);
}
/* void Dstar::insert(state u)
* --------------------------
* Inserts state u into openList and openHash.
*/
void Dstar::insert(state u) {
ds_oh::iterator cur;
float csum;
u = calculateKey(u);
cur = openHash.find(u);
csum = keyHashCode(u);
// return if cell is already in list. TODO: this should be
// uncommented except it introduces a bug, I suspect that there is a
// bug somewhere else and having duplicates in the openList queue
// hides the problem...
//if ((cur != openHash.end()) && (close(csum,cur->second))) return;
openHash[u] = csum;
openList.push(u);
}
/* void Dstar::remove(state u)
* --------------------------
* Removes state u from openHash. The state is removed from the
* openList lazilily (in replan) to save computation.
*/
void Dstar::remove(state u) {
ds_oh::iterator cur = openHash.find(u);
if (cur == openHash.end()) return;
openHash.erase(cur);
}
/* double Dstar::trueDist(state a, state b)
* --------------------------
* Euclidean cost between state a and state b.
*/
double Dstar::trueDist(state a, state b) {
float x = a.x-b.x;
float y = a.y-b.y;
return sqrt(x*x + y*y);
}
/* double Dstar::heuristic(state a, state b)
* --------------------------
* Pretty self explanitory, the heristic we use is the 8-way distance
* scaled by a constant C1 (should be set to <= min cost).
*/
double Dstar::heuristic(state a, state b) {
return eightCondist(a,b)*C1;
}
/* state Dstar::calculateKey(state u)
* --------------------------
* As per [S. Koenig, 2002]
*/
state Dstar::calculateKey(state u) {
double g=getG(u);
double rhs=getRHS(u);
//double val = fmin(g,rhs);
if (g > rhs) {
u.k.first = rhs + 4*heuristic(u,s_start) + k_m;
u.k.second = rhs;}
else {
u.k.first = g + heuristic(u,s_start)
评论4
最新资源