// Parse a statement. // // statement: // compound-statement // return-statement // ... // declaration-statement Stmt& Parser::statement() { switch (lookahead()) { case lbrace_tok: return compound_statement(); case return_tok: return return_statement(); case var_tok: case def_tok: case struct_tok: case class_tok: case enum_tok: case namespace_tok: case template_tok: return declaration_statement(); default: return expression_statement(); } }
bool lookahead(Type& to) { return lookahead(std::addressof(to), sizeof(Type)); }
Token Parser::curTok() { return lookahead(0); }
static int could_read_(struct parser *p, const char *s, int len) { return lookahead(p, len) && memcmp(p->lookahead, s, len) == 0; }
static rsexp read_full_list (Reader* reader) { rsexp res; rsexp datum; res = R_FAILURE; if (!match (reader, TKN_LP)) goto exit; if (match (reader, TKN_RP)) { res = R_NULL; goto exit; } datum = read_datum (reader); if (r_failure_p (datum)) { syntax_error (reader, "bad syntax"); goto exit; } r_gc_scope_open (reader->r); ensure_or_goto (res = r_cons (reader->r, datum, R_NULL), clean); while (TRUE) { rtokenid id = lookahead (reader)->_id; if (id == TKN_DOT || id == TKN_RP) break; datum = read_datum (reader); if (r_failure_p (datum)) { syntax_error (reader, "bad syntax"); res = R_FAILURE; goto clean; } ensure_or_goto (res = r_cons (reader->r, datum, res), clean); } res = r_reverse_x (reader->r, res); if (match (reader, TKN_DOT)) { datum = read_datum (reader); if (r_failure_p (datum)) { syntax_error (reader, "datum expected"); res = R_FAILURE; goto clean; } res = r_append_x (reader->r, res, datum); } if (!match (reader, TKN_RP)) { syntax_error (reader, "missing close parenthesis"); res = R_FAILURE; } clean: r_gc_scope_close_and_protect (reader->r, res); exit: return res; }
void RTAStar::plan(const Eigen::VectorXd &q_start, const KDL::Frame &x_goal, std::list<Eigen::VectorXd > &path_q, MarkerPublisher &markers_pub) { path_q.clear(); V_.clear(); E_.clear(); setGoal(x_goal); std::vector<double > tmp_vec; tmp_vec.resize(transform_delta_vec_.size(), 0.0); int q_new_idx_ = 0; { RTAStarState state_start( transform_delta_vec_.size() ); state_start.h_value_ = 0.0; state_start.q_ = q_start; state_start.dq_.resize(ndof_); state_start.dq_.setZero(); state_start.parent_idx_ = -1; // state_start.T_B_E_ = KDL::Frame(KDL::Vector(q_start(0), q_start(1), q_start(2))); kin_model_->calculateFk(state_start.T_B_E_, effector_name_, q_start); V_.insert(std::make_pair(0, state_start)); } int current_node_idx = 0; while (true) { std::map<int, RTAStarState >::iterator v_it = V_.find(current_node_idx); if (v_it == V_.end()) { std::cout << "ERROR: RTAStar::plan v_it == V_.end() " << current_node_idx << std::endl; return; } if ((v_it->second.T_B_E_.p - x_goal.p).Norm() < 0.08 && v_it->second.q_.innerSize() == ndof_) { std::cout << "goal reached" << std::endl; while (true) { path_q.push_front(v_it->second.q_); current_node_idx = v_it->second.parent_idx_; if (current_node_idx < 0) { break; } v_it = V_.find(current_node_idx); if (v_it == V_.end()) { std::cout << "ERROR: v_it == V_.end() " << current_node_idx << std::endl; } } return; } // From a given current state, the neighbouring states are generated for (int tr_idx = 0; tr_idx < transform_delta_vec_.size(); tr_idx++) { // the neighbour state was not yet visited from the current node if (v_it->second.neighbour_nodes_[tr_idx] == -1) { // create a new state and add it to the graph RTAStarState new_state( transform_delta_vec_.size() ); new_state.T_B_E_ = transform_delta_vec_[tr_idx] * v_it->second.T_B_E_; // check if this neighbour state already exists int state_idx = -1;//checkIfStateExists(new_state.T_B_E_); if (state_idx == -1) { // create a new state q_new_idx_++; new_state.h_value_ = getCostH(new_state.T_B_E_); new_state.parent_idx_ = v_it->first; new_state.parent_tr_idx_ = tr_idx; v_it->second.neighbour_nodes_[tr_idx] = q_new_idx_; V_.insert(std::make_pair(q_new_idx_, new_state)); markers_pub.addSinglePointMarker(q_new_idx_, new_state.T_B_E_.p, 0, 1, 0, 1, 0.05, "world"); } else { // add existing state to neighbour list v_it->second.neighbour_nodes_[tr_idx] = state_idx; } } } // The heuristic function, augmented by lookahead search, is applied to each, // and then the cost of the edge to each neighbouring state is added to this value, // resulting in an f value for each neighbour of the current state. bool validTransitionExists = false; for (int tr_idx = 0; tr_idx < transform_delta_vec_.size(); tr_idx++) { int neighbour_idx = v_it->second.neighbour_nodes_[tr_idx]; if (neighbour_idx >= 0) { std::map<int, RTAStarState >::const_iterator n_it = V_.find(neighbour_idx); if (n_it == V_.end()) { std::cout << "ERROR: RTAStar::plan n_it == V_.end() " << neighbour_idx << std::endl; return; } // calculate the lookahead heuristics for each neighbour and add the cost from current state to neighbour double cost_h = lookahead(n_it->second.T_B_E_, 5); tmp_vec[tr_idx] = cost_h + getCostLine(v_it->second.T_B_E_, n_it->second.T_B_E_); std::cout << " cost_h " << tr_idx << " " << cost_h << std::endl; validTransitionExists = true; } else { tmp_vec[tr_idx] = 10000.0; } } if (!validTransitionExists) { // remove the current node std::cout << " no possible transition " << v_it->first << std::endl; std::map<int, RTAStarState >::iterator parent_it = V_.find(v_it->second.parent_idx_); if (parent_it == V_.end()) { std::cout << "no parent node" << std::endl; return; } parent_it->second.neighbour_nodes_[v_it->second.parent_tr_idx_] = -2; markers_pub.addSinglePointMarker(v_it->first, v_it->second.T_B_E_.p, 0, 0, 0, 1, 0.05, "world"); V_.erase(current_node_idx); current_node_idx = parent_it->first; markers_pub.publish(); ros::spinOnce(); continue; } // The node with the minimum f value is chosen for the new current state and a move to that state // is executed. At the same time, the next best f value is stored at the previous current state. // get the two smallest values double min1 = -1.0, min2 = -1.0; int min1_idx = -1; for (int tr_idx = 0; tr_idx < transform_delta_vec_.size(); tr_idx++) { if (min1 < 0 || min1 > tmp_vec[tr_idx]) { min1 = tmp_vec[tr_idx]; min1_idx = tr_idx; } } for (int tr_idx = 0; tr_idx < transform_delta_vec_.size(); tr_idx++) { if (tr_idx == min1_idx) { continue; } if (min2 < 0 || min2 > tmp_vec[tr_idx]) { min2 = tmp_vec[tr_idx]; } } std::cout << "current_node_idx " << current_node_idx << " min: " << min1 << " " << min2 << std::endl; // execute the move int next_node_idx = v_it->second.neighbour_nodes_[min1_idx]; if (next_node_idx < 0) { std::cout << "ERROR: next_node_idx < 0: " << next_node_idx << std::endl; return; } std::map<int, RTAStarState >::iterator n_it = V_.find(next_node_idx); // std::cout << n_it->second.T_B_E_.p[0] << " " << n_it->second.T_B_E_.p[1] << " " << n_it->second.T_B_E_.p[2] << std::endl; // check if the next state is possible if (!v_it->second.simulated_nodes_[min1_idx]) { // simulate n_it->second.q_.resize(ndof_); n_it->second.dq_.resize(ndof_); KDL::Frame nT_B_E; std::list<KDL::Frame > path_x; std::list<Eigen::VectorXd > path_q; KDL::Frame T_B_E(n_it->second.T_B_E_); bool col_free = collisionFree(v_it->second.q_, v_it->second.dq_, v_it->second.T_B_E_, n_it->second.T_B_E_, 0, n_it->second.q_, n_it->second.dq_, nT_B_E, &path_x, &path_q); int similar_state = checkIfStateExists(n_it->first, n_it->second); if (!col_free || similar_state >=0) { n_it->second.h_value_ = 100.0; if (!col_free) { std::cout << " invalid state change, removing node " << next_node_idx << std::endl; } else { std::cout << " joining states " << next_node_idx << " to " << similar_state << std::endl; } V_.erase(next_node_idx); v_it->second.neighbour_nodes_[min1_idx] = -2; markers_pub.addSinglePointMarker(n_it->first, n_it->second.T_B_E_.p, 0, 0, 0, 1, 0.05, "world"); markers_pub.publish(); ros::spinOnce(); continue; } v_it->second.simulated_nodes_[min1_idx] = true; } /* int similar_state = checkIfStateExists(n_it->first, n_it->second); if (similar_state >= 0) { // TODO: possible infinite loop! // join states v_it->second.neighbour_nodes_[min1_idx] = similar_state; V_.erase(next_node_idx); std::cout << "joining states: " << next_node_idx << " to " << similar_state << std::endl; next_node_idx = similar_state; } */ /* if (!col_free) {isPoseValid(n_it->second.T_B_E_)) { n_it->second.h_value_ = 10.0; markers_pub.addSinglePointMarker(n_it->first, n_it->second.T_B_E_.p, 0, 0, 0, 1, 0.05, "world"); markers_pub.publish(); ros::spinOnce(); std::cout << " invalid pose" << std::endl; continue; } */ double min_h = 10000.0; int min_idx = -1; for (std::map<int, RTAStarState >::const_iterator v2_it = V_.begin(); v2_it != V_.end(); v2_it++) { if (v2_it->second.h_value_ < min_h && v2_it->second.q_.innerSize() == ndof_) { min_h = v2_it->second.h_value_; min_idx = v2_it->first; } // markers_pub.addVectorMarker(v2_it->first+6000, v2_it->second.T_B_E_.p + KDL::Vector(0,0,0.05), v2_it->second.T_B_E_.p + KDL::Vector(0,0,0.05 + v2_it->second.h_value_*0.1), 0, 0.7, 0, 0.5, 0.01, "world"); } next_node_idx = min_idx; markers_pub.addSinglePointMarker(v_it->first, v_it->second.T_B_E_.p, 1, 0, 0, 1, 0.05, "world"); markers_pub.addSinglePointMarker(n_it->first, n_it->second.T_B_E_.p, 0, 0, 1, 1, 0.05, "world"); markers_pub.publish(); ros::spinOnce(); // getchar(); markers_pub.addSinglePointMarker(v_it->first, v_it->second.T_B_E_.p, 0, 1, 0, 1, 0.05, "world"); markers_pub.addSinglePointMarker(n_it->first, n_it->second.T_B_E_.p, 0, 1, 0, 1, 0.05, "world"); // save the second minimum value v_it->second.h_value_ = min2; std::cout << "changing state to " << next_node_idx << std::endl; // go to the next state current_node_idx = next_node_idx; // getchar(); } }
int march_solve_rec() { int branch_literal = 0, _result, _percentage_forced; int skip_left = 0, skip_right = 0, top_flag = 0; #ifdef DISTRIBUTION int record_index = current_record; top_flag = TOP_OF_TREE; if( fix_recorded_literals(record_index) == UNSAT ) return UNSAT; if( record_index == 0 ) record_index = init_new_record( ); #endif #ifdef SUPER_LINEAR if( depth < sl_depth ) subtree_size = 0; else if( subtree_size == SL_MAX ) return UNSAT; else subtree_size++; #endif #ifdef TIMEOUT if( (int) clock() > CLOCKS_PER_SEC * TIMEOUT ) return UNKNOWN; #endif #ifdef SUBTREE_SIZE path_length++; #endif #ifdef CUT_OFF if( depth <= CUT_OFF ) last_SAT_bin = -1; if( solution_bin == last_SAT_bin ) { #ifdef DISTRIBUTION records[ record_index ].UNSAT_flag = 1; #endif return UNSAT; } #endif #ifdef DETECT_COMPONENTS determine_components(); #endif #ifdef DISTRIBUTION branch_literal = records[record_index].branch_literal; if( branch_literal != 0 ) dist_acc_flag = 1; else // if( branch_literal == 0 ) #endif do { #ifdef LOCAL_AUTARKY int _depth; _depth = analyze_autarky(); if( _depth == 0 ) printf("c global autarky found at depth %i\n", depth ); else if( _depth != depth ) printf("c autarky found at depth %i (from %i)\n", depth, _depth ); #endif nodeCount++; // printf("node %i @ depth %i\n", nodeCount, depth ); if( ConstructCandidatesSet() == 0 ) { if( depth > 0 ) { if( checkSolution() == SAT ) return verifySolution(); } if( PreselectAll() == 0 ) return verifySolution(); } else ConstructPreselectedSet(); if( lookahead() == UNSAT ) { lookDead++; return UNSAT; } if( propagate_forced_literals() == UNSAT ) return UNSAT; branch_literal = get_signedBranchVariable(); // printf("c branch literal %i", branch_literal ); } while( (percentage_forced > 50.0) || (branch_literal == 0) ); #ifdef PLOT if( plotCount++ >= 10000 ) return UNSAT; printf("\t%i\t%.3f\t%i\n", plotCount, sum_plot / count_plot, depth ); #endif _percentage_forced = percentage_forced; #ifdef PARALLEL if( depth < para_depth ) if( para_bin & (1 << (para_depth - depth - 1) ) ) branch_literal *= -1; #endif // printf("branch_literal = %i at depth %i\n", branch_literal, depth ); #ifdef GLOBAL_AUTARKY if( depth == 0 ) { int i; for( i = 1; i <= nrofvars; i++ ) { TernaryImpReduction[ i ] = 0; TernaryImpReduction[ -i ] = 0; if( kSAT_flag ) { btb_size[ i ] = 0; btb_size[ -i ] = 0; } } if( kSAT_flag ) for( i = 0; i < nrofbigclauses; ++i ) clause_reduction[ i ] = 0; // branch_literal = 10; } #endif NODE_START(); #ifdef BLOCK_PRESELECT set_block_stamps( branch_literal ); #endif #ifdef DISTRIBUTION if( top_flag ) { branch_literal *= -1; current_rights++; UPDATE_BIN(); } skip_left = skip_node(); #endif if( (skip_left==0) && IFIUP( branch_literal, FIX_BRANCH_VARIABLE ) ) { #ifdef DISTRIBUTION current_record = LEFT_CHILD; #endif _result = recursive_solve(); #ifdef DISTRIBUTION LEFT_CHILD = current_record; #endif if( _result == SAT || _result == UNKNOWN ) return _result; } else { #ifdef DISTRIBUTION if( (LEFT_CHILD != 0) && records[ LEFT_CHILD ].UNSAT_flag == 0 ) { records[ LEFT_CHILD ].UNSAT_flag = 1; // printf("c left child %i UNSAT by parent!\n", LEFT_CHILD ); } #endif PUSH( r, STACK_BLOCK );} NODE_END(); #ifdef BACKJUMP if( backjump_literal != 0 ) if( timeAssignments[ backjump_literal ] >= NARY_MAX ) { // printf("backjumping at depth %i due to literal %i\n", depth, backjump_literal ); return UNSAT; } backjump_literal = 0; #endif #ifdef DISTRIBUTION if( top_flag ) { current_rights--; UPDATE_BIN(); } #endif percentage_forced = _percentage_forced; #ifdef PARALLEL if( depth < para_depth ) goto parallel_skip_node; #endif #ifdef GLOBAL_AUTARKY if( depth == 0 ) if( kSAT_flag ) { int i; for( i = 1; i <= nrofvars; ++i ) { assert( TernaryImpReduction[ i ] == 0 ); assert( TernaryImpReduction[ -i ] == 0 ); assert( btb_size[ i ] == 0 ); assert( btb_size[ -i ] == 0 ); } for( i = 0; i < nrofbigclauses; ++i ) { clause_red_depth[ i ] = nrofvars; clause_SAT_flag[ i ] = 0; } } if( kSAT_flag ) { int i; for( i = 1; i <= nrofvars; ++i ) { assert( TernaryImpReduction[ i ] >= 0 ); assert( TernaryImpReduction[ -i ] >= 0 ); assert( btb_size[ i ] >= 0 ); assert( btb_size[ -i ] >= 0 ); } } #endif NODE_START(); #ifdef BLOCK_PRESELECT set_block_stamps( branch_literal ); #endif if( top_flag == 0 ) { #ifdef DISTRIBUTION current_rights++; #endif UPDATE_BIN(); } #ifdef DISTRIBUTION skip_right = skip_node(); #endif if( (skip_right == 0) && IFIUP( -branch_literal, FIX_BRANCH_VARIABLE ) ) { #ifdef DISTRIBUTION current_record = RIGHT_CHILD; #endif _result = recursive_solve(); #ifdef DISTRIBUTION RIGHT_CHILD = current_record; #endif if( _result == SAT || _result == UNKNOWN ) return _result;} else { #ifdef DISTRIBUTION if( (RIGHT_CHILD != 0) && records[ RIGHT_CHILD ].UNSAT_flag == 0 ) { records[ RIGHT_CHILD ].UNSAT_flag = 1; // printf("c right child %i UNSAT by parent!\n", RIGHT_CHILD ); } #endif PUSH( r, STACK_BLOCK );} NODE_END(); if( top_flag == 0 ) { #ifdef DISTRIBUTION current_rights--; #endif UPDATE_BIN(); } #ifdef PARALLEL parallel_skip_node:; #endif // printf("ended node %i at depth %i\n", nodeCount, depth ); #ifdef SUBTREE_SIZE if( (skip_flag == 0) && (jump_depth == 0) && (current_rights == 0) ) { int subtree = path_length - depth; // float cost = nodeCount * (CLOCKS_PER_SEC / ((float)(clock() + 10 - first_time))); // printf("c nodes per second = %.3f at level %i\n", cost, depth ); if( jump_depth >= 30 ) jump_depth = 999; if( subtree > SUBTREE_SIZE ) { jump_depth = depth; while( subtree > 2*SUBTREE_SIZE ) { jump_depth++; subtree = subtree / 2; } if( jump_depth >= 20 ) jump_depth = 999; skip_flag = 1; } } #endif #ifdef DISTRIBUTION record_node( record_index, branch_literal, skip_left, skip_right ); current_record = record_index; #endif #ifdef BACKJUMP if( kSAT_flag ) { int *_rstackp = rstackp, nrval; while( --_rstackp > rstack ) { nrval = *_rstackp; if( (TernaryImpReduction[ nrval ] + TernaryImpReduction[ -nrval ]) != 0 ) { backjump_literal = nrval; break; } } } #endif #ifdef ADD_CONFLICT printConflict(); #endif return UNSAT; }
// Consume up to but not including the given token kind. void Parser::consume_until(Token_kind k) { while (!ts_.eof() && lookahead() != k) ts_.get(); }
Factor_const_num::Factor_const_num() { value = strtod(lookahead(0).str.c_str(), 0); eat(tok_const_num); };
int get_child(void* p, int level, int b) { return lookahead(p, spec.get_child(p, level, b)); }
int get_root(void* p) { return lookahead(p, spec.get_root(p)); }
int64 ByteArray::CByteArray::lookahead(void* const dst, const int64 size) const { return lookahead(dst, m_pos, size); }
static rsexp read_compound_datum (Reader* reader) { return TKN_HASH_LP == lookahead (reader)->_id ? read_vector (reader) : read_list (reader); }
static rsexp read_list (Reader* reader) { return TKN_LP == lookahead (reader)->_id ? read_full_list (reader) : read_abbreviation (reader); }
struct listnode* processoperators(struct listnode *newnode,int c) { int expect[10],ifyes[10],code; newnode->token.tokentype=OPERATOR; switch(c) { case '+' : expect[0]='+'; ifyes[0]=PLUSPLUS; expect[1]='=' ; ifyes[1]=PLUSEQ; code=lookahead(expect,ifyes,2); if(code) newnode->token.Ctoken.operator->code=code; else newnode->token.Ctoken.operator->code=c; return newnode; case '-' : expect[0]='-'; ifyes[0]=MINUSMINUS; expect[1]='='; ifyes[1]=MINUSEQ; expect[2]='>'; ifyes[2]=ARROW; code=lookahead(expect,ifyes,3); if(code) newnode->token.Ctoken.operator->code=code; else newnode->token.Ctoken.operator->code=c; return newnode; case '*' : expect[0]='='; ifyes[0]=STAREQ; if( (code=lookahead(expect,ifyes,1)) != 0 ) newnode->token.Ctoken.operator->code=code; else newnode->token.Ctoken.operator->code=c; return newnode; case '/': newnode->token.Ctoken.operator->code=c; expect[0]='='; ifyes[0]=SLASHEQ; if( (code=lookahead(expect,ifyes,1)) != 0 ) newnode->token.Ctoken.operator->code=code; return newnode; case '^' : newnode->token.Ctoken.operator->code=c; expect[0]='='; ifyes[0]=EXOREQ; if( (code=lookahead(expect,ifyes,1)) != 0 ) newnode->token.Ctoken.operator->code=code; return newnode; case '%': newnode->token.Ctoken.operator->code=c; expect[0]='='; ifyes[0]=MODEQ; if( (code=lookahead(expect,ifyes,1)) != 0 ) newnode->token.Ctoken.operator->code=code; return newnode; case '=': newnode->token.Ctoken.operator->code=c; expect[0]='='; ifyes[0]=EQ; if( (code=lookahead(expect,ifyes,1)) != 0 ) newnode->token.Ctoken.operator->code=code; return newnode; case '!': newnode->token.Ctoken.operator->code=c; expect[0]='='; ifyes[0]=NE; if( (code=lookahead(expect,ifyes,1)) != 0 ) newnode->token.Ctoken.operator->code=code; return newnode; case '|': newnode->token.Ctoken.operator->code=c; expect[0]='|'; ifyes[0]=OR; expect[1]='='; ifyes[1]=OREQ; if( (code=lookahead(expect,ifyes,2)) != 0 ) newnode->token.Ctoken.operator->code=code; return newnode; case '&': newnode->token.Ctoken.operator->code=c; expect[0]='&'; ifyes[0]=AND; expect[1]='='; ifyes[1]=ANDEQ; if( (code=lookahead(expect,ifyes,2)) != 0 ) newnode->token.Ctoken.operator->code=code; return newnode; case '<': newnode->token.Ctoken.operator->code=c; expect[0]='='; ifyes[0]=LE; expect[1]='<'; ifyes[1]=LS; if( (code=lookahead(expect,ifyes,2)) != 0 ) newnode->token.Ctoken.operator->code=code; if(code==LS) { expect[0]='='; ifyes[0]=LSE; if( (code=lookahead(expect,ifyes,1))!=0 ) newnode->token.Ctoken.operator->code=code; } return newnode; case '>': newnode->token.Ctoken.operator->code=c; expect[0]='='; ifyes[0]=GE; expect[1]='>'; ifyes[1]=RS; if( (code=lookahead(expect,ifyes,2)) != 0 ) newnode->token.Ctoken.operator->code=code; if(code==RS) { expect[0]='='; ifyes[0]=RSE; if( (code=lookahead(expect,ifyes,1))!=0 ) newnode->token.Ctoken.operator->code=code; } return newnode; case '?' : newnode->token.Ctoken.operator->code=c; if( (code=trigraph()) != 0 ) return processoperators(newnode,c); /*recursion*/ /*else return '?' to stream*/ ungetc(c,yyin); return newnode; default: newnode->token.Ctoken.operator->code=c; return newnode; }/*end of switch*/ }/*End of process operators*/
/****************************************************************** ini_scan() Called by scan(). *******************************************************************/ static void ini_scan() { lookahead();/* move to next non blank */ Rbuffptr = Read_buffer; }
Factor_const_str::Factor_const_str() { value = lookahead(0).str; eat(tok_const_str); }
// Require a token of the given kind. Behavior is // udefined if the token does not match. Token Parser::require(Token_kind k) { assert(lookahead() == k); return ts_.get(); }
Factor_var::Factor_var() { name = lookahead(0).str; eat(tok_identifier); };
double RTAStar::lookahead(const KDL::Frame &T_B_E, int depth) const { int state_idx = -1;//checkIfStateExists(T_B_E); double min_cost = -1; if (state_idx == -1) { if (depth == 1) { for (int tr_idx = 0; tr_idx < transform_delta_vec_.size(); tr_idx++) { double costF = getCostLine(KDL::Frame(), transform_delta_vec_[tr_idx]) + getCostH(transform_delta_vec_[tr_idx] * T_B_E); // std::cout << "RTAStar::lookahead " << costF << " " << getCostLine(KDL::Frame(), transform_delta_vec_[tr_idx]) << " " << getCostH(transform_delta_vec_[tr_idx] * T_B_E) << std::endl; if (min_cost < 0 || costF < min_cost) { min_cost = costF; } } } else { for (int tr_idx = 0; tr_idx < transform_delta_vec_.size(); tr_idx++) { double costF = getCostLine(KDL::Frame(), transform_delta_vec_[tr_idx]) + lookahead(transform_delta_vec_[tr_idx] * T_B_E, depth - 1); if (min_cost < 0 || costF < min_cost) { min_cost = costF; } } } } else { std::map<int, RTAStarState >::const_iterator v_it = V_.find(state_idx); min_cost = getCostLine(T_B_E, v_it->second.T_B_E_) + v_it->second.h_value_; } return min_cost; }
/** * Construct an AVT by parsing the string, and either * constructing a vector of AVTParts, or simply hold * on to the string if the AVT is simple. */ AVT::AVT( StylesheetConstructionContext& constructionContext, const LocatorType* locator, const XalanDOMChar* name, const XalanDOMChar* stringedValue, const PrefixResolver& resolver) : m_parts(0), m_partsSize(0), m_simpleString(0), m_simpleStringLength(0), m_name(constructionContext.getPooledString(name)) { StringTokenizer tokenizer(stringedValue, theTokenDelimiterCharacters, true); const StringTokenizer::size_type nTokens = tokenizer.countTokens(); if(nTokens < 2) { // Do the simple thing m_simpleStringLength = length(stringedValue); m_simpleString = constructionContext.allocateXalanDOMCharVector(stringedValue, m_simpleStringLength, false); } else { // This over-allocates, but we probably won't waste that much space. If necessary, // we could tokenize twice, just counting the numbers of AVTPart instances we // will need the first time. m_parts = constructionContext.allocateAVTPartPointerVector(nTokens + 1); XalanDOMString buffer(constructionContext.getMemoryManager()); XalanDOMString exprBuffer(constructionContext.getMemoryManager()); XalanDOMString t(constructionContext.getMemoryManager()); // base token XalanDOMString lookahead(constructionContext.getMemoryManager()); // next token while(tokenizer.hasMoreTokens()) { if(length(lookahead)) { t = lookahead; clear(lookahead); } else { nextToken(constructionContext, locator, tokenizer, t); } if(length(t) == 1) { const XalanDOMChar theChar = charAt(t, 0); switch(theChar) { case(XalanUnicode::charLeftCurlyBracket): { // Attribute Value Template start nextToken(constructionContext, locator, tokenizer, lookahead); if(equals(lookahead, theLeftCurlyBracketString)) { // Double braces mean escape to show brace append(buffer, lookahead); clear(lookahead); break; // from switch } else { if(length(buffer) > 0) { assert(m_partsSize + 1 < nTokens); m_parts[m_partsSize++] = constructionContext.createAVTPart( c_wstr(buffer), length(buffer)); clear(buffer); } clear(exprBuffer); while(length(lookahead) > 0 && !equals(lookahead, theRightCurlyBracketString)) { if(length(lookahead) == 1) { switch(charAt(lookahead, 0)) { case XalanUnicode::charApostrophe: case XalanUnicode::charQuoteMark: { // String start append(exprBuffer, lookahead); const XalanDOMChar quote[2] = { lookahead[0], 0 }; // Consume stuff 'till next quote nextToken(constructionContext, locator, tokenizer, lookahead); while(!equals(lookahead, quote)) { append(exprBuffer, lookahead); nextToken(constructionContext, locator, tokenizer, lookahead); } append(exprBuffer,lookahead); break; } case XalanUnicode::charLeftCurlyBracket: { GetCachedString theGuard(constructionContext); // What's another brace doing here? constructionContext.error( XalanMessageLoader::getMessage( theGuard.get(), XalanMessages::LeftBraceCannotAppearWithinExpression), 0, locator); break; } default: // part of the template stuff, just add it. append(exprBuffer, lookahead); break; } // end inner switch } // end if lookahead length == 1 else { // part of the template stuff, just add it. append(exprBuffer,lookahead); } nextToken(constructionContext, locator, tokenizer, lookahead); } // end while(!equals(lookahead, "}")) assert(equals(lookahead, theRightCurlyBracketString)); // Proper close of attribute template. Evaluate the // expression. clear(buffer); assert(m_partsSize + 1 < nTokens); m_parts[m_partsSize++] = constructionContext.createAVTPart( locator, c_wstr(exprBuffer), length(exprBuffer), resolver); clear(lookahead); // breaks out of inner while loop } break; } case(XalanUnicode::charRightCurlyBracket): { nextToken(constructionContext, locator, tokenizer, lookahead); if(equals(lookahead, theRightCurlyBracketString)) { // Double brace mean escape to show brace append(buffer, lookahead); clear(lookahead); // swallow } else { GetCachedString theGuard(constructionContext); constructionContext.error( XalanMessageLoader::getMessage( theGuard.get(), XalanMessages::UnmatchedWasFound), 0, locator); } break; } default: { // Anything else just add to string. append(buffer, theChar); } } // end switch t } // end if length == 1 else { // Anything else just add to string. append(buffer,t); } } // end while(tokenizer.hasMoreTokens()) if(length(buffer) > 0) { assert(m_partsSize + 1 < nTokens); m_parts[m_partsSize++] = constructionContext.createAVTPart(c_wstr(buffer), length(buffer)); clear(buffer); } } // end else nTokens > 1 }
::std::unique_ptr<TokenStream> expand(const Span& sp, const ::AST::Crate& crate, const ::std::string& ident, const TokenTree& tt, AST::Module& mod) override { Token tok; auto lex = TTStream(sp, tt); if( ident != "" ) ERROR(sp, E0000, "asm! doesn't take an ident"); auto template_text = get_string(sp, lex, crate, mod); ::std::vector<::AST::ExprNode_Asm::ValRef> outputs; ::std::vector<::AST::ExprNode_Asm::ValRef> inputs; ::std::vector<::std::string> clobbers; ::std::vector<::std::string> flags; // Outputs if( lex.lookahead(0) == TOK_DOUBLE_COLON ) { GET_TOK(tok, lex); lex.putback(Token(TOK_COLON)); } else if( lex.lookahead(0) == TOK_COLON ) { GET_TOK(tok, lex); while( lex.lookahead(0) == TOK_STRING ) { //auto name = get_string(sp, lex); GET_CHECK_TOK(tok, lex, TOK_STRING); auto name = mv$(tok.str()); GET_CHECK_TOK(tok, lex, TOK_PAREN_OPEN); auto val = Parse_Expr0(lex); GET_CHECK_TOK(tok, lex, TOK_PAREN_CLOSE); outputs.push_back( ::AST::ExprNode_Asm::ValRef { mv$(name), mv$(val) } ); if( lex.lookahead(0) != TOK_COMMA ) break; GET_TOK(tok, lex); } } else { } // Inputs if( lex.lookahead(0) == TOK_DOUBLE_COLON ) { GET_TOK(tok, lex); lex.putback(Token(TOK_COLON)); } else if( lex.lookahead(0) == TOK_COLON ) { GET_TOK(tok, lex); while( lex.lookahead(0) == TOK_STRING ) { GET_CHECK_TOK(tok, lex, TOK_STRING); auto name = mv$(tok.str()); GET_CHECK_TOK(tok, lex, TOK_PAREN_OPEN); auto val = Parse_Expr0(lex); GET_CHECK_TOK(tok, lex, TOK_PAREN_CLOSE); inputs.push_back( ::AST::ExprNode_Asm::ValRef { mv$(name), mv$(val) } ); if( lex.lookahead(0) != TOK_COMMA ) break; GET_TOK(tok, lex); } } else { } // Clobbers if( lex.lookahead(0) == TOK_DOUBLE_COLON ) { GET_TOK(tok, lex); lex.putback(Token(TOK_COLON)); } else if( lex.lookahead(0) == TOK_COLON ) { GET_TOK(tok, lex); while( lex.lookahead(0) == TOK_STRING ) { GET_CHECK_TOK(tok, lex, TOK_STRING); clobbers.push_back( mv$(tok.str()) ); if( lex.lookahead(0) != TOK_COMMA ) break; GET_TOK(tok, lex); } } else { } // Flags if( lex.lookahead(0) == TOK_DOUBLE_COLON ) { GET_TOK(tok, lex); lex.putback(Token(TOK_COLON)); } else if( lex.lookahead(0) == TOK_COLON ) { GET_TOK(tok, lex); while( lex.lookahead(0) == TOK_STRING ) { GET_CHECK_TOK(tok, lex, TOK_STRING); flags.push_back( mv$(tok.str()) ); if( lex.lookahead(0) != TOK_COMMA ) break; GET_TOK(tok, lex); } } else { } // trailing `: voltaile` - TODO: Is this valid? if( lex.lookahead(0) == TOK_DOUBLE_COLON ) { GET_TOK(tok, lex); lex.putback(Token(TOK_COLON)); } else if( lex.lookahead(0) == TOK_COLON ) { GET_TOK(tok, lex); if( GET_TOK(tok, lex) == TOK_IDENT && tok.str() == "volatile" ) { flags.push_back( "volatile" ); } else { PUTBACK(tok, lex); } } else { } // has to be the end if( lex.lookahead(0) != TOK_EOF ) { ERROR(sp, E0000, "Unexpected token in asm! - " << lex.getToken()); } // Convert this into an AST node and insert as an intepolated expression ::AST::ExprNodeP rv = ::AST::ExprNodeP( new ::AST::ExprNode_Asm { mv$(template_text), mv$(outputs), mv$(inputs), mv$(clobbers), mv$(flags) } ); return box$( TTStreamO(sp, TokenTree(Token( InterpolatedFragment(InterpolatedFragment::EXPR, rv.release()) )))); }
TOK lalex() { static nocast = 0; // prevent ID CAST, FOR CAST, etc. //static in_op = 0; // prevent OPERATOR op CAST static incast = 0; // don't scan in already recognized cast static in_enum; static fr; char en = 0; switch (la_start()) { case VOLATILE: case SIGNED: error('w',"keyword%k (ignored)",latok->tok); return lalex(); case ENUM: en = 1; case AGGR: switch (tk) { /* strictly speaking these ought to be included, but they only cause grief case ELSE: case COLON: // label case RP: // end of condition */ case 0: case LP: // argument list case CM: case NEW: case CAST: case RP: // yok, old C: f(i) struct s * i; {} case OPERATOR: case DO: case TYPE: // might be "const" case COLON: // label case SM: // end of statement case RC: // end of statement case LC: // first statement break; default: error(&curloc,"';' missing afterS orD before\"%k\"",latok->tok); return tk=SM; } { TOK t = lookahead(); TOK x; switch (t) { case TNAME: x = lookahead(); break; case ID: // hidden or undefined x = lookahead(); backup(); switch (x) { case LC: in_enum = en; case COLON: // defining: return AGGR ID backup(); fr = 0; DO_RET; default: { Pname n = ktbl->look(latok->retval.s,HIDDEN); if (n == 0) { // new tag: define it n = new name(latok->retval.s); n->lex_level = 0; n = n->tname(latok->last->retval.t); modified_tn = modified_tn->l; } else { switch (n->tp->base) { case COBJ: case EOBJ: break; default: error('i',"hidden%n:%t",n,n->tp); } } latok->tok = TNAME; latok->retval.pn = n; } } (void)lookahead(); break; case LC: in_enum = en; default: fr = 0; DO_RET; }; switch (x) { case LC: // class x { in_enum = en; case COLON: // class x : fr = 0; DO_RET; case SM: if (tk!=NEW && fr==0) { // no further processing necessary deltok(); // class deltok(); // x deltok(); // ; return lalex(); } // new class x ; => new x ; default: deltok(); // AGGR(?) TNAME(x) => TNAME(x) fr = 0; DO_RET; } } case LP: fr = 0; if (nocast) { nocast = 0; DO_RET; // } else if (in_op) { // in_op = 0; // DO_RET; } else if (incast) DO_RET; /* possible cast */ bad_cast = 0; if (scan_type()) { if (scan_mod()) { if (lookahead() != RP) DO_RET; switch (lookahead()) { case CM: case RP: case SM: case LC: case ASSIGN: /* arg type list in declaration */ if (tk != SIZEOF) DO_RET; break; case PLUS: case MINUS: case MUL: case AND: case NEW: case DELETE: case SIZEOF: case MEM: case NOT: case COMPL: case ICOP: case LP: case CAST: case ID: case TYPE: case TNAME: case THIS: case OPERATOR: case ZERO: case ICON: case FCON: case CCON: case STRING: // cast of a term break; default: // something wrong... // binary op, missing ;, etc. // "bad cast" could be legal expr // "( TNAME() )" (ctor call) if (bad_cast) DO_RET; else break; } backup(); front->tok = CAST; latok->tok = ENDCAST; if (bad_cast) { error("can't cast to function"); rep_cast(); } incast = 1; } } DO_RET; case CAST: incast++; DO_RET; case ENDCAST: if (--incast == 0) nocast = 0; DO_RET; case ID: { char* s = front->retval.s; fr = 0; nocast = 1; switch (lookahead()) { case ID: { // handle ID ID // assume ID is a missing, hidden, or misspelt TNAME char* s2 = latok->retval.s; backup(); Pname n = ktbl->look(s,HIDDEN); if (n == 0) { // new tag: define it error("%s %s:TX (%s is not a TN)",s,s2,s); n = new name(s); n->lex_level = 0; n = n->tname(0); modified_tn = modified_tn->l; n->tp = any_type; } else { error("%s %s: %s is hidden",s,s2,s); } latok->tok = TNAME; latok->retval.pn = n; break; } case LC: backup(); front->retval.pn = new name(s); front->retval.pn->lex_level--; break; default: backup(); front->retval.pn = new name(s); } DO_RET; } case CASE: case DEFAULT: case PUBLIC: case ELSE: fr = 0; switch (tk) { case COLON: // label case SM: // end of statement case RC: // end of statement case LC: // first statement DO_RET; default: error(&curloc,"';' missing afterS orD before\"%k\"",latok->tok); return tk=SM; } case DO: case GOTO: case CONTINUE: case BREAK: case RETURN: fr = 0; switch (tk) { case ELSE: case DO: case COLON: // label case RP: // end of condition case SM: // end of statement case RC: // end of statement case LC: // first statement DO_RET; default: error(&curloc,"';' missing afterS orD before\"%k\"",latok->tok); return tk=SM; } case IF: case WHILE: case FOR: case SWITCH: fr = 0; switch (tk) { case ELSE: case DO: case COLON: // label case RP: // end of condition case SM: // end of statement case RC: // end of statement case LC: // first statement nocast = 1; DO_RET; default: error(&curloc,"';' missing afterS orD before\"%k\"",latok->tok); return tk=SM; } case TYPE: // dangerous to diddle with: constructor notation fr = 0; switch (tk) { case ID: // case RP: old C function definition case RB: error(&curloc,"';' missing afterS orD before\"%k\"",latok->tok); return tk=SM; } if (latok->retval.t == FRIEND) fr = 1; nocast = 1; DO_RET; case TNAME: // dangerous to diddle with: name hiding { Pname n = latok->retval.pn; if (fr) { // guard against: TYPE(friend) TNAME(x) SM nocast = 1; fr = 0; DO_RET; } fr = 0; // fprintf(stderr,"type or tname %d %s\n",tk,n->string); fflush(stderr); switch (tk) { case TYPE: // int TN ? or unsigned TN ? // beware of unsigned etc. switch (lookahead()) { case SM: case RB: case COLON: case ASSIGN: goto hid; // case LP: // the real problem default: nocast = 1; DO_RET; } case TNAME: // TN TN ? switch (lookahead()) { case MEM: // cl_name::mem_name case DOT: // anachronism: cl_name.mem_name nocast = 1; DO_RET; } hid: backup(); // undo lookahead after TNAME n->hide(); n = new name(n->string); n->n_oper = TNAME; latok->tok = ID; latok->retval.pn = n; } } case NEW: fr = 0; nocast = 1; DO_RET; /* case OPERATOR: switch (lookahead()) { case LP: in_op = 1; if (lookahead() != RP) error("bad operator"); break; case LB: if (lookahead() != RB) error("bad operator"); break; case TYPE: case TNAME: while (lookahead() == MUL) ; backup(); // default : 'regular' operator } if (lookahead() == LP) in_op = 1; DO_RET; */ case RC: fr = 0; switch (tk) { case RC: // nested } (either stmt or expr) case LC: // empty block: {} case SM: // end of statement break; default: { TOK t; loc x = curloc; switch (t = lookahead()) { case ELSE: case RC: // } } probably end of initializer case CM: // } , definitely end of initializer case SM: // } ; probably end of initializer or class case RP: // int f( struct { ... } ); break; default: // either "= { ... E } SorD" // or " SorD }" // or enum { a, b } c; - yuk if (in_enum == 0) { error(&x,"';'X at end ofS orD before '}'"); return tk=SM; } in_enum = 0; } } } in_enum = 0; default: fr = 0; nocast = 0; DO_RET; } ret: // hand optimized return: //TOK deltok() //{ toknode* T = front; tk = T->tok; yylval = T->retval; if (front = front->next) front->last = 0; delete T; return tk; //} }