/* 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; }