void Token::print() const {
if( eol() ) std::cout << "NEWLINE" ;
else if( eof() ) std::cout << "ENDMARKER" ;
else if( indent() ) std::cout << "INDENT";
else if( dedent() ) std::cout << "DEDENT";
else if( isOpenBrace() ) std::cout << " { ";
else if( isCloseBrace() ) std::cout << " } ";
else if( isComma() ) std::cout << " , ";
else if( isPeriod()) std::cout<< ".";
else if( isEqual() ) std::cout << " == ";
else if( isNotEqual() ) std::cout << " != ";
else if( isLessThan() ) std::cout << " < ";
else if( isGreaterThan() ) std::cout << " > ";
else if( isLessThanEqual() ) std::cout << " <= ";
else if( isGreaterThanEqual() ) std::cout << " >= ";
else if( isOpenParen() ) std::cout << " ( " ;
else if( isCloseParen() ) std::cout << " ) " ;
else if( isAssignmentOperator() ) std::cout << " = ";
else if( isColon() ) std::cout << " : " ;
else if( isMultiplicationOperator() ) std::cout << " * " ;
else if( isAdditionOperator() ) std::cout << " + ";
else if( isSubtractionOperator() ) std::cout << " - ";
else if( isModuloOperator() ) std::cout << " % ";
else if( isDivisionOperator() ) std::cout << " / ";
else if( isFloorDivision() ) std::cout << " // ";
else if( isOpenBrack() ) std::cout<< "[";
else if( isCloseBrack() ) std::cout<< "]";
else if( isName() ) std::cout << getName();
else if( isKeyword() ) std::cout << getKeyword();
else if( isWholeNumber() ) std::cout << getWholeNumber();
else if( isFloat() ) std::cout << getFloat();
else if( isString() ) std::cout << getString();
else if( isCall() ) std::cout << "CALL " << getName();
else if( isSub() ) std::cout << "ARRAY SUB " << getName();
else if( isAppend() ) std::cout << "ARRAY APPEND " << getName();
else if( isPop() ) std::cout << "ARRAY POP " << getName();
else std::cout << "Uninitialized token.\n";
}
dirnode* DirTree::getDir(string p)
// Try to obtain a directory given its name.
{
vector<string>* path_toks = tokenizeV(p.substr((root->name).size()),"/");
// Set the current node to the root of the directory tree, and start an
// iterator on the current node's directory list.
dirnode* currnode = root;
list<dirnode*>::iterator d = currnode->dirs.begin();
// While there are still more paths to navigate...
for (size_t i = 0; i < path_toks->size();)
{
// Scan the directory list for the current directory.
for (; d != currnode->dirs.end() && strcmp(path_toks->at(i).c_str(),(*d)->name.c_str()) != 0 && !isLessThan(path_toks->at(i),(*d)->name); d++);
// If current directory exists, go to it.
if (d != currnode->dirs.end() && strcmp(path_toks->at(i).c_str(),(*d)->name.c_str()) == 0)
{
currnode = *d;
d = currnode->dirs.begin();
i++;
}
// If current directory does not exist, return null
else
return NULL;
}
return *d;
}
ompl::base::PlannerStatus ompl::geometric::LBTRRT::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
// update goal and check validity
base::Goal *goal = pdef_->getGoal().get();
base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
if (!goal)
{
OMPL_ERROR("%s: Goal undefined", getName().c_str());
return base::PlannerStatus::INVALID_GOAL;
}
// update start and check validity
while (const base::State *st = pis_.nextStart())
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state_, st);
motion->id_ = nn_->size();
idToMotionMap_.push_back(motion);
nn_->add(motion);
lowerBoundGraph_.addVertex(motion->id_);
}
if (nn_->size() == 0)
{
OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
if (nn_->size() > 1)
{
OMPL_ERROR("%s: There are multiple start states - currently not supported!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
if (!sampler_)
sampler_ = si_->allocStateSampler();
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
Motion *solution = lastGoalMotion_;
Motion *approxSol = nullptr;
double approxdif = std::numeric_limits<double>::infinity();
// e*(1+1/d) K-nearest constant, as used in RRT*
double k_rrg = boost::math::constants::e<double>() +
boost::math::constants::e<double>() / (double)si_->getStateDimension();
Motion *rmotion = new Motion(si_);
base::State *rstate = rmotion->state_;
base::State *xstate = si_->allocState();
unsigned int statesGenerated = 0;
bestCost_ = lastGoalMotion_ ? lastGoalMotion_->costApx_ : std::numeric_limits<double>::infinity();
while (ptc() == false)
{
iterations_++;
/* sample random state (with goal biasing) */
if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
goal_s->sampleGoal(rstate);
else
sampler_->sampleUniform(rstate);
/* find closest state in the tree */
Motion *nmotion = nn_->nearest(rmotion);
base::State *dstate = rstate;
/* find state to add */
double d = si_->distance(nmotion->state_, rstate);
if (d == 0) // this takes care of the case that the goal is a single point and we re-sample it multiple times
continue;
if (d > maxDistance_)
{
si_->getStateSpace()->interpolate(nmotion->state_, rstate, maxDistance_ / d, xstate);
dstate = xstate;
}
if (checkMotion(nmotion->state_, dstate))
{
statesGenerated++;
/* create a motion */
Motion *motion = new Motion(si_);
si_->copyState(motion->state_, dstate);
/* update fields */
double distN = distanceFunction(nmotion, motion);
motion->id_ = nn_->size();
idToMotionMap_.push_back(motion);
lowerBoundGraph_.addVertex(motion->id_);
motion->parentApx_ = nmotion;
std::list<std::size_t> dummy;
lowerBoundGraph_.addEdge(nmotion->id_, motion->id_, distN, false, dummy);
motion->costLb_ = nmotion->costLb_ + distN;
motion->costApx_ = nmotion->costApx_ + distN;
nmotion->childrenApx_.push_back(motion);
std::vector<Motion*> nnVec;
unsigned int k = std::ceil(k_rrg * log((double)(nn_->size() + 1)));
nn_->nearestK(motion, k, nnVec);
nn_->add(motion); // if we add the motion before the nearestK call, we will get ourselves...
IsLessThan isLessThan(this, motion);
std::sort(nnVec.begin(), nnVec.end(), isLessThan);
//-------------------------------------------------//
// Rewiring Part (i) - find best parent of motion //
//-------------------------------------------------//
if (motion->parentApx_ != nnVec.front())
{
for (std::size_t i(0); i < nnVec.size(); ++i)
{
Motion *potentialParent = nnVec[i];
double dist = distanceFunction(potentialParent, motion);
considerEdge(potentialParent, motion, dist);
}
}
//------------------------------------------------------------------//
// Rewiring Part (ii) //
// check if motion may be a better parent to one of its neighbors //
//------------------------------------------------------------------//
for (std::size_t i(0); i < nnVec.size(); ++i)
{
Motion *child = nnVec[i];
double dist = distanceFunction(motion, child);
considerEdge(motion, child, dist);
}
double dist = 0.0;
bool sat = goal->isSatisfied(motion->state_, &dist);
if (sat)
{
approxdif = dist;
solution = motion;
}
if (dist < approxdif)
{
approxdif = dist;
approxSol = motion;
}
if (solution != nullptr && bestCost_ != solution->costApx_)
{
OMPL_INFORM("%s: approximation cost = %g", getName().c_str(),
solution->costApx_);
bestCost_ = solution->costApx_;
}
}
}
bool solved = false;
bool approximate = false;
if (solution == nullptr)
{
solution = approxSol;
approximate = true;
}
if (solution != nullptr)
{
lastGoalMotion_ = solution;
/* construct the solution path */
std::vector<Motion*> mpath;
while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->parentApx_;
}
/* set the solution path */
PathGeometric *path = new PathGeometric(si_);
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
path->append(mpath[i]->state_);
// Add the solution path.
base::PathPtr bpath(path);
base::PlannerSolution psol(bpath);
psol.setPlannerName(getName());
if (approximate)
psol.setApproximate(approxdif);
pdef_->addSolutionPath(psol);
solved = true;
}
si_->freeState(xstate);
if (rmotion->state_)
si_->freeState(rmotion->state_);
delete rmotion;
OMPL_INFORM("%s: Created %u states", getName().c_str(), statesGenerated);
return base::PlannerStatus(solved, approximate);
}
bool isGreaterThan(TreeNode *node, int val) {
return node == NULL ||
(node->val > val && isGreaterThan(node->right, node->val) &&
isGreaterThan(node->left, val) &&
isLessThan(node->left, node->val));
}
bool isValidBST(TreeNode* root) {
return root == NULL ||
(isLessThan(root->left, root->val) &&
isGreaterThan(root->right, root->val));
}
void Galaxy::buildByName()
// Build a galaxy by organizing files by their names.
{
list<filenode*> sorted_files;
list<list<filenode*>*> file_lists;
size_t charindex = 0;
// Sort the galaxy's file list by name of the files.
list<filenode*>::iterator i = files->begin();
list<filenode*>::iterator j;
sorted_files.push_back(*i);
i++;
while (sorted_files.size() < files->size())
{
j = sorted_files.begin();
for(;j != sorted_files.end() && isLessThan((*j)->name,(*i)->name);j++);
if (j == sorted_files.end()) sorted_files.push_back(*i);
else sorted_files.insert(j,(*i));
i++;
}
// Done sorting the sectors.
// Preliminary test. Go through the sorted list, and see at what index do
// the file names start being different.
i = sorted_files.end();
filenode *b = (*(sorted_files.begin()));
for (charindex = 0; i == sorted_files.end(); charindex++)
{
for (i = sorted_files.begin(); i != sorted_files.end(); i++)
if (charindex < (*i)->name.size() && charindex < b->name.size())
if ((*i)->name[charindex] != b->name[charindex])
break;
}
// End preliminary test.
// Begin splitting sorted list.
i = sorted_files.begin();
list<filenode*>* temp = new list<filenode*>;
temp->push_back(*i);
j = i;
j++;
while (j != sorted_files.end())
{
if (charindex >= (*j)->name.size() || charindex >= (*i)->name.size() || (*j)->name[charindex] != (*i)->name[charindex])
{
file_lists.push_back(temp);
temp = new list<filenode*>;
}
temp->push_back(*j);
i = j;
j++;
}
file_lists.push_back(temp);
// End splitting sorted list.
// Start making the sectors.
float arc_begin = 0;
float arc_width = 0;
cout << "creating sectors divided by name\n";
for (list<list<filenode*>*>::iterator i = file_lists.begin(); i != file_lists.end(); i++)
{
string name = (*((*i)->begin()))->name;
name = name.substr(0,charindex);
arc_begin += arc_width;
arc_width = 360.0*((float)(*i)->size()/(float)sorted_files.size());
sectors->push_back(new GSector(NULL,(*i),radius,arc_begin,arc_width,name));
}
// End making sectors.
}
