/* bool Dstar::replan()
* --------------------------
* Updates the costs for all cells and computes the shortest path to
* goal. Returns true if a path is found, false otherwise. The path is
* computed by doing a greedy search over the cost+g values in each
* cells. In order to get around the problem of the robot taking a
* path that is near a 45 degree angle to goal we break ties based on
* the metric euclidean(state, goal) + euclidean(state,start).
*/
bool Dstar::replan() {
path.clear();
int res = computeShortestPath();
//printf("res: %d ols: %d ohs: %d tk: [%f %f] sk: [%f %f] sgr: (%f,%f)\n",res,openList.size(),openHash.size(),openList.top().k.first,openList.top().k.second, s_start.k.first, s_start.k.second,getRHS(s_start),getG(s_start));
if (res < 0) {
fprintf(stderr, "NO PATH TO GOAL\n");
return false;
}
list<state> n;
list<state>::iterator i;
state cur = s_start;
if (isinf(getG(s_start))) {
fprintf(stderr, "NO PATH TO GOAL\n");
return false;
}
while(cur != s_goal) {
path.push_back(cur);
getSucc(cur, n);
if (n.empty()) {
fprintf(stderr, "NO PATH TO GOAL\n");
return false;
}
double cmin = INFINITY;
double tmin;
state smin;
for (i=n.begin(); i!=n.end(); i++) {
//if (occupied(*i)) continue;
double val = cost(cur,*i);
double val2 = trueDist(*i,s_goal) + trueDist(s_start,*i); // (Euclidean) cost to goal + cost to pred
val += getG(*i);
if (close(val,cmin)) {
if (tmin > val2) {
tmin = val2;
cmin = val;
smin = *i;
}
} else if (val < cmin) {
tmin = val2;
cmin = val;
smin = *i;
}
}
n.clear();
cur = smin;
}
path.push_back(s_goal);
return true;
}
void Nodo::elimHijo(int cont){
Index* key = llaves[cont];
if (hijos[cont]->cant_Key >= Orden){
Index*pred = getPred(cont);
llaves[cont] = pred;
hijos[cont]->eliminar(pred);
}else if(hijos[cont+1]->cant_Key >= Orden){
Index* succ = getSucc(cont);
llaves[cont] = succ;
hijos[cont+1]->eliminar(succ);
}
else{
merge(cont);
hijos[cont]->eliminar(key);
}
return;
}
/* 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);
}
bool Icc_parser::parseFromStream(std::istream &is)
{
int strnum = 0;
bool isContextFound = false;
BBlock * tbb = 0;
string fnname;
BBlock * zeroBlock = 0;
int zeroBlockStr;
//dump_info.addFunction();
while ( !is.eof())
{
string current;
int tmp;
getline( is, current);
strnum++;
if ( ( tmp = bbNum( current)) != -1)
{
if ( tmp == 0) //Handling zero blocks for detecting function names
{
zeroBlock = new BBlock( strnum);
zeroBlockStr = strnum;
tbb = zeroBlock;
}
else
tbb = dump_info.addBBlock( tmp, strnum, fnname);
isContextFound = false;
while ( !getPred( current, *tbb))
{
getline( is, current);
strnum++;
}
while ( !getSucc( current, *tbb))
{
getline( is, current);
strnum++;
}
continue;
}
if ( isRootContext( current))
isContextFound = false;
if ( isContextFound)
{
if ( !tbb)
throw exNotFound( -1);
string t;
if ( ( t = isFnName(current))!= "" && zeroBlock)
{
fnname = t;
dump_info.addFunction( fnname);
tbb = dump_info.addBBlock( 0, zeroBlockStr, fnname, tbb);
delete zeroBlock;
zeroBlock = 0;
}
tbb->addText( current, strnum);
}
isContextFound |= isContext( current);
}
return true;
}
/* bool Dstar::replan()
* --------------------------
* Updates the costs for all cells and computes the shortest path to
* goal. Returns true if a path is found, false otherwise. The path is
* computed by doing a greedy search over the cost+g values in each
* cells. In order to get around the problem of the robot taking a
* path that is near a 45 degree angle to goal we break ties based on
* the metric euclidean(state, goal) + euclidean(state,start).
*/
bool Dstar::replan() {
path.clear();
int res = computeShortestPath();
// printf("res: %d ols: %d ohs: %d tk: [%f %f] sk: [%f %f] sgr: (%f,%f)\n",res,openList.size(),openHash.size(),openList.top().k.first,openList.top().k.second, s_start.k.first, s_start.k.second,getRHS(s_start),getG(s_start));
if (res < 0) {
//fprintf(stderr, "NO PATH TO GOAL\n");
path.cost = INFINITY;
return false;
}
list<state> n;
list<state>::iterator i;
state cur = s_start;
state prev = s_start;
if (isinf(getG(s_start))) {
//fprintf(stderr, "NO PATH TO GOAL\n");
path.cost = INFINITY;
return false;
}
// constructs the path
while(cur != s_goal) {
path.path.push_back(cur);
path.cost += cost(prev,cur);
getSucc(cur, n);
if (n.empty()) {
//fprintf(stderr, "NO PATH TO GOAL\n");
path.cost = INFINITY;
return false;
}
// choose the next node in the path by selecting the one with smallest
// g() + cost. Break ties by choosing the neighbour that is closest
// to the line between start and goal (i.e. smallest sum of Euclidean
// distances to start and goal).
double cmin = INFINITY;
double tmin = INFINITY;
state smin = cur;
for (i=n.begin(); i!=n.end(); i++) {
if (occupied(*i)) continue;
double val = cost(cur,*i);
double val2 = trueDist(*i,s_goal) + trueDist(s_start,*i); // (Euclidean) cost to goal + cost to pred
double val3 = getG(*i);
val += val3;
// tiebreak if curent neighbour is equal to current best
// choose the neighbour that has the smallest tmin value
if (!isinf(val) && near(val,cmin)) {
if (val2 < tmin) {
tmin = val2;
cmin = val;
smin = *i;
}
}
// if next neighbour (*i) is scrictly lower cost than the
// current best, then set it to be the current best.
else if (val < cmin) {
tmin = val2;
cmin = val;
smin = *i;
}
} // end for loop
n.clear();
if( isinf(cmin) ) break;
prev = cur;
cur = smin;
} // end while loop
path.path.push_back(s_goal);
path.cost += cost(prev,s_goal);
return true;
}
