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