int main() {

	Screen screen;
	
	long long previousTime = (long long)Util::getCurrentTimeInMiliseconds();
	long long accumulateTime = 0;
	Keyboard::startListening();
	
	Point f_pos(50,20);
	Color  f_col(255,255,255,255);
	Pixel f_plane(f_pos, f_col);
	propeller.setDrawPosition(145,30);
	
	bird.setPosition(50,20);
	fish.setPosition(50,20);

	roda1.setRadius(10);
	roda1.setCenter(Point(40,50));

	roda2.setRadius(10);
	roda2.setCenter(Point(120,50));
	parasut.setPosition(50,10);
	//plane.setPosition(200,10);
	//plane.fillColor(f_plane);
	//plane.fillPattern(&bird);
	
	ship.setPosition(10,250);
	
	
	
	f_pos.x = 50; f_pos.y = 20;
	Pixel f_ship(f_pos, f_col);
	//ship.fillColor(f_ship);
	
	f_pos.x = ship.getWidth() / 2;
	f_pos.y = ship.getHeight() / 2;
	
	
	ship.fillWithFloodFill(f_pos,&fish);
	

	int ii = 0;

	float speed = 0;
	float initialPercentage = 0;
	
	/* Game Clock */
	while (true) {
		if (accumulateTime>(SECONDS_PER_FRAME)) {
			handleInput();
			
			printf("%c[%d;%df",0x1B,199,99);
			screen.beginBatch();
			
			
			if (isPlane){
			  //planeEx += 0.05f;
			  //meledak.setPosition(meledak.getPosition().x - 2,meledak.getPosition().y - 2);

			
	
			    initialPercentage += 0.07;
			plane.explode(initialPercentage);
				speed += 0.1f;

			propeller.applyGravity((int)speed);

			applyGravity(&parasut,(int)speed);
			screen.draw(&parasut);

			if (!isPlaneGrounded){
				isPlaneGrounded = roda1.applyGravity((int)speed);
				roda2.applyGravity((int)speed);
			}else{
				propeller.setState(0);
				isBanSelesai = !roda1.bounce();
				roda2.bounce();
			}

			usleep(FRAMERATE/4);
			  //screen.draw(&meledak, planeEx);
			  
			  if (isBanSelesai){

			      //screen.beginBatch();
			      //screen.endBatch();

			  	  isPlane = false;
			      //isAftermath = true;

				//gotoxy(1, 10);
			      //printf("\t\tSHIP WIN!\n\n\n\n\n\n\n\n\n\n");
			      isFinish = true;

				  /* TODO:
			         - Animasi pesawat pecah
			         - Kasih Gravity tiap pecahan pesawat biar jatuh:
			           - baling-baling
			           - ban
			           - potongan badan pesawat
			           - pilot keluar dari pesawat pake parasut
			         - Bikin ban mantul-mantul di atas tanah -_-
			      */

			      // gotoxy(10, 10);
			      // printf("SHIP WIN!\n\n\n\n\n\n\n\n\n\n");

			      // exit(0);

			  }
			}

			screen.draw(&plane, isFlipPlane);
			screen.draw(&roda1);
			screen.draw(&roda2);
			

			propeller.draw(&screen);

			if (isShip){
			  shipEx += 0.05f;
			  
			  meledak.setPosition(meledak.getPosition().x - 2,meledak.getPosition().y - 2);
			  screen.draw(&meledak, shipEx);
			  
			  if (shipEx > 2){
			      screen.beginBatch();
			      screen.endBatch();
			      //gotoxy(10, 10);
			      //printf("PLANE WIN!\n\n\n\n\n\n\n\n\n\n");
			      isFinish = true;
			      isShip = false;
			  }
			}else if (!isPlane) screen.draw(&ship, isFlipShip);
			
			// handle peluru
			for (int i = 0; i < 100; i++){
				if (b[i] != NULL){
					Point st = b[i]->getPoint();
					bool arah = b[i]->arah;
					b[i] = bf.create(BulletFactory::LASER);
					
					if (arah == true){ //pesawat yang nembak, pelurunya kebawah
						b[i]->arah = true;
						b[i]->rotate(180);
						b[i]->setPoint(Point(st.x, st.y + 1));
						
						if (st.y > 330) b[i] = NULL;
						
						if (st.x > ship.getPosition().x  - 10 && st.x < ship.getPosition().x + 150 && st.y > 220){	// COLLISION
						    meledak.setPosition(ship.getPosition().x,ship.getPosition().y);
						    isShip = true;
						    b[i] = NULL;
						}
						
					}else{
						b[i]->arah = false;
						b[i]->setPoint(Point(st.x, st.y - 1));
						
						if (st.y < 0) b[i] = NULL;
						
						if (st.x > plane.getPosition().x - 5 && st.x < plane.getPosition().x + 180 && st.y < 40){	// COLLISION
						    meledak.setPosition(plane.getPosition().x,plane.getPosition().y);
						    isPlane = true;
						    b[i] = NULL;

							Point explosionCenter(plane.getWidth()/2,plane.getHeight()/2);

							//Ini harus dipanggil sebelum meledak
							plane.startExplode(explosionCenter);
						}
					}
					if (b[i] != NULL) screen.draw(b[i]);
				}
			}

			if (isAftermath) {
				
			//	speed += 1; // speed untuk gravity pull
			//	isPlaneGrounded = plane.applyGravity(speed); /* Kasih efek gravity, return valuenya bakal 1 kalo object nya udah sampe "tanah" */				
			//	screen.draw(&plane, isFlipPlane); /* Ini buat gambar objek2 yang udah mulai jatoh ke tanah */
			//	usleep(FRAMERATE); // ini buat ngedelay kecepetan refresh frame biar gak terlalu cepet
				initialPercentage += 0.01;
				plane.explode(initialPercentage);

				if(initialPercentage == 1) { // periksa kalo semua objek udah sampe tanah, berarti game nya pindah ke state finish
					isAftermath = false;
					isFinish = true;

					screen.beginBatch();
					screen.draw(&plane, isFlipPlane); // ini buat gambar object2 yang udah berserakan di tanah
					screen.endBatch();

					//gotoxy(10,10);
				//	printf("Ship Wins!\n\n\n\n\n");
				//	gotoxy(40,40);
				//	printf("\n");
				}
			}

			if (isFinish) {
				exit(0);
			}

			screen.endBatch();
			
			while (accumulateTime<(SECONDS_PER_FRAME))
				accumulateTime -= (SECONDS_PER_FRAME);
				
		}
		else {
			long long currentTime = (long long)Util::getCurrentTimeInMiliseconds();
			accumulateTime += (currentTime - previousTime);
			previousTime = currentTime;
		}
	}

	return 0;	
}
Esempio n. 2
0
void SparseTrackerAM::pointCloudCallback(const PointCloudT::ConstPtr& cloud_in_ptr)
{
  boost::mutex::scoped_lock(mutex_);

  struct timeval start_callback, end_callback;
  struct timeval start_features, end_features;
  struct timeval start_model, end_model;
  struct timeval start_icp, end_icp;

  gettimeofday(&start_callback, NULL);

  // **** initialize ***********************************************************

  if (!initialized_)
  {
    initialized_ = getBaseToCameraTf(cloud_in_ptr);
    if (!initialized_) return;
  }

  // **** extract features *****************************************************

  gettimeofday(&start_features, NULL);

  // **** ab prediction

  ros::Time cur_time = cloud_in_ptr->header.stamp;
  double dt = (cur_time  - prev_time_).toSec();
  prev_time_ = cur_time;

  tf::Transform predicted_f2b;

  double pr_x, pr_y, pr_z, pr_roll, pr_pitch, pr_yaw;

  if (use_alpha_beta_)
  {
    double cx, cy, cz, croll, cpitch, cyaw;
    getXYZRPY(f2b_, cx, cy, cz, croll, cpitch, cyaw);

    pr_x     = cx     + v_x_     * dt;
    pr_y     = cy     + v_y_     * dt;
    pr_z     = cz     + v_z_     * dt;
    pr_roll  = croll  + v_roll_  * dt;
    pr_pitch = cpitch + v_pitch_ * dt;
    pr_yaw   = cyaw   + v_yaw_   * dt;

    btVector3 pr_pos(pr_x, pr_y, pr_z);
    btQuaternion pr_q;
    pr_q.setRPY(pr_roll, pr_pitch, pr_yaw);

    predicted_f2b.setOrigin(pr_pos);
    predicted_f2b.setRotation(pr_q);
  }
  else
  {
    predicted_f2b = f2b_;
  }

  PointCloudOrb::Ptr orb_features_ptr = boost::make_shared<PointCloudOrb>();
  bool orb_extract_result = extractOrbFeatures(cloud_in_ptr, orb_features_ptr);
  pcl::transformPointCloud (*orb_features_ptr , *orb_features_ptr, eigenFromTf(predicted_f2b * b2c_));
  orb_features_ptr->header.frame_id = fixed_frame_;

  // FIXME - removed canny

  PointCloudCanny::Ptr canny_features_ptr = boost::make_shared<PointCloudCanny>();
  bool canny_extract_result;// = extractCannyFeatures(cloud_in_ptr, canny_features_ptr);
  pcl::transformPointCloud (*canny_features_ptr , *canny_features_ptr, eigenFromTf(predicted_f2b * b2c_));
  canny_features_ptr->header.frame_id = fixed_frame_;

  gettimeofday(&end_features, NULL);

  // **** ICP ******************************

  gettimeofday(&start_icp, NULL);

  bool orb_icp_result = false;
  bool canny_icp_result = false;

  if (orb_extract_result)
  {
    //printf("~ORB~\n");
    orb_icp_result = OrbICP(orb_features_ptr);

    double eps_roll_  = 0.3;
    double eps_pitch_ = 0.3;
    double eps_yaw_   = 0.3;
    double eps_x_     = 0.3;
    double eps_y_     = 0.3;
    double eps_z_     = 0.3;

    if (orb_icp_result)
    {
      tf::Transform corr = tfFromEigen(orb_reg_.getFinalTransformation());

      // particles add error to motion
      double c_x, c_y, c_z, c_roll, c_pitch, c_yaw;
      double e_x, e_y, e_z, e_roll, e_pitch, e_yaw;
      getXYZRPY(corr, c_x, c_y, c_z, c_roll, c_pitch, c_yaw);

      for (int i = 0; i < 20; i++)
      {
        e_roll  = getUrand() * eps_roll_  * c_roll;
        e_pitch = getUrand() * eps_pitch_ * c_pitch;
        e_yaw   = getUrand() * eps_yaw_   * c_yaw;
        e_x = getUrand() * eps_x_  * c_x;
        e_y = getUrand() * eps_y_  * c_y;
        e_z = getUrand() * eps_z_  * c_z;

        btVector3 e_pos(c_x + e_x, c_y + e_y, c_z + e_z);
        btQuaternion e_q;
        e_q.setRPY(c_roll + e_roll, c_pitch + e_pitch, c_yaw + e_yaw);

        tf::Transform e_corr;

        e_corr.setOrigin(e_pos);
        e_corr.setRotation(e_q);

        pf2b_[i] = e_corr * pf2b_[i];
      }

      // end particles

      tf::Transform measured_f2b = corr * predicted_f2b;

      // **** ab estmation
      if (use_alpha_beta_)
      {
        double m_x, m_y, m_z, m_roll, m_pitch, m_yaw;
        getXYZRPY(measured_f2b, m_x, m_y, m_z, m_roll, m_pitch, m_yaw);

        // residuals

        double r_x     = m_x     - pr_x;
        double r_y     = m_y     - pr_y;
        double r_z     = m_z     - pr_z;
        double r_roll  = m_roll  - pr_roll;
        double r_pitch = m_pitch - pr_pitch;
        double r_yaw   = m_yaw   - pr_yaw;

        fixAngleD(r_roll);
        fixAngleD(r_pitch);
        fixAngleD(r_yaw);

        // final position

        double f_x     = pr_x     + alpha_ * r_x;
        double f_y     = pr_y     + alpha_ * r_y;
        double f_z     = pr_z     + alpha_ * r_z;
        double f_roll  = pr_roll  + alpha_ * r_roll;
        double f_pitch = pr_pitch + alpha_ * r_pitch;
        double f_yaw   = pr_yaw   + alpha_ * r_yaw;

        btVector3 f_pos(f_x, f_y, f_z);
        btQuaternion f_q;
        f_q.setRPY(f_roll, f_pitch, f_yaw);

        f2b_.setOrigin(f_pos);
        f2b_.setRotation(f_q);

        // final velocity

        v_x_     = v_x_     + beta_ * r_x     / dt;
        v_y_     = v_y_     + beta_ * r_y     / dt;
        v_z_     = v_z_     + beta_ * r_z     / dt;
        v_roll_  = v_roll_  + beta_ * r_roll  / dt;
        v_pitch_ = v_pitch_ + beta_ * r_pitch / dt;
        v_yaw_   = v_yaw_   + beta_ * r_yaw   / dt;

        //printf("VEL: %f, %f, %f\n", v_x_, v_y_, v_z_);
      }
      else
      {
        f2b_ = measured_f2b;
      }
    }
  }


  if (!orb_icp_result)
    printf("ERROR\n");

  gettimeofday(&end_icp, NULL);

  // **** ADD features to model

  gettimeofday(&start_model, NULL);

  if (model_->points.size() == 0)
  {
    *model_ += *orb_features_ptr;
  }
  else
  {
    pcl::KdTreeFLANN<PointOrb> tree_model;
    tree_model.setInputCloud(model_);

    std::vector<int> indices;
    std::vector<float> distances;

    indices.resize(1);
    distances.resize(1);

    for (int i = 0; i < orb_features_ptr->points.size(); ++i)
    {
      PointOrb& p = orb_features_ptr->points[i];

      int n_found = tree_model.nearestKSearch(p, 1, indices, distances);

      if (n_found == 0)
      {
        model_->points.push_back(p);
        model_->width++;
      }
      else
      {
        if ( distances[0] > 0.01)
        {
          //found far away, insert new one

          model_->points.push_back(p);
          model_->width++;
        }
        else
        {
          // found near, modify old one

          //PointOrb& q = model_->points[indices[0]];
          //q.x = 0.5*(p.x + q.x);
          //q.y = 0.5*(p.y + q.y);
          //q.z = 0.5*(p.z + q.z);
        }
      }
    }
  }

  gettimeofday(&end_model, NULL);

  // *** broadcast tf **********************************************************

  broadcastTF(cloud_in_ptr);

  // *** counter  **************************************************************

  frame_count_++;

  // **** print diagnostics ****************************************************

  gettimeofday(&end_callback, NULL);

  double features_dur  = msDuration(start_features, end_features);
  double model_dur     = msDuration(start_model,    end_model);
  double icp_dur       = msDuration(start_icp,      end_icp);
  double callback_dur  = msDuration(start_callback, end_callback);

  //int model_frames = orb_history_.getSize();
  int icp_iterations = orb_reg_.getFinalIterations();
  int orb_features_count = orb_features_ptr->points.size();
  int canny_features_count =canny_features_ptr->points.size();
  int model_size = model_->points.size();

  printf("F[%d][%d] %.1f \t M[%d] %.1f \t ICP[%d] %.1f \t TOTAL %.1f\n",
      orb_features_count, canny_features_count, features_dur,
      model_size, model_dur,
      icp_iterations,
      icp_dur, callback_dur);

}