Esempio n. 1
0
void display()
{
	glDrawPixels(windowWidth,windowHeight,GL_RGB,GL_FLOAT,image.data());

	if(t<=1.0)
	{
		glBegin(GL_LINES);
			for(unsigned int c = 0 ; c < vertices.size() ; ++c)
			{
				glVertex2fv(vertices[c].v1);
				glVertex2fv(vertices[c].v1 + t*directions[c].v1);
				glVertex2fv(vertices[c].v2);
				glVertex2fv(vertices[c].v2 + t*directions[c].v2);
				glVertex2fv(vertices[c].v3);
				glVertex2fv(vertices[c].v3 + t*directions[c].v3);
			} // end for
		glEnd();
		t += 0.030f;
	}
	else
	{
		t = 0.0;
		glBegin(GL_TRIANGLES);
			for(unsigned int c = 0 ; c < vertices.size() ; ++c)
			{
				glVertex2fv(vertices[c].v1);
				glVertex2fv(vertices[c].v2);
				glVertex2fv(vertices[c].v3);
			} // end for
		glEnd();
		vertices.clear();
		directions.clear();

		// start new level
		while(nCurrentLevel--)	// foreach triangle in this level
		{
			// calculate center triangle's vertices
			Triangle &t = triangles.front();	// does not remove the head element of the queue
			Vector2D<float> c1 = (t.v1+t.v2)/2;
			Vector2D<float> c2 = (t.v2+t.v3)/2;
			Vector2D<float> c3 = (t.v3+t.v1)/2;

			// register center triangle
			vertices.push_back(Triangle(c1,c2,c3));
			directions.push_back(Triangle(c2-c1,c3-c2,c1-c3));

			// put smaller triangles in the queue
			triangles.push_back(Triangle(t.v1,c1,c3));
			triangles.push_back(Triangle(c1,t.v2,c2));
			triangles.push_back(Triangle(c3,c2,t.v3));

			// discard used triangle
			triangles.pop_front();

			nNextLevel += 3;
		} // end while
		nCurrentLevel = nNextLevel;
		nNextLevel = 0;

		++level;
		glReadPixels(0,0,windowWidth,windowHeight,GL_RGB,GL_FLOAT,image.data());
		if(level > levels)
			glutDisplayFunc(simpleDisplay);
	} // end else

	glutSwapBuffers();
} // end function display
Esempio n. 2
0
void PriorityNoPre(){
	out << alg.name << std::endl;

	int timer = 0;

	//who is ready at 0 seconds, add to priority
	while(!master.empty()){
		struct Qu t = master.front();
		if(t.arrival_time == 0) priority.push_back(t);	
		else	waiting.push(t);
		master.pop();
	}

	while(completed.size() != alg.number_of_proc){
		int flag_inner = 0;
		
		//sort lowest cpu first
		int min_cpu;
		while(!priority.empty()){
			flag_inner = 1;
			if(priority.size() == 1){
				ready.push(priority.front());
				priority.pop_front();
			}
			else{
				//if priority is bigger than 1, then need to sort
				int s = priority.size();
				struct Qu t[s];
				int i = 0;
				while(!priority.empty()){
					t[i] = priority.front();
					++i;
					priority.pop_front();
				}
				//sort in array, then add back to prioroity
				int j;
				for(j=0; j<s; ++j){
					int k;
					for(k=1; k<s; ++k){
						if(t[j].priority>t[k].priority && j<k){
							struct Qu u = t[j];
							t[j] = t[k];
							t[k] = u;
							
						} } }
				for(j=0; j<s; ++j) ready.push(t[j]);
			}	
		}
		//now in ready, run all, set var, timer
		while(!ready.empty()){	
			flag_inner = 1;
			ready.front().time_start = timer;
			out << timer << "\t" << ready.front().proc_num << std::endl;

			while(ready.front().cpu_burst != 0){
				--ready.front().cpu_burst;
				++timer;
			}
			ready_to_completed(timer);	
		}

		//increment timer
		if(flag_inner == 1) flag_inner = 0;
		else	++timer;

		int i;
		for(i=0; i<waiting.size(); ++i){
			if(waiting.front().arrival_time <= timer){
				priority.push_back(waiting.front());
				waiting.pop();
				--i;
			} }
	}

	//calculate waiting time, sum of all starts
	double total_w = 0;
	while(!completed.empty()){
		total_w += completed.front().time_start;
		completed.pop();
	}
	total_w /= alg.number_of_proc;
	out << std::setprecision(3)  << "AVG Waiting Time: " << total_w << std::endl;
}
Esempio n. 3
0
static wint_t lookahead_pop(void)
{
    wint_t result = lookahead_list.front();
    lookahead_list.pop_front();
    return result;
}
Esempio n. 4
0
int main(int argc, char *argv[])
{
   int provided = MPI_THREAD_SINGLE;
   MPI_Init_thread(&argc, &argv, MPI_THREAD_MULTIPLE, &provided);
   if (provided != MPI_THREAD_MULTIPLE) {
      std::cerr << "insufficient thread support in MPI: MPI_THREAD_MULTIPLE is required (maybe set MPICH_MAX_THREAD_SAFETY=multiple?)" << std::endl;
      exit(1);
   }
   int rank = -1;
   MPI_Comm_rank(MPI_COMM_WORLD, &rank);

   std::unique_ptr<std::thread> hubthread;

   bool fromVistle = false;
   if (argc > 1) {
       std::string arg1(argv[1]);
       fromVistle = arg1=="-from-vistle";
   }

   std::vector<std::string> args;
   args.push_back(argv[0]);
   if (fromVistle) {
       // skip -from-vistle
       for (int c=2; c<argc; ++c) {
           args.push_back(argv[c]);
       }
   }

#ifdef MODULE_THREAD
   std::string rank0;
   unsigned short port=0, dataPort=0;
   if (!fromVistle && rank == 0) {
       std::cerr << "not called from vistle, creating hub in a thread" << std::endl;
       auto hub = new Hub(true);
       hub->init(argc, argv);
       port = hub->port();
       dataPort = hub->dataPort();
       rank0 = hostname();
       hubthread.reset(new std::thread([hub, argc, argv](){
           hub->run();
           std::cerr << "Hub exited..." << std::endl;
           delete hub;
       }));

   }

   if (!fromVistle) {
       boost::mpi::broadcast(boost::mpi::communicator(), rank0, 0);
       boost::mpi::broadcast(boost::mpi::communicator(), port, 0);
       boost::mpi::broadcast(boost::mpi::communicator(), dataPort, 0);

       args.push_back(rank0);
       args.push_back(std::to_string(port));
       args.push_back(std::to_string(dataPort));
   }
#else
   if (!fromVistle) {
      std::cerr << "should be called from vistle, expecting 1st argument to be -from-vistle" << std::endl;
      exit(1);
   }
#endif

#ifdef COVER_ON_MAINTHREAD
#if defined(HAVE_QT) && defined(MODULE_THREAD)
   if (!qApp) {
       std::cerr << "early creation of QApplication object" << std::endl;
#if defined(Q_OS_UNIX) && !defined(Q_OS_MAC)
       if (xcb_connection_t *xconn = xcb_connect(nullptr, nullptr))
       {
           if (!xcb_connection_has_error(xconn)) {
               std::cerr << "X11 connection!" << std::endl;
               IceSetIOErrorHandler(&iceIOErrorHandler);
               auto app = new QApplication(argc, argv);
           }
           xcb_disconnect(xconn);
       }
#else
       auto app = new QApplication(argc, argv);
#endif
       if (qApp) {
           qApp->setAttribute(Qt::AA_MacDontSwapCtrlAndMeta);
           QIcon icon(":/icons/vistle.png");
           qApp->setWindowIcon(icon);
       }
   }
#endif
   auto t = std::thread([args](){
#endif

   try {
      vistle::registerTypes();
      std::vector<char *> argv;
      for (auto &a: args) {
          argv.push_back(const_cast<char *>(a.data()));
          std::cerr << "arg: " << a << std::endl;
      }
      Vistle(argv.size(), argv.data()).run();
   } catch(vistle::exception &e) {

      std::cerr << "fatal exception: " << e.what() << std::endl << e.where() << std::endl;
      exit(1);
   } catch(std::exception &e) {

      std::cerr << "fatal exception: " << e.what() << std::endl;
      exit(1);
   }

#ifdef COVER_ON_MAINTHREAD
   std::unique_lock<std::mutex> lock(main_thread_mutex);
   main_done = true;
   main_thread_cv.notify_all();
   });


   for (;;) {
      std::unique_lock<std::mutex> lock(main_thread_mutex);
      main_thread_cv.wait(lock, []{ return main_done || !main_func.empty(); });
      if (main_done) {
         //main_thread_cv.notify_all();
         break;
      }

      std::function<void()> f;
      if (!main_func.empty()) {
         std::cerr << "executing on main thread..." << std::endl;
         f = main_func.front();
         if (f)
            f();
         main_func.pop_front();
      }
      lock.unlock();
      main_thread_cv.notify_all();
   }

   t.join();
#endif

   MPI_Finalize();
   return 0;
}
Esempio n. 5
0
static void requeue(std::deque<int> &trg, std::deque<int> &src)
{
  trg.push_back(src.front());
  src.pop_front();
}
Esempio n. 6
0
 void clean(int ts) {
   int i = ts - 300;
   while (m_cnter.size() && m_cnter.front() <= i) {
     m_cnter.pop_front();
   }
 }
