int ralloc(ident_t id, int regmask){
int reg = 0, i;
if(((store_t)(id->extra))->cur_reg) return ((store_t)(id->extra))->cur_reg;
if(((store_t)(id->extra))->wanted_reg){
reg = ((store_t)(id->extra))->wanted_reg;
}else{
for(i = R_EAX; i < R_END; i<<=1)
if((i & regmask) && rstate(i)){
reg = i;
break;
}
if(!reg)reg = listoldest(regmask);
}
if(!rstate(reg)){
rfree(reg);
}
rlock(reg);
((store_t)(id->extra))->cur_reg=reg;
listadd(id);
return reg;
}
void rfree(int reg){
list_t p = record, q = 0;
if(!reg)return;
if(rstate(reg))return;
runlock(reg);
while(p){
if(((store_t)(p->id->extra))->cur_reg == reg){
if(q)
q->next = p->next;
else
record = p->next;
if(p->id->loc == LOC_GLOBAL || p->id->type == VAR || (p->id->last >= 0 && current_index <= p->id->last))
rstore(p->id);
((store_t)(p->id->extra))->cur_reg = 0;
mfree(p);
break;
}
q = p;
p = p->next;
}
}
int
VerifyState(const Options &opts, const ReadUserLog::FileState &state )
{
ReadUserLogState rstate( state, 60 );
const ReadUserLogState::FileState *istate;
ReadUserLogState::convertState( state, istate );
const FieldData *wdata = opts.getField( );
if ( wdata == NULL ) {
fprintf( stderr, "Verify: no field!\n" );
return -1;
}
bool ok;
switch( wdata->m_field )
{
case FIELD_SIGNATURE:
ok = CompareStr( opts, istate->m_signature );
break;
case FIELD_VERSION:
ok = CompareInt( opts, istate->m_version );
break;
case FIELD_UPDATE_TIME:
ok = CompareTime( opts, istate->m_update_time );
break;
case FIELD_BASE_PATH:
ok = CompareStr( opts, istate->m_base_path );
break;
case FIELD_CUR_PATH:
ok = CompareStr( opts, rstate.CurPath(state) );
break;
case FIELD_UNIQ_ID:
ok = CompareStr( opts, istate->m_uniq_id );
break;
case FIELD_SEQUENCE:
ok = CompareInt( opts, istate->m_sequence );
break;
case FIELD_MAX_ROTATION:
ok = CompareInt( opts, istate->m_max_rotations );
break;
case FIELD_ROTATION:
ok = CompareInt( opts, istate->m_rotation );
break;
case FIELD_OFFSET:
ok = CompareFsize( opts, istate->m_offset.asint );
break;
case FIELD_EVENT_NUM:
ok = CompareFsize( opts, istate->m_event_num.asint );
break;
case FIELD_GLOBAL_POSITION:
ok = CompareFsize( opts, istate->m_log_position.asint );
break;
case FIELD_GLOBAL_RECORD_NUM:
ok = CompareFsize( opts, istate->m_log_record.asint );
break;
case FIELD_INODE:
ok = CompareInode( opts, istate->m_inode );
break;
case FIELD_CTIME:
ok = CompareTime( opts, istate->m_ctime );
break;
case FIELD_SIZE:
ok = CompareFsize( opts, istate->m_size.asint );
break;
default:
return -1;
}
return ok ? 0 : 1;
}
int
DumpState( const Options &opts,
const ReadUserLog::FileState &state,
const FieldData *wdata )
{
ReadUserLogState rstate( state, 60 );
const ReadUserLogState::FileState *istate;
ReadUserLogState::convertState( state, istate );
if ( NULL == wdata ) {
wdata = opts.getField( );
}
switch( wdata->m_field )
{
case FIELD_NONE:
return -1;
break;
case FIELD_SIGNATURE:
printf( " %s: '%s'\n", wdata->m_name, istate->m_signature );
break;
case FIELD_VERSION:
printf( " %s: %d\n", wdata->m_name, istate->m_version );
break;
case FIELD_UPDATE_TIME:
if ( opts.getNumeric() ) {
printf( " %s: %lu\n", wdata->m_name,
(long unsigned) istate->m_update_time );
} else {
printf( " %s: '%s'\n", wdata->m_name,
timestr(istate->m_update_time) );
}
break;
case FIELD_BASE_PATH:
printf( " %s: '%s'\n", wdata->m_name, istate->m_base_path );
break;
case FIELD_CUR_PATH:
printf( " %s: '%s'\n", wdata->m_name, rstate.CurPath(state) );
break;
case FIELD_UNIQ_ID:
printf( " %s: '%s'\n", wdata->m_name, istate->m_uniq_id );
break;
case FIELD_SEQUENCE:
printf( " %s: %d\n", wdata->m_name, istate->m_sequence );
break;
case FIELD_MAX_ROTATION:
printf( " %s: %d\n", wdata->m_name, istate->m_max_rotations );
break;
case FIELD_ROTATION:
printf( " %s: %d\n", wdata->m_name, istate->m_rotation );
break;
case FIELD_OFFSET:
printf( " %s: " FILESIZE_T_FORMAT "\n",
wdata->m_name, istate->m_offset.asint );
break;
case FIELD_EVENT_NUM:
printf( " %s: " FILESIZE_T_FORMAT "\n",
wdata->m_name, istate->m_event_num.asint );
break;
case FIELD_GLOBAL_POSITION:
printf( " %s: " FILESIZE_T_FORMAT "\n",
wdata->m_name, istate->m_log_position.asint );
break;
case FIELD_GLOBAL_RECORD_NUM:
printf( " %s: " FILESIZE_T_FORMAT "\n",
wdata->m_name, istate->m_log_record.asint );
break;
case FIELD_INODE:
printf( " %s: %lu\n", wdata->m_name,
(long unsigned) istate->m_inode );
break;
case FIELD_CTIME:
if ( opts.getNumeric() ) {
printf( " %s: %lu\n", wdata->m_name,
(long unsigned) istate->m_ctime );
} else {
printf( " %s: '%s'\n", wdata->m_name,
timestr(istate->m_ctime) );
}
break;
case FIELD_SIZE:
printf( " %s: " FILESIZE_T_FORMAT "\n",
wdata->m_name, istate->m_size.asint );
break;
case FIELD_ALL:
DumpState( opts, state, opts.lookupField(FIELD_SIGNATURE) );
DumpState( opts, state, opts.lookupField(FIELD_VERSION) );
DumpState( opts, state, opts.lookupField(FIELD_UPDATE_TIME) );
DumpState( opts, state, opts.lookupField(FIELD_BASE_PATH) );
DumpState( opts, state, opts.lookupField(FIELD_CUR_PATH) );
DumpState( opts, state, opts.lookupField(FIELD_UNIQ_ID) );
DumpState( opts, state, opts.lookupField(FIELD_SEQUENCE) );
DumpState( opts, state, opts.lookupField(FIELD_MAX_ROTATION) );
DumpState( opts, state, opts.lookupField(FIELD_ROTATION) );
DumpState( opts, state, opts.lookupField(FIELD_OFFSET) );
DumpState( opts, state, opts.lookupField(FIELD_EVENT_NUM) );
DumpState( opts, state, opts.lookupField(FIELD_GLOBAL_POSITION) );
DumpState( opts, state, opts.lookupField(FIELD_GLOBAL_RECORD_NUM) );
DumpState( opts, state, opts.lookupField(FIELD_INODE) );
DumpState( opts, state, opts.lookupField(FIELD_CTIME) );
DumpState( opts, state, opts.lookupField(FIELD_SIZE) );
break;
default:
return -1;
}
return 0;
}
ompl::base::PlannerStatus
ompl::geometric::TRRTConnect::solve(const base::PlannerTerminationCondition &ptc) {
//Basic error checking
checkValidity();
//Goal information
base::GoalSampleableRegion *goal(dynamic_cast<base::GoalSampleableRegion*>
(pdef_->getGoal().get()));
if (!goal) {
OMPL_ERROR("%s: Unknown type of goal",getName().c_str());
return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
}
//Input States
//Loop through valid input states and add to tree
while (const base::State *st = pis_.nextStart()) {
//Create a motion
Motion *motion(new Motion(si_));
si_->copyState(motion->state,st);
motion->root = motion->state;
//Add motion to the tree
tStart_->add(motion);
si_->copyState(tStart_.root,motion->root);
//Check if now the tree has a motion inside BoxZos
if (!tStart_.stateInBoxZos_) {
tStart_.stateInBoxZos_ = ((ompl::base::FOSOptimizationObjective*)opt_.get())->
boxZosDistance(motion->state) < DBL_EPSILON;
if (tStart_.stateInBoxZos_) tStart_.temp_ = initTemperature_;
}
//Update frontier nodes and non frontier nodes count
++tStart_.frontierCount_;//Participates in the tree expansion
}
//Check that input states exist
if (tStart_->size() == 0) {
OMPL_ERROR("%s: Motion planning start tree could not be initialized!",
getName().c_str());
return base::PlannerStatus::INVALID_START;
}
//Check that goal states exist
if (!goal->couldSample()) {
OMPL_ERROR("%s: Insufficient states in sampleable goal region",
getName().c_str());
return base::PlannerStatus::INVALID_GOAL;
}
//Create state sampler if the first run
if (!sampler_) sampler_ = si_->allocStateSampler();
OMPL_INFORM("%s: Starting planning with %d states already in datastructure",
getName().c_str(),int(tStart_->size() + tGoal_->size()));
Motion *rmotion(new Motion(si_));
base::State *rstate(rmotion->state);
TreeGrowingInfo tgi;
tgi.xstate = si_->allocState();
bool startTree(true);
bool solved(false);
double temp(0.0);
unsigned int iter(0);
extendCount = 0;
connectCount = 0;
//Begin sampling
while (!ptc) {
//Choose tree to extend
TreeData &tree(startTree ? tStart_ : tGoal_);
startTree = !startTree;
TreeData &otherTree(startTree ? tStart_ : tGoal_);
if (((2*pis_.getSampledGoalsCount()) < (tGoal_->size())) ||
(tGoal_->size() == 0)) {
const base::State *st((tGoal_->size() == 0) ?
pis_.nextGoal(ptc) : pis_.nextGoal());
if (st) {
//Create a motion
Motion *motion(new Motion(si_));
si_->copyState(motion->state,st);
motion->root = motion->state;
//Add motion to the tree
tGoal_->add(motion);
si_->copyState(tGoal_.root,motion->root);
//Check if now the tree has a motion inside BoxZos
if (!tGoal_.stateInBoxZos_) {
tGoal_.stateInBoxZos_ = ((ompl::base::FOSOptimizationObjective*)opt_.get())->
boxZosDistance(motion->state) < DBL_EPSILON;
if (tGoal_.stateInBoxZos_) tGoal_.temp_ = initTemperature_;
}
//Update frontier nodes and non frontier nodes count
++tGoal_.frontierCount_;//Participates in the tree expansion
}
if (tGoal_->size() == 0) {
OMPL_ERROR("%s: Unable to sample any valid states for goal tree",
getName().c_str());
break;
}
}
//Sample random state
if (rng_.uniform01() < 0.05) {
si_->copyState(rstate,otherTree.root);
} else {
sampler_->sampleUniform(rstate);
}
//Extend tree
ExtendResult extendResult(extend(tree,tgi,rmotion));
temp += tree.temp_;
iter++;
if (extendResult != TRAPPED) {
//Remember which motion was just added
Motion *addedMotion(tgi.xmotion);
//If reached, it means we used rstate directly, no need to copy again
if (extendResult == ADVANCED) si_->copyState(rstate,tgi.xstate);
//Attempt to connect trees
otherTree.temp_ *= tempChangeFactor_;
bool connected(connect(otherTree,tgi,rmotion));
otherTree.temp_ /= tempChangeFactor_;
Motion *startMotion(startTree ? tgi.xmotion : addedMotion);
Motion *goalMotion(startTree ? addedMotion : tgi.xmotion);
//If we connected the trees in a valid way (start and goal pair is valid)
if (connected && goal->isStartGoalPairValid(startMotion->root,
goalMotion->root)) {
//It must be the case that either the start tree or the goal tree
//has made some progress so one of the parents is not NULL.
//We go one step 'back' to avoid having a duplicate state
//on the solution path
if (startMotion->parent) {
startMotion = startMotion->parent;
} else {
goalMotion = goalMotion->parent;
}
connectionPoint_ = std::make_pair(startMotion->state,goalMotion->state);
//Construct the solution path
Motion *solution(startMotion);
std::vector<Motion*> mpath1;
while (solution) {
mpath1.push_back(solution);
solution = solution->parent;
}
solution = goalMotion;
std::vector<Motion*> mpath2;
while (solution) {
mpath2.push_back(solution);
solution = solution->parent;
}
//Set the solution path
PathGeometric *path(new PathGeometric(si_));
path->getStates().reserve(mpath1.size() + mpath2.size());
for (int i(mpath1.size() - 1); i >= 0; --i) {
path->append(mpath1[i]->state);
}
for (std::size_t i(0); i < mpath2.size(); ++i) {
path->append(mpath2[i]->state);
}
pdef_->addSolutionPath(base::PathPtr(path),false,0.0);
solved = true;
break;
}
}
}
//Clean up
si_->freeState(tgi.xstate);
si_->freeState(rstate);
delete rmotion;
OMPL_INFORM("%s: Created %u states (%u start + %u goal)",getName().c_str(),
tStart_->size()+tGoal_->size(),tStart_->size(),tGoal_->size());
OMPL_INFORM("%s: Average temperature %f ",getName().c_str(),temp/double(iter));
//std::cout << extendCount << " " << connectCount << " " << iter << std::endl;
double costs(0);
std::vector<Motion*> start;
tStart_->list(start);
for (unsigned int i = 0; i < start.size(); ++i) {
if (start.at(i)->parent) {
costs += opt_->motionCost(start.at(i)->parent->state,start.at(i)->state).value()
/si_->distance(start.at(i)->parent->state,start.at(i)->state);
}
}
std::vector<Motion*> data;
tGoal_->list(data);
for (unsigned int i = 0; i < data.size(); ++i) {
if (data.at(i)->parent) {
costs += opt_->motionCost(data.at(i)->state,data.at(i)->parent->state).value()
/si_->distance(data.at(i)->parent->state,data.at(i)->state);
}
}
//std::cout << "avg cost " << costs/double(start.size()+data.size()-2) << std::endl;
return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}