void ompl::geometric::BasicPRMmodif::growRoadmap(const std::vector<Milestone*> &start,
const std::vector<Milestone*> &goal,
const base::PlannerTerminationCondition &ptc,
base::State *workState)
{
while (ptc() == false)
{
// search for a valid state
bool found = false;
while (!found && ptc() == false)
{
unsigned int attempts = 0;
do
{
found = sampler_->sample(workState);
attempts++;
} while (attempts < FIND_VALID_STATE_ATTEMPTS_WITHOUT_TIME_CHECK && !found);
}
// add it as a milestone
if (found)
{
addMilestone(si_->cloneState(workState));
if (haveSolution(start, goal))
break;
}
}
}
void Rubik3D::keyboard(unsigned char key, int, int)
{
if(haveSolution())
{
return; // don't rotate until the Cube is solved!
}
if(key=='S')
{
AutoPlayOn=true;
return;
}
int x=0,y=0,z=0;
bool inv=(key & 32)==0;
key &= 95;
convertToCoordinates(key,x,y,z);
Singleton->twister(x,y,z,inv);
}
void Rubik3D::mymenu(int ID)
{
const bool inv=1-(ID%2); // even numbers
int x=0,y=0,z=0;
switch(ID)
{
case 1:
case 2:
x=1;
break;
case 3:
case 4:
y=1;
break;
case 5:
case 6:
z=1;
break;
case 7:
case 8:
x=-1;
break;
case 9:
case 10:
y=-1;
break;
case 11:
case 12:
z=-1;
break;
case 13:
AutoPlayOn=true;
break;
case 14:
glutLeaveMainLoop();
return;
default:
;
}
if(AutoPlayOn==false && haveSolution()==false)
{
Singleton->twister(x,y,z,inv);
}
}
bool ompl::geometric::BasicPRMmodif::solve(const base::PlannerTerminationCondition &ptc)
{
pis_.checkValidity();
base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
if (!goal)
{
msg_.error("Goal undefined or unknown type of goal");
return false;
}
std::vector<Milestone*> startM;
std::vector<Milestone*> goalM;
// add the valid start states as milestones
while (const base::State *st = pis_.nextStart())
startM.push_back(addMilestone(si_->cloneState(st)));
if (startM.size() == 0)
{
msg_.error("There are no valid initial states!");
return false;
}
if (!goal->canSample())
{
msg_.error("Insufficient states in sampleable goal region");
return false;
}
if (!sampler_)
sampler_ = si_->allocValidStateSampler();
unsigned int nrStartStates = milestones_.size();
msg_.inform("Starting with %u states", nrStartStates);
base::State *xstate = si_->allocState();
std::pair<Milestone*, Milestone*> solEndpoints;
while (ptc() == false)
{
// find at least one valid goal state
if (goal->maxSampleCount() > goalM.size())
{
const base::State *st = goalM.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
if (st)
goalM.push_back(addMilestone(si_->cloneState(st)));
if (goalM.empty())
{
msg_.error("Unable to find any valid goal states");
break;
}
}
// if there already is a solution, construct it
// if (haveSolution(startM, goalM, &solEndpoints))
// {
// constructSolution(solEndpoints.first, solEndpoints.second);
// break;
// }
// othewise, spend some time building a roadmap
else
{
// if it is worth looking at other goal regions, plan for part of the time
if (goal->maxSampleCount() > goalM.size())
growRoadmap(startM, goalM, boost::bind(&growRoadmapTerminationCondition, ptc, time::now() + time::seconds(0.1)), xstate);
// otherwise, just go ahead and build the roadmap
else
growRoadmap(startM, goalM, ptc, xstate);
// if a solution has been found, construct it
// if (haveSolution(startM, goalM, &solEndpoints))
// {
// constructSolution(solEndpoints.first, solEndpoints.second);
// break;
// }
}
}
if (haveSolution(startM, goalM, &solEndpoints))
{
constructSolution(solEndpoints.first, solEndpoints.second);
}
si_->freeState(xstate);
msg_.inform("Created %u states", milestones_.size() - nrStartStates);
return goal->isAchieved();
}