Esempio n. 7
0
void JoystickHistoryView::Update(const InputState &input_state) {
	locations_.push_back(Location(curX_, curY_));
	if ((int)locations_.size() > maxCount_) {
		locations_.pop_front();
	}
}
    void GenerateFaceIndicators(const int &sampleIdx,
                                std::vector<double> &eyesHeightVec, 
                                FaceFeature &faceFeatures,
                                std::deque<InfoPERCLOS> &PERCLOSDeque, 
                                std::deque<InfoBLINK> &BLINKDeque, 
                                const double &intervalTime)
    {
        double eyesHeight = 0, lastEyesHeight = faceFeatures.lastHeight;
        double PERCLOS = 0, MICROSLEEP = 0, MICROSLEEPTime = faceFeatures.MICROSLEEPTime, BLINK = 0;
        
        double eyesHeight_b = faceFeatures.Height_Baseline;
        //Find the min size between two eyes.
        if (!eyesHeightVec.empty()) {
            sort(eyesHeightVec.begin(), eyesHeightVec.end(), SortBigger);
            eyesHeight = eyesHeightVec.front();
            if(eyesHeight > 2*eyesHeight_b )
                eyesHeight = 2*eyesHeight_b;
        }
        else {
            //Negative, if can NOT detect eyes correctly
            eyesHeight = lastEyesHeight;
        }

/*********************************************/
/*  PERCLOS: Percentage of eye closure       */
/*********************************************/   
        InfoPERCLOS perclosInfo;
        perclosInfo.time = intervalTime;
        if(!PERCLOSDeque.empty()){
            /// Calculate the number of frames in fixed time
            if (eyesHeight < faceFeatures.Height_Baseline * THRESHOLD_PERCLOS) {
                perclosInfo.eyePERCLOS = 1; // eye closed
                perclosInfo.timePERCLOS = PERCLOSDeque.back().timePERCLOS +intervalTime;
            }
            else {
                perclosInfo.eyePERCLOS = 0;
                perclosInfo.timePERCLOS = PERCLOSDeque.back().timePERCLOS;
                
            }
            perclosInfo.winTimePERCLOS = PERCLOSDeque.back().winTimePERCLOS + intervalTime;

            //! Only focus on the fixed time interval.
            while(perclosInfo.winTimePERCLOS > TIME_PERCLOS_WINDOW)
            {
                perclosInfo.winTimePERCLOS -= PERCLOSDeque.front().time;
                if(PERCLOSDeque.front().eyePERCLOS == 1)
                {
                    perclosInfo.timePERCLOS -= PERCLOSDeque.front().time;
                }
                PERCLOSDeque.pop_front();
            }
            
            //cout << "time PERCLOS: " << perclosInfo.timePERCLOS << endl;
        }
        else 
        {// The first frames without any PERCLOSInfo
            if (eyesHeight < faceFeatures.Height_Baseline * THRESHOLD_PERCLOS) {
                perclosInfo.eyePERCLOS = 1;
                perclosInfo.timePERCLOS = intervalTime;
            }
            else {
                perclosInfo.eyePERCLOS = 0;
                perclosInfo.timePERCLOS = 0;
            }
            perclosInfo.winTimePERCLOS = intervalTime;
        }
        PERCLOSDeque.push_back(perclosInfo);
        
        //! PERCLOS
        if(perclosInfo.winTimePERCLOS < TIME_PERCLOS_WINDOW / 2)
            PERCLOS = 0;//In first time interval too high value
        else 
            PERCLOS = perclosInfo.timePERCLOS / perclosInfo.winTimePERCLOS;
        
/************************************************/
/*  Statistics of Continuous Eye Closure        */
/*  MICROSLEEP: ND: 0~0.5                       */
/*              SD: 0.5~1                       */
/*              MD: 1~2                         */
/*              VD: 2~4                         */
/*              ED: 4~                          */
/************************************************/
        if (eyesHeight < faceFeatures.Height_Baseline * THRESHOLD_CLOSURE) {
            MICROSLEEPTime += intervalTime;
        } 
        else {
            MICROSLEEPTime -= intervalTime;
            MICROSLEEPTime = MICROSLEEPTime > 0 ? MICROSLEEPTime : 0;
        }
        
        //! When MICROSLEEPTime not equal to 0, Update the MICROSLEEP
        if (MICROSLEEPTime < 0.5)
            MICROSLEEP = 0.25 * MICROSLEEPTime;  //Alert
        else if (MICROSLEEPTime >= 0.5 && MICROSLEEPTime < 1)
            MICROSLEEP = 0.5 * MICROSLEEPTime - 0.125;  //Slightly Drowsy
        else if (MICROSLEEPTime >= 1 && MICROSLEEPTime < 2)
            MICROSLEEP = 0.25 * MICROSLEEPTime + 0.125;  //Moderately Drowsy
        else if(MICROSLEEPTime >= 2 && MICROSLEEPTime < 4)
            MICROSLEEP = 0.125 * MICROSLEEPTime + 0.375;  //Very Drowsy
        else if(MICROSLEEPTime >= 4 && MICROSLEEPTime < 5)
            MICROSLEEP = 0.125 * MICROSLEEPTime + 0.375;
        else
            MICROSLEEP = 1;    //Extremely Drowsy
        
/************************************************/
//! BLINK: Blink Frequency Statistics 
/************************************************/
        InfoBLINK blinkInfo;
        blinkInfo.time = intervalTime;
        if(!BLINKDeque.empty()) {
            /// Calculate the number of frames in fixed time
            if (eyesHeight < faceFeatures.Height_Baseline * THRESHOLD_CLOSURE) {
                blinkInfo.eyeBLINK = 1;
            }
            else {
                blinkInfo.eyeBLINK = 0;
            }
            blinkInfo.winTimeBLINK = BLINKDeque.back().winTimeBLINK + intervalTime;
            
            //! Only focus on the fixed time interval.
            while(blinkInfo.winTimeBLINK  > TIME_BLINK_WINDOW)
            {
                blinkInfo.winTimeBLINK  -= BLINKDeque.front().time;
                BLINKDeque.pop_front();
            }
        }
        else {
            if (eyesHeight < faceFeatures.Height_Baseline * THRESHOLD_CLOSURE) {
                blinkInfo.eyeBLINK = 1;
            }
            else {
                blinkInfo.eyeBLINK = 0;
            }
            blinkInfo.winTimeBLINK = intervalTime;
        }
        BLINKDeque.push_back(blinkInfo);

        //! Calculate the BLINK number 
        int flagBLINK = 0, numBLINK = 0;//start with open eyes
        for (std::deque<InfoBLINK>::const_iterator iter = BLINKDeque.begin(); iter != BLINKDeque.end(); ++iter)
        {
            if (iter->eyeBLINK == 1) {   
                //! closed eyes at first should change flag = 1.
                if (flagBLINK == 0) {
                    flagBLINK = 1;
                    numBLINK ++;
                }
            }
            else {  
                //! open eyes will change flag = 0.
                if (flagBLINK == 1) {
                    flagBLINK = 0;
                } 
            }// end if
        }// end for
        
        BLINK = numBLINK;
//        BLINK = (double)numBLINK / (double)BLINKDeque.size();
//        BLINK = (BLINK - faceFeatures.BLINK_Baseline) / faceFeatures.BLINK_Baseline;
//        BLINK = BLINK < 0 ? 0 : BLINK; 
//        BLINK = BLINK > 1 ? 1 : BLINK; 
        
        
        //! Update the faceFeatures
        faceFeatures.frame  = sampleIdx;
        faceFeatures.Height = eyesHeight;
        faceFeatures.lastHeight = eyesHeight;
        faceFeatures.PERCLOS = PERCLOS;
        faceFeatures.MICROSLEEPTime = MICROSLEEPTime;
        faceFeatures.MICROSLEEP = MICROSLEEP;
        faceFeatures.BLINK = BLINK;
        
    }///GenerateFaceIndicators
Esempio n. 9
0
bool init()
{
    //Initialization flag
    bool success = true;

    //Initialize SDL
    if( SDL_Init( SDL_INIT_VIDEO | SDL_INIT_AUDIO ) < 0 )
    {
        printf( "SDL could not initialize! SDL Error: %s\n", SDL_GetError() );
        success = false;
    }
    else
    {
        //Set texture filtering to linear
        if( !SDL_SetHint( SDL_HINT_RENDER_SCALE_QUALITY, "1" ) )
        {
            printf( "Warning: Linear texture filtering not enabled!" );
        }

        //Create window
        gWindow = SDL_CreateWindow( "SDL Tutorial", SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED, SCREEN_WIDTH, SCREEN_HEIGHT, SDL_WINDOW_SHOWN );
        if( gWindow == nullptr )
        {
            printf( "Window could not be created! SDL Error: %s\n", SDL_GetError() );
            success = false;
        }
        else
        {
            //Create renderer for window
            gRenderer = SDL_CreateRenderer( gWindow, -1, SDL_RENDERER_ACCELERATED );
            if( gRenderer == nullptr )
            {
                printf( "Renderer could not be created! SDL Error: %s\n", SDL_GetError() );
                success = false;
            }
            else
            {
                //Initialize renderer color
                SDL_SetRenderDrawColor( gRenderer, 0xFF, 0xFF, 0xFF, 0xFF );

                 texture = SDL_CreateTexture(gRenderer, SDL_PIXELFORMAT_ARGB8888, SDL_TEXTUREACCESS_STREAMING, CHIP8_W,CHIP8_H);
                 if (!texture) {
                     printf("texture could not initialize!\n");
                     success = false;
                 } else {
                    //Initialize SDL_audio
                    SDL_AudioSpec spec;
                    spec.freq     = 44100;
                    spec.format   = AUDIO_S16SYS;
                    spec.channels = 2;
                    spec.samples  = spec.freq / 20; // 0.05 seconds of latency
                    spec.callback = [] (void*, Uint8* stream, int len)
                    {
                        // Generate square wave
                        short* target = (short*)stream;
                        while(len > 0 && !AudioQueue.empty())
                        {
                            auto& data = AudioQueue.front();
                            for(; len && data.first; target += 2, len -= 4, --data.first)
                                target[0] = target[1] = data.second*300*((len&128)-64);
                            if(!data.first) AudioQueue.pop_front();
                        }
                    };
                    if(SDL_OpenAudio(&spec, &obtained) != 0 ) {
                        printf( "SDL_audio could not initialize! SDL_mixer Error: %s\n", SDL_GetError());
                        success = false;
                    }
                    SDL_PauseAudio(0);
                 }
            }
        }
    }

    return success;
}
void TransIcelandicExpress::simulate() {
	sgVec3 fwd, right, up, frict, v;
	sgVec3 playerForce;
	float cf_accel = 1.0,  // m/s^2 
		  maxVel = 0.5,    // m/2
		  cf_friction_land = 8.0,
		  cf_friction_ice = 5.0;
	float pv, 
		  ff; // friction fudge... more friction for slow objects

	float timestep = 0.01f;
	static float timeleft = 0.0f;	
	timeleft += deltaT;

	sgSetVec3( up, 0.0, 1.0, 0.0 );
	sgSetVec3( playerForce, 0.0, 0.0, 0.0 );	

	while (timeleft > timestep) {
		
		sgCopyVec3( fwd, cameraPos );
		sgNegateVec3( fwd );
		fwd[1] = 0.0;
		sgNormalizeVec3( fwd );
		sgVectorProductVec3( right, fwd, up );

		// todo: if on the ground
		sgScaleVec3( fwd, cf_moveForward );
		sgAddVec3( playerForce, fwd );

		sgScaleVec3( right, cf_moveSideways );
		sgAddVec3( playerForce, right );

		sgScaleVec3( playerForce, cf_accel * timestep );
		sgAddVec3( player->vel, playerForce );

		pv = sgLengthVec3( player->vel ) ;
		ff = (1.0 - ((pv / maxVel)* 0.8));
		ff = ff*ff;

		sgCopyVec3( frict, player->vel );
		sgNegateVec3( frict );
		sgScaleVec3( frict, ff * cf_friction_ice * timestep );	
		sgAddVec3( player->vel, frict );		
		
		dbgVel.push_back( pv );
		if (dbgVel.size() > 100 ) {
			dbgVel.pop_front();
		}
		
		if ( pv > maxVel ) {
			//printf("maxvel!\n" );
			sgNormalizeVec3( player->vel );
			sgScaleVec3( player->vel, maxVel );
		}

		sgCopyVec3( v, player->vel );
		sgScaleVec3( v, timestep );
		sgAddVec3( player->pos, v );
	
		// advance
		timeleft -= timestep;		
	}

	player->pos[1] = getHeight( player->pos );
}
Esempio n. 11
0
gfx::Polygon3D PopFront(std::deque<gfx::Polygon3D>& aPolygons)
{
  gfx::Polygon3D polygon = std::move(aPolygons.front());
  aPolygons.pop_front();
  return polygon;
}
Esempio n. 12
0
int sio_read(std::string &result, int timeout, int pollfd) {
  int once = 1;
  GLOB_LOCK();

  if (!postponed.empty()) {
	result = postponed.front();
	postponed.pop_front();
	//printf("accepted: [%s] (%d)\n", result.c_str(), postponed.size());
	GLOB_UNLOCK();
	return result.length();
  };

  if (inside_read) {    
    GLOB_UNLOCK();
    sio_write(SIO_ERROR, "recursive sio_read");
    return -1;
  };

  int64 stoptime = time64() + timeout*1000;

  while (once || (stoptime > time64())) {
	once = 0;
	int eol_pos = readbuf.find(eol_char);
	if (eol_pos != std::string::npos) {
	  result = readbuf.substr(0, eol_pos);
	  readbuf = readbuf.substr(eol_pos+1);

	  if (sio_commdump & 2) {
		printf("READ\t%s\n", result.c_str());
	  };

	  GLOB_UNLOCK();

	  int rv = _sio_handlemsg(result); // might call other functs
	  if (rv == 0) {
	    GLOB_LOCK();
	    continue;
	  };
	  if (rv < 0) return rv;
	  return result.length();
	};
  
	inside_read = 1;
	GLOB_UNLOCK();

	// wait for some more data...
	fd_set fdr; struct timeval tv;
	int delta = (int)( (stoptime - time64()) / 1000 );
	if (delta < 0) delta = 0;
	tv.tv_sec = delta/1000;  tv.tv_usec = (delta%1000)*1000;
	FD_ZERO(&fdr); FD_SET(ser_fd_read, &fdr);
	if (pollfd!=-1) FD_SET(pollfd, &fdr);
	int rv = select(max(ser_fd_read, pollfd)+1, &fdr, 0, 0, (timeout>=0)?&tv:0);
	if ( (rv<0) && (errno != EINTR) ) {
	  inside_read = 0;
	  return -1; // error
	};

	if (pollfd != -1) if (FD_ISSET(pollfd, &fdr)) {
	  inside_read = 0;
	  return -3; // pollfd is select'ed
	};

	GLOB_LOCK();
	inside_read = 0;
	char bf[257];
	rv = read(ser_fd_read, bf, sizeof(bf)-1);
	if ( (rv<=0) && (errno!=EINTR) && (errno != EAGAIN)) {
	  GLOB_UNLOCK();
	  return -1; // error or timeout
	};
	if (rv<0) continue;
	if (rv==0) {
	  GLOB_UNLOCK();
	  return -1; // EOF from server
	};
	bf[rv]=0;
	//printf("\n\nREAD[[[%s]]]\n", bf);
	readbuf = readbuf + bf;
  };

  GLOB_UNLOCK();
  return 0; // timeout
};
Esempio n. 13
0
void Regex::build(std::deque<Regex::Composition>& tokens) {
    // Expands tokens until [min,max] = [0,1] or [1,1] or [0,inf] for all of them
    std::deque<Composition> newList;
    auto normalizeToken = [&](const Composition& token, bool push) {
        Composition newToken;
        newToken.pattern = token.pattern;
        newToken.min = (token.min == 0) ? 0 : token.min - 1;
        newToken.max = (token.max == -1) ? -1 : token.max - 1;
        if (token.min == 0 && token.max == -1) {
            newToken.ready = true;
        }
        newToken.nestingLevel = token.nestingLevel;
        newToken.special = token.special;
        if (push) {
            newList.push_back(std::move(token));
        }
        tokens.pop_front();
        tokens.push_front(std::move(newToken));
    };
    while (!tokens.empty()) {
        Composition& token = tokens.front();
        if ((token.min != 0 || token.max != 1)
            && (token.min != 1 || token.max != 1)
            && !token.ready) {

            bool push = true;
            if (token.pattern == PATTERN_CONTEXT_END) {
                // The multiplier applies to the group
                std::size_t currSize = newList.size();
                std::size_t index = currSize - 1;
                int level = 0;
                for (auto& t : utils::make_reverse(newList)) {
                    if (t.pattern == PATTERN_CONTEXT_START && level == 0) {
                        break;
                    } else if (t.pattern == PATTERN_CONTEXT_START) {
                        level--;
                    } else if (t.pattern == PATTERN_CONTEXT_END) {
                        level++;
                    }
                    index--;
                }
                newList.push_back(token);
                while (index < currSize) {
                    newList.push_back(newList[index]);
                    index++;
                }
                push = false;
            }
            normalizeToken(token, push);
        } else {
            newList.push_back(std::move(token));
            tokens.pop_front();
        }
    }

    tokens.swap(newList);
    std::size_t i = 0;
    std::size_t size = tokens.size();

    // Links all closing brackets to its opening counterparts
    std::unordered_map<std::size_t, std::size_t> bracketOpenings;
    std::stack<std::size_t> bracketStack;
    while (i < size) {
        if (tokens[i].pattern == PATTERN_CONTEXT_START) {
            bracketStack.push(i);
        } else if (tokens[i].pattern == PATTERN_CONTEXT_END) {
            assert(!bracketStack.empty());
            bracketOpenings[i] = bracketStack.top();
            bracketStack.pop();
        }
        i++;
    }
    assert(bracketStack.empty());

    i = 0;
    // Calculates all next references
    while (i < size - 1) {
        if (tokens[i].pattern == PATTERN_CONTEXT_START) {
            // Context-starters point to the starting symbol of all branches
            tokens[i].next.push_back(i + 1);
            unsigned level = tokens[i].nestingLevel;
            std::size_t j = i + 1;
            while (tokens[j].nestingLevel >= level && j < size) {
                if (tokens[j].nestingLevel == level && tokens[j].pattern == PATTERN_OR) {
                    tokens[i].next.push_back(j + 1);
                }
                j++;
            }
        } else if (tokens[i + 1].pattern != PATTERN_OR) {
            // Simple symbols point to the next symbol
            tokens[i].next = {i + 1};
        } else {
            // The | operator points to the next )
            std::size_t j = i + 1;
            int count = 0;
            // Will always find a match since the last token is a )
            while (count > 0 || tokens[j].pattern != PATTERN_CONTEXT_END) {
                if (tokens[j].pattern == PATTERN_CONTEXT_START) {
                    count++;
                } else if (tokens[j].pattern == PATTERN_CONTEXT_END) {
                    count--;
                }
                j++;
            }
            tokens[i].next = {j};
        }
        i++;
    }

    std::unordered_map<std::size_t, std::size_t> entityToState;
    // Creates all necessary states
    for (i = 0; i < size; i++) {
        if (tokens[i].pattern != PATTERN_OR) {
            entityToState[i] = stateList.size();
            stateList.push_back(State());
        }
    }
    acceptingState = entityToState[size - 1];

    i = 0;
    // Creates all transitions
    while (i < size) {
        Composition& token = tokens[i];
        if (token.pattern != PATTERN_OR) {
            for (std::size_t index : token.next) {
                std::size_t from = entityToState[i];
                std::size_t to = entityToState[index];
                if (token.special) {
                    stateList[from].spontaneous.insert(to);
                } else {
                    stateList[from].transitions[token.pattern] = to;
                }

                if (token.min == 0) {
                    if (token.pattern == PATTERN_CONTEXT_END) {
                        stateList[entityToState[bracketOpenings[i]]].spontaneous.insert(from);
                    }
                    stateList[from].spontaneous.insert(to);
                }

                if (token.max == -1 && token.ready) {
                    if (token.special) {
                        stateList[from].spontaneous.insert(entityToState[bracketOpenings[i]]);
                    } else {
                        stateList[from].spontaneous.insert(entityToState[i - 1]);
                    }
                }
            }
        }
        i++;
    }

    reset();

    // for (i = 0; i < tokens.size(); i++) {
    //     tokens[i].id = i;
    //     debug(tokens[i]);
    //     ECHO("");
    // }

    // for (std::size_t i = 0; i < stateList.size(); i++) {
    //     ECHO(std::to_string(i) + ":");
    //     for (auto& pair : stateList[i].transitions) {
    //         ECHO("\t" + pair.first + " -> " + std::to_string(pair.second));
    //     }
    //     for (auto& index : stateList[i].spontaneous) {
    //         ECHO("\t& -> " + std::to_string(index));
    //     }
    // }
    // TRACE(acceptingState);
}
Esempio n. 14
0
void IrrealVM :: execute( uint64_t thread_id ){
	
	//if( global_vm_queue.size() < 1 ){ return; }
	
	pthread_mutex_lock( &global_vm_queue_lock );
	
	if( global_vm_queue.size() < 1 ){
		pthread_mutex_unlock( &global_vm_queue_lock );
		return; 
		}
	
	uint64_t ctx_id = global_vm_queue.front();
	global_vm_queue.pop_front();
	
	IrrealContext *ctx = global_contexts[ ctx_id ];
	
	pthread_mutex_unlock( &global_vm_queue_lock );
	
	test_for_error( ctx == NULL, "Invalid context!" );
	
	ctx->lock_context();
	
	global_running_threads_vm[ thread_id ] = ctx_id;
	
	//printf( "Executing vm #%lu\n", ctx_id );
	//_debug_running_threads();
	
	
	IrrealStack *current, *code;
	
	current = ctx->getCurrentStack();
	code = ctx->getCodeStack();
	
	test_for_error( current == NULL, "Invalid current stack!" );
	test_for_error( code == NULL, "Invalid code stack!" );
	
	uint8_t state = ctx->getState();
	
	switch( state ){
		case STATE_JOINING:
			if( !current->isJoined() ){
				
				pthread_mutex_lock( &global_vm_queue_lock );
				global_vm_queue.push_back( ctx_id ); 
				pthread_mutex_unlock( &global_vm_queue_lock );
	
				ctx->mark();
	
				ctx->unlock_context();
				return;
				}
		break;
		case STATE_SYNCING:
			
			test_for_error( current->peek() == NULL, "Not enough values to perform 'sync'!" );
			
			//printf( "syncing... ('%s')\n", current->peek()->getValue().c_str() );
			if( current->peek()->getState() == STATE_NOT_YET ){
				
				pthread_mutex_lock( &global_vm_queue_lock );
				global_vm_queue.push_back( ctx_id ); 
				pthread_mutex_unlock( &global_vm_queue_lock );
				
				ctx->mark();
				
				ctx->unlock_context();
				return;
				}
			//printf( "Data synced, now continuing... \n" );
		break;
		}
	
	std::string anon_name;
	bool anon_state = false;
	uint64_t begin_end_counter = 0;
	bool done = false;
	IrrealStack *anon_stack = NULL;
	
	long int debug_value;
	
	IrrealStack *debug_stack;
	
	debug_value = ctx->getStack("PARAMS")->size();
	debug_stack = ctx->getStack("PARAMS");
	while( !done ){
		ctx->mark();
		//printf( "\n\n" ); 
		IrrealValue *q = code->pop();
		//printf( "current: " ); current->_debug_print();
		//printf( "params: " ); ctx->getStack("PARAMS")->_debug_print();
		//printf( "out: " ); ctx->getStack("OUT")->_debug_print();
		if( q == NULL ){
			//printf( "q == NULL\n" );
			done = true; 
			if( ctx->getReturnValue() != NULL ){
				//printf( "Returning value! ('%s')\n", ctx->getReturnValue()->getValue().c_str() );
				ctx->getStack( ctx->getReturnValue()->getValue() )->merge( ctx->getStack( "OUT" ), false ); 
				
				ctx->getReturnValue()->setType( TYPE_SYMBOL );
				ctx->getReturnValue()->setState( STATE_OK );
				}
			
			pthread_mutex_lock( &global_running_vms_lock );
				--global_running_vms;
			pthread_mutex_unlock( &global_running_vms_lock );
			
			continue; 
			}
		if( q->getType() & TYPE_OPERATOR ){
			printf( "q = {'%s', %s} \n", q->getValue().c_str(), debug_cmd_names[ q->getType() & (~0x80 ) ].c_str() );
			}
		else{
			printf( "q = {'%s', %i} \n", q->getValue().c_str(), q->getType() );
			}
		if( anon_state ){
			if( q->getType() & TYPE_OPERATOR ){
				switch( q->getType() ){
					case CMD_BEGIN:
						++begin_end_counter;
					break;
					case CMD_END:
						--begin_end_counter;
					break;
					}
				anon_stack->push( q );
				if( begin_end_counter == 0 ){
					 test_for_error( anon_stack == NULL, "Stack error when parsing block!" );
					 anon_stack->pop();
					 
					 anon_state = false;
					 IrrealValue* value = new IrrealValue();
					 value->setType( TYPE_SYMBOL );
					 value->setValue( anon_name );
					 current->push( value );
					 //printf( "pushing to stack: '%s' \n", current->peek()->getValue().c_str() );
					 continue;
					 }
				}
			else{
				test_for_error( anon_stack == NULL, "Stack error when parsing block!" );
				anon_stack->push( q );
				}
			}
		else{
			
			if( q->getType() & TYPE_OPERATOR ){
				//printf( "Executing command: %s \n", debug_cmd_names[ q->getType() & (~0x80) ].c_str() );
				switch( q->getType() ){
					
					case CMD_BEGIN:
					{
						anon_name = ctx->spawnNewAnonymousStack();
						anon_stack = ctx->getStack( anon_name );
						test_for_error( anon_stack == NULL, "Unable to spawn new anonymous stack!" );
						
						begin_end_counter = 1;
						anon_state = true;
						
					}
					break;
					
					case CMD_PUSH:
					{
						IrrealValue *target_stack_name;
						IrrealValue *value;
						IrrealStack *target_stack;
						
						target_stack_name = current->pop();
						
						value = current->pop();
						
						test_for_error( target_stack_name == NULL, "Not enough values to perform 'push'!" );
						test_for_error( value == NULL, "Not enough values to perform 'push'!" );
						
						
						target_stack = ctx->getStack( target_stack_name->getValue() );
						
						test_for_error( target_stack == NULL, "PUSH: Stack not found!" );
						
						target_stack->push( value );
					}
					break;
					
					case CMD_POP:
					{	
						IrrealValue *target_stack_name;
						IrrealStack *target_stack, *testing;
						IrrealValue *value;
						
						target_stack_name = current->pop();
						
						test_for_error( target_stack_name == NULL, "Not enough values to perform 'pop'!" );
						
						target_stack = ctx->getStack( target_stack_name->getValue() );
						testing = ctx->getStack( target_stack_name->getValue() );
							
						test_for_error( target_stack == NULL, "POP: Stack not found!" );
						
						value = target_stack->pop();
						
						
						if( value == NULL ){
							printf( "\n\n**** Debug info***\n\n" );
							printf( "In PARAMS stack there were %li entries in the beginning...\n", debug_value ); 
							printf( "target_stack_name = '%s' \n", target_stack_name->getValue().c_str() );
							printf( "Context mark count: %lu \n", ctx->read_marks() );
							printf( "target_stack pop_counter = %lu \n", target_stack->_debug_get_counter() );
							printf( "target_stack = %p, target_stack->id = %lu \n", target_stack, target_stack->get_id() );
							printf( "debug_stack = %p, debug_stack->id = %lu \n", debug_stack, debug_stack->get_id() );
							printf( "testing: %p, testing->id = %lu\n", testing, testing->get_id() );
							printf( "debug_stack->size = %lu, target_stack->size = %lu, testing->size = %lu \n", debug_stack->size(), target_stack->size(), testing->size() );
							value = testing->pop();
							printf( "testing->pop = %p, value->getValue = %s\n", value, value->getValue().c_str() ); 
							printf( "\n" );
							fflush( stdout );
							value = NULL;
							}
						test_for_error( value == NULL, "POP: Target stack empty!" );
						
						current->push( value );
					}
					break;
					
					case CMD_DEF:
					{	
						IrrealValue *target_name;
						IrrealValue *value;
						
						target_name = current->pop();
						value = current->pop();
						
						test_for_error( target_name == NULL, "Not enough values to perform 'def'!" );
						test_for_error( value == NULL, "No enough values to perform 'def'!" );
						
						ctx->spawnNewStack( target_name->getValue() );
						
						switch( value->getType() ){
							case TYPE_SYMBOL:
							{
								IrrealStack *target_stack = ctx->getStack( target_name->getValue() );
								IrrealStack *source_stack = ctx->getStack( value->getValue() );
								
								test_for_error( target_stack == NULL, "DEF: Target stack not found!" );
								test_for_error( source_stack == NULL, "DEF: Source stack not found!" );
								
								
								IrrealValue *tmp = source_stack->pop();
								
								
								while( tmp != NULL ){
									target_stack->push( tmp );
									tmp = source_stack->pop();
									}
							}
							break;
							default:
							{
								IrrealStack *target_stack = ctx->getStack( target_name->getValue() );
								test_for_error( target_stack == NULL, "DEF: Target stack not found!" );
								target_stack->push( value );
							}	
							break;
							}
					}	
					break;
					
					case CMD_MERGE:
					{
						IrrealValue *target_name;
						IrrealStack *target_stack;
						
						target_name = current->pop();
						test_for_error( target_name == NULL, "Not enough values to perform 'merge'!" );
						
						target_stack = ctx->getStack( target_name->getValue() );
						
						test_for_error( target_stack == NULL, "MERGE: Stack not found!" );
						
						current->merge( target_stack, false );
					}	
					break;
					
					case CMD_CALL:
					{
						IrrealValue *func, *nparams, *return_value;
						
						nparams = current->pop();
						func = current->pop();
						
						test_for_error( nparams == NULL, "Not enough values to perform 'call'!" );
						test_for_error( func == NULL, "Not enough values to perform 'call'!" );
						
						
						IrrealContext *new_ctx = new IrrealContext();
						
						new_ctx->lock_context();
						
						return_value = new IrrealValue();
						return_value->setType( TYPE_SENTINEL );
						return_value->setState( STATE_NOT_YET );
						return_value->setValue( ctx->spawnNewAnonymousStack() );
						
						//printf( "current->peek() = '%s' \n", current->peek()->getValue().c_str() );
						
						new_ctx->setReturnValue( return_value );
						
						IrrealStack *func_stack = ctx->getStack( func->getValue() );
						
						test_for_error( func_stack == NULL, "CALL: Function not found!" );
						
						new_ctx->getCodeStack()->nondestructive_merge( func_stack, true );
						
						size_t N = string_to_integer( nparams->getValue() );
						
						//printf( "nparams: %lu \n", N );
						
						IrrealStack *params = new_ctx->getStack( "PARAMS" );
						for( size_t i = 0 ; i < N ; ++i ){
							IrrealValue *p = current->pop();
							test_for_error( p == NULL, "Not enough values to perform 'call'!" );
							if( p->getType() == TYPE_SYMBOL ){
								std::string stack_name = ctx->spawnNewAnonymousStack();
								IrrealStack *pstack = ctx->getStack( stack_name );
								IrrealStack *target_stack = ctx->getStack( p->getValue() );
								
								test_for_error( pstack == NULL, "CALL: Unable to spawn new anonymous stack!" );
								test_for_error( target_stack == NULL, "CALL: Undefined symbol!" );
								
								pstack->nondestructive_merge( target_stack, false );
								params->push( new IrrealValue( TYPE_SYMBOL, STATE_OK, stack_name ) );
								}
							else{
								params->push( p );
								}
								
							}
						//printf( "Calling with params: "); params->_debug_print();
						
						//printf( "Merging scope...\n" );
						
						new_ctx->mergeScope( ctx->getScope() );
						
						new_ctx->unlock_context();
						
						pthread_mutex_lock( &global_vm_queue_lock );
						global_vm_queue.push_front( new_ctx->get_id() );
						pthread_mutex_unlock( &global_vm_queue_lock );
						
						pthread_mutex_lock( &global_running_vms_lock );
							++global_running_vms;
						pthread_mutex_unlock( &global_running_vms_lock );
						
						
						current->push( return_value );
						//printf( "current->peek() = '%s' \n", current->peek()->getValue().c_str() );
						
					}
					break;
					
					case CMD_JOIN:
						ctx->setState( STATE_JOINING );
						done = true;
						pthread_mutex_lock( &global_vm_queue_lock );
						global_vm_queue.push_back( ctx->get_id() );
						pthread_mutex_unlock( &global_vm_queue_lock );
						
					break;
					
					case CMD_ADD:
					{
						IrrealValue *first, *second, *value;
						first = current->pop();
						second = current->pop();
						
						test_for_error( first == NULL, "Not enough values to perform 'add'!" );
						test_for_error( second == NULL, "Not enough values to perform 'add'!" );
						
						
						value = new IrrealValue();
						value->setType( TYPE_INTEGER );
						value->setValue( integer_to_string( string_to_integer( first->getValue() ) + string_to_integer( second->getValue() ) ) );
						
						current->push( value );
					}
					break;
					
					case CMD_PRINT:
					{
						IrrealValue *value;
						value = current->pop();
						test_for_error( value == NULL, "Not enough values to perform 'print'!" );
						printf( "print: type = %i, state = %i, value = '%s' \n", value->getType(), value->getState(), value->getValue().c_str() );
						
					}
					break;
					
					case CMD_SYNC:
						ctx->setState( STATE_SYNCING );
						done = true;
						pthread_mutex_lock( &global_vm_queue_lock );
						global_vm_queue.push_back( ctx->get_id() );
						pthread_mutex_unlock( &global_vm_queue_lock );
					break;
					
					case CMD_DUP:
					{
						IrrealValue *value, *new_value;
						value = current->pop();
						test_for_error( value == NULL, "Not enough values to perform 'dup'!" );
						new_value = new IrrealValue( value->getType(), value->getState(), value->getValue() );
					
						current->push( value );
						current->push( new_value );
					}
					break;

					case CMD_WHILE:
					{
						IrrealValue *test, *body;
						
						
/*
{...} {some tests} while

some tests
{ ... 
* {...} {some tests} while 
} {} if

*/
						test = current->pop();
						body = current->pop();
						
						test_for_error( test == NULL, "Not enough values to perform 'while'!" );
						test_for_error( body == NULL, "Not enough values to perform 'while'!" );
						
						
						IrrealStack *new_code = new IrrealStack();
						IrrealStack *test_stack = ctx->getStack( test->getValue() );
						IrrealStack *body_stack = ctx->getStack( body->getValue() );
						
						test_for_error( test_stack == NULL, "Invalid test stack for 'while'!" );
						test_for_error( body_stack == NULL, "Invalid body stack for 'while'!" );
						
						new_code->nondestructive_merge( test_stack, true );
						 						
						new_code->push( new IrrealValue( CMD_BEGIN, STATE_OK, "" ) );
						new_code->nondestructive_merge( body_stack, true );
						new_code->push( body );
						new_code->push( test );
						new_code->push( new IrrealValue( CMD_WHILE, STATE_OK, "" ) );
						new_code->push( new IrrealValue( CMD_END, STATE_OK, "" ) );
						new_code->push( new IrrealValue( CMD_BEGIN, STATE_OK, "" ) );
						new_code->push( new IrrealValue( CMD_END, STATE_OK, "" ) );
						new_code->push( new IrrealValue( CMD_IF, STATE_OK, "" ) );
						
						
						
						//printf( "while: new_code: " ); new_code->_debug_print();
						
						code->merge( new_code, false );
						
						delete new_code;
					}
					break;
					
					case CMD_IF:
					{
						IrrealValue *test, *block_true, *block_false;
							
						block_false = current->pop();
						block_true = current->pop();
						test = current->pop();
						
						test_for_error( block_false ==  NULL, "Not enough values to perform 'if'!" );
						test_for_error( block_true ==  NULL, "Not enough values to perform 'if'!" );
						test_for_error( test ==  NULL, "Not enough values to perform 'if'!" );
						
						
						IrrealStack *stack_true, *stack_false;
						
						stack_true = ctx->getStack( block_true->getValue() );
						stack_false = ctx->getStack( block_false->getValue() );
						
						test_for_error( stack_true == NULL, "IF: Stack (true) not found!" );
						test_for_error( stack_false == NULL, "IF: Stack (false) not found!" );
						
						
						//printf( "if: stack_true: " ); stack_true->_debug_print();
						//printf( "if: stack_false: " ); stack_false->_debug_print();
						
						//if( test == NULL ){ printf( "if: test: null!\n" ); } 
						//printf( "if: test value: %li \n", string_to_integer( test->getValue() ) );
						
						if( string_to_integer( test->getValue() ) ){
							
							code->nondestructive_merge( stack_true, false );
							}
						else{
							code->nondestructive_merge( stack_false, false );
							}
						
					}
					break;
					
					case CMD_SUB:
					{
						IrrealValue *first, *second, *value;
						second = current->pop();
						first = current->pop();
						
						test_for_error( first == NULL, "Not enough values to perform 'sub'!" );
						test_for_error( second == NULL, "Not enough values to perform 'sub'!" );
						
						
						value = new IrrealValue();
						value->setType( TYPE_INTEGER );
						value->setValue( integer_to_string( string_to_integer( first->getValue() ) - string_to_integer( second->getValue() ) ) );
						
						current->push( value );
					}
					break;

					case CMD_MUL:
					{
						IrrealValue *first, *second, *value;
						first = current->pop();
						second = current->pop();
						
						test_for_error( first == NULL, "Not enough values to perform 'mul'!" );
						test_for_error( second == NULL, "Not enough values to perform 'mul'!" );
						
						
						value = new IrrealValue();
						value->setType( TYPE_INTEGER );
						value->setValue( integer_to_string( string_to_integer( first->getValue() ) * string_to_integer( second->getValue() ) ) );
						
						current->push( value );
					}
					break;

					case CMD_DIV:
					{
						IrrealValue *first, *second, *value;
						second = current->pop();
						first = current->pop();
						
						test_for_error( first == NULL, "Not enough values to perform 'div'!" );
						test_for_error( second == NULL, "Not enough values to perform 'div'!" );
						
						
						value = new IrrealValue();
						value->setType( TYPE_INTEGER );
						value->setValue( integer_to_string( string_to_integer( first->getValue() ) / string_to_integer( second->getValue() ) ) );
						
						current->push( value );
					}
					break;

					case CMD_MOD:
					{
						IrrealValue *first, *second, *value;
						second = current->pop();
						first = current->pop();
						
						test_for_error( first == NULL, "Not enough values to perform 'mod'!" );
						test_for_error( second == NULL, "Not enough values to perform 'mod'!" );
						
						value = new IrrealValue();
						value->setType( TYPE_INTEGER );
						value->setValue( integer_to_string( string_to_integer( first->getValue() ) % string_to_integer( second->getValue() ) ) );
						
						current->push( value );
					}
					break;

					case CMD_LENGTH:
					{
						IrrealValue *value;
						value = current->pop();
						
						test_for_error( value == NULL, "Not enough values to perform 'length'!" );
						
						current->push( new IrrealValue( TYPE_INTEGER, STATE_OK, integer_to_string( ctx->getStack( value->getValue() )->size() ) ) );
					
					}
					break;
					
					case CMD_MACRO:
					{
						IrrealValue *value;
						value = current->pop();
						
						test_for_error( value == NULL, "Not enough values to perform 'macro'!" );
						
						//printf( "MACRO: debug: stack name = '%s'\n", value->getValue().c_str() );
						
						IrrealStack *source_stack = ctx->getStack( value->getValue() );
						
						test_for_error( source_stack == NULL, "MACRO: Invalid source stack!" );
						
						code->nondestructive_merge( source_stack, true );
						
					}
					break;
					
					case CMD_SWAP:
					{
						IrrealValue *stack_name, *value0, *value1;
						IrrealStack *target_stack;
						
						stack_name = current->pop();
						
						test_for_error( stack_name == NULL, "Not enough values to perform 'swap'!" );
						
						target_stack = ctx->getStack( stack_name->getValue() );
						
						test_for_error( target_stack == NULL, "SWAP: Invalid stack!" );
						
						value0 = target_stack->pop();
						value1 = target_stack->pop();
						
						test_for_error( value0 == NULL, "SWAP: Not enough values in target stack!" );
						test_for_error( value1 == NULL, "SWAP: Not enough values in target stack!" );
						
						target_stack->push( value0 );
						target_stack->push( value1 );
						
					
					}
					break;
					
					case CMD_ROTR:
					{
						
					
					}
					break;
					
					default:
					break;
					}
				}
			else{
				current->push( q );
				}
			}
		
		}
	
	
	ctx->unlock_context();
	}
Esempio n. 15
0
void processCloud(const sensor_msgs::PointCloud2 msg)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr curr_pc (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PCLPointCloud2 cloud;
	pcl::PointCloud<pcl::PointXYZ> pcl_pc;
	std::vector<int> nan_indices;
	pcl::PointCloud<pcl::PointXYZ>::Ptr prev_pc (new pcl::PointCloud<pcl::PointXYZ>); 
		
	
	//********* Retirive and process raw pointcloud************
	pcl_conversions::toPCL(msg,cloud);
	pcl::fromPCLPointCloud2(cloud,pcl_pc);
	pcl::removeNaNFromPointCloud(pcl_pc,pcl_pc,nan_indices);
	*curr_pc =pcl_pc;

	//*********** Remove old data, update the data***************	
	cloud_seq_loaded.push_back(pcl_pc);
	std::cout<<cloud_seq_loaded.size()<<std::endl;
	
	if(cloud_seq_loaded.size()>2){
		cloud_seq_loaded.pop_front();
	
	}
	if(cloud_seq_loaded.size()==1){
		static_pc = pcl_pc;
		}
	
	//*********** Process currently observerd and buffered data*********

	if(cloud_seq_loaded.size()==2){
		
		
		*prev_pc =static_pc; //cloud_seq_loaded.front(); ss
		
		//*************Create octree structure and search
		pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree (0.5);
		octree.setInputCloud(prev_pc);
		octree.addPointsFromInputCloud();
		octree.switchBuffers();
		octree.setInputCloud(curr_pc);
		octree.addPointsFromInputCloud();
		std::vector<int> newPointIdxVector;
		// Get vector of point indices from octree voxels which did not exist in previous buffer
		octree.getPointIndicesFromNewVoxels (newPointIdxVector);
		std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
		pcl::PointCloud<pcl::PointXYZ>::Ptr dynamic_points (new pcl::PointCloud<pcl::PointXYZ>);
		dynamic_points->header.frame_id = "some_tf_frame";
		
		for (size_t i = 0; i < newPointIdxVector.size (); ++i){
			pcl::PointXYZ point;
			point.x = pcl_pc.points[newPointIdxVector[i]].x;
			point.y = pcl_pc.points[newPointIdxVector[i]].y;
			point.z = pcl_pc.points[newPointIdxVector[i]].z;
			dynamic_points->push_back(point);
			//std::cout << i << "# Index:" << newPointIdxVector[i]<< "  Point:" << pcl_pc.points[newPointIdxVector[i]].x << " "<< pcl_pc.points[newPointIdxVector[i]].y << " "<< pcl_pc.points[newPointIdxVector[i]].z << std::endl;
			
		}	
		std::cout<<newPointIdxVector.size ()<<std::endl;
		
		//***************Filter point cloud to detect nearby changes only *****************
		pcl::PassThrough<pcl::PointXYZ> pass;
		pass.setInputCloud (dynamic_points);
		pass.setFilterFieldName ("z");
		pass.setFilterLimits (0.0, 3.0);
		pass.filter (*dynamic_points);
		
		pcl::StatisticalOutlierRemoval<pcl::PointXYZ> dy_sor;
		dy_sor.setInputCloud (dynamic_points);
		dy_sor.setMeanK (50);
		dy_sor.setStddevMulThresh (1.0);
		dy_sor.filter (*dynamic_points);
		
		//**********************Publish the data************************************
		ros::NodeHandle k;
		ros::Publisher pub = k.advertise<pcl::PointCloud<pcl::PointXYZ> >("dynamicPoints",2);
		pub.publish(dynamic_points);
		ros::Time time = ros::Time::now();
		//Wait a duration of one second.
		ros::Duration d = ros::Duration(1.5, 0);
		d.sleep();
		ros::spinOnce();
		
				

		}
	std::cout<<"finished"<<std::endl;
	std::cout<<std::endl;
	}
Esempio n. 16
0
	bool InotifyDir::EventPoll(Event& event)
	{
		for (;;)
		{
			if (!m_pendingEv.empty())
			{
				event = m_pendingEv.front();
				m_pendingEv.pop_front();
				return true;
			}

			if (m_buffIdx >= m_buffLen)
			{
				m_buffIdx = 0;
				m_buffLen = read(m_wfd, m_buff, sizeof(m_buff));

				if (m_buffLen <= 0)
				{
					return false;
				}
			}

			inotify_event* ev = (inotify_event*)&m_buff[m_buffIdx];
			m_buffIdx += ev->len + sizeof(inotify_event);

			if (ev->mask & (IN_IGNORED | IN_UNMOUNT))
			{
				Unwatch(ev->wd);
				continue;
			}

			if (ev->mask & (IN_CREATE | IN_MOVED_TO))
			{
				if (ev->mask & IN_ISDIR)
				{
					std::string s = m_watchFd[ev->wd] + ev->name;

					if (ev->mask & IN_MOVED_TO)
					{
						AddPathEvent(s.c_str(), ADD);
					}

					continue;
				}
			}

			if (ev->mask & IN_MOVED_FROM && ev->mask & IN_ISDIR)
			{
				//event can't catch
				continue;
			}

			if (ev->mask & IN_MOVE_SELF)
			{
				continue;
			}

			if (ev->mask & IN_DELETE)
			{
				if (ev->mask & IN_ISDIR)
				{
					continue;
				}
			}

			event.clear();

			if (ev->mask & (IN_MOVED_TO | IN_CREATE))
			{
				event.type = ADD;
			}
			else if (ev->mask & (IN_MOVED_FROM | IN_DELETE))
			{
				event.type = DELETE;
			}
			else if (ev->mask & IN_MODIFY)
			{
				event.type = MODIFY;
			}
			else
			{
				event.type = UNKNOWN;
			}

			event.name = m_watchFd[ev->wd] + ev->name;
			return true;
		}
	}