Пример #1
0
int read_triggers(struct storage *store, trigger ** tp)
{
    for (;;) {
        trigger_type *ttype;
        char zText[128];

        READ_TOK(store, zText, sizeof(zText));
        if (!strcmp(zText, "end"))
            break;
        ttype = tt_find(zText);
        assert(ttype || !"unknown trigger-type");
        *tp = t_new(ttype);
        if (ttype->read) {
            int i = ttype->read(*tp, store);
            switch (i) {
            case AT_READ_OK:
                tp = &(*tp)->next;
                break;
            case AT_READ_FAIL:
                t_free(*tp);
                *tp = NULL;
                break;
            default:
                assert(!"invalid return value");
                break;
            }
        }
    }
    return 0;
}
Пример #2
0
trigger *trigger_removecurse(curse * c, unit * target)
{
  trigger *t = t_new(&tt_removecurse);
  removecurse_data *td = (removecurse_data *) t->data.v;
  td->curse = c;
  td->target = target;
  return t;
}
Пример #3
0
trigger *trigger_gate(building * b, region * target)
{
  trigger *t = t_new(&tt_gate);
  gate_data *td = (gate_data *) t->data.v;
  td->gate = b;
  td->target = target;
  return t;
}
Пример #4
0
trigger *trigger_giveitem(unit * u, const item_type * itype, int number)
{
    trigger *t = t_new(&tt_giveitem);
    giveitem_data *td = (giveitem_data *)t->data.v;
    td->number = number;
    td->u = u;
    td->itype = itype;
    return t;
}
Пример #5
0
trigger *trigger_createunit(region * r, struct faction * f,
    const struct race * rc, int number)
{
    trigger *t = t_new(&tt_createunit);
    createunit_data *td = (createunit_data *)t->data.v;
    td->r = r;
    td->f = f;
    td->race = rc;
    td->number = number;
    return t;
}
Пример #6
0
trigger *trigger_unitmessage(unit * target, const char *string, int type,
  int level)
{
  trigger *t = t_new(&tt_unitmessage);
  unitmessage_data *td = (unitmessage_data *) t->data.v;
  td->target = target;
  td->string = _strdup(string);
  td->type = type;
  td->level = level;
  return t;
}
Пример #7
0
trigger *trigger_createcurse(struct unit * mage, struct unit * target,
    const curse_type * ct, double vigour, int duration, double effect, int men)
{
    trigger *t = t_new(&tt_createcurse);
    createcurse_data *td = (createcurse_data *)t->data.v;
    td->mage = mage;
    td->target = target;
    td->type = ct;
    td->vigour = vigour;
    td->duration = duration;
    td->effect = effect;
    td->men = men;
    return t;
}
		void rosSpin() {					
			
			double dt;
			
			Eigen::Vector3d t(0.0, 0.0, 0.0);
			Eigen::Vector3d t_new(0.0, 0.0, 0.0);
			Eigen::Vector3d t_vel(0.0, 0.0, 0.0);
			Eigen::Vector3d t_imu(0.0, 0.0, 0.0);
			Eigen::Vector3d t_arm(-0.19, 0.0, 0.02);
			Eigen::Quaterniond q_imu;
			
			ros::Rate rate(15);	
			
			ros::Time time_prev, time_now;
			
			time_prev = ros::Time::now();

			while(ros::ok()) {	
				
				ros::spinOnce();
				
				time_now = ros::Time::now();			
				dt = (time_now - time_prev).toSec();
				time_prev = time_now;		
				
				// Compute Rotation
				
				q_imu = w_imu.getQuaternionForAngularChange(dt);
								
				q = q * q_imu;
								
				// Compute Translation		
				
				t_imu = v_wheel.getRotationArm()*v_wheel.getTranslation() - q_imu.toRotationMatrix()*t_arm + t_arm;
				
				t_new = t + q.toRotationMatrix()*t_imu;
				
				t_vel = (t_new - t)/dt;
				
				t = t_new;
								
				setPose(t, q, t_new);		
				
				pub_data.publish(odometry);
						
				rate.sleep();
			}			
			
		};
Пример #9
0
void
tunDeviceRecForceXValuators (TunDevicePtr tun, int n)
{
  if (n >= tun->nof_xvaluators)
    {
      int		i;
      
      tun->xval_to_lval_tbl = tun->xval_to_lval_tbl
	? t_renew (TunValuatorPtr, tun->xval_to_lval_tbl, n)
	: t_new (TunValuatorPtr, n);
      
      for (i = tun->nof_xvaluators; i < n; i++)
	tun->xval_to_lval_tbl [i] = NULL;
      
      tun->nof_xvaluators = n;
    }
}
Пример #10
0
static void
index_storage_get_status_cache_fields(struct mailbox *box,
				      struct mailbox_status *status_r)
{
	const struct mail_cache_field *fields;
	enum mail_cache_decision_type dec;
	ARRAY_TYPE(const_string) *cache_fields;
	unsigned int i, count;

	fields = mail_cache_register_get_list(box->cache,
					      pool_datastack_create(), &count);

	cache_fields = t_new(ARRAY_TYPE(const_string), 1);
	t_array_init(cache_fields, count);
	for (i = 0; i < count; i++) {
		dec = fields[i].decision & ~MAIL_CACHE_DECISION_FORCED;
		if (dec != MAIL_CACHE_DECISION_NO)
			array_append(cache_fields, &fields[i].name, 1);
	}
	status_r->cache_fields = cache_fields;
}
Пример #11
0
trigger *trigger_clonedied(unit * u)
{
  trigger *t = t_new(&tt_clonedied);
  t->data.v = (void *)u;
  return t;
}
Пример #12
0
trigger *trigger_killunit(unit * u)
{
    trigger *t = t_new(&tt_killunit);
    t->data.v = (void *)u;
    return t;
}
Пример #13
0
trigger *trigger_xmasgate(building * b)
{
    trigger *t = t_new(&tt_xmasgate);
    t->data.v = b;
    return t;
}
Пример #14
0
void
tunInitDeviceRec (int fd, LocalDevicePtr local, TunDevicePtr tun, TunDeviceInfo info)
{
  if (TUN_DEVICE_HAS_VAL_ABS (info) || TUN_DEVICE_HAS_VAL_REL (info))
    { /* valuators first ... */
      int		i,first_valuator = -1;
      int		last_valuator;
      int		nof_valuators = 0;
      TunValuatorPtr	val;
      TunAbsValuatorInfo aval_info;

      if (TUN_DEVICE_HAS_VAL_ABS (info))
	{ /* absolute valuators */
	  for (i = 0, first_valuator = -1; i < ABS_MAX; i++)
	    if (TUN_DEVICE_TEST_VAL_ABS (info, i))
	      {
		last_valuator = i;
		if (first_valuator == -1)
		  first_valuator = i;
		nof_valuators ++;

		TLOG ("[%s] Found absolute valuator %d (%s)", local->name,
		      i, tunGetAbsValuatorName (i));
	      }

	  tun->first_avaluator = first_valuator;
	  tun->nof_avaluators = last_valuator - first_valuator + 1;
	  tun->avaluators = t_new (TunValuatorRec, tun->nof_avaluators);

	  for (i = first_valuator; i <= last_valuator; i++)
	    {
	      val = tun->avaluators + (i - first_valuator);
	      val->xiv = -1;

	      if (TUN_DEVICE_TEST_VAL_ABS (info, i))
		{
		  val->lid = i;
		  tunQueryAbsValuator (fd, i, &aval_info);
		}
	      else
		val->lid = -1;

	      val->value = aval_info.value;
	      val->rel_value = aval_info.value;

	      val->min = aval_info.min;
	      val->max = aval_info.max;

	      TLOG ("Konfigurace valuatoru %d: (%d,%d,%d)",
		    i, val->value, val->min, val->max);

	      val->upsidedown = FALSE;
	      val->mouse_wheel_hack = FALSE;
	      val->abs_is_relspeed = FALSE;
	      val->is_absolute = TRUE;
	    }
	} /* absolute valuators */

      if (TUN_DEVICE_HAS_VAL_REL (info))
	{ /* relative valuators */
	  for (i = 0, first_valuator = -1; i < REL_MAX; i++)
	    if (TUN_DEVICE_TEST_VAL_REL (info, i))
	      {
		if (i > last_valuator) last_valuator = i;
		if (first_valuator == -1) first_valuator = i;
		nof_valuators++;

		TLOG ("[%s] Found relative valuator %d (%s)", local->name,
		      i, tunGetRelValuatorName (i));
	      }
	  
	  tun->first_rvaluator = first_valuator;
	  tun->nof_rvaluators = last_valuator - first_valuator + 1;
	  tun->rvaluators = t_new (TunValuatorRec, tun->nof_rvaluators);
	  
	  for (i = first_valuator; i <= last_valuator; i++)
	    {
	      val = tun->rvaluators + (i - first_valuator);
	      
	      if (TUN_DEVICE_TEST_VAL_REL (info, i))
		val->lid = i;
	      else
		val->lid = -1;

	      val->xiv = -1;

	      val->value = 0;
	      val->min = val->max = -1;

	      val->upsidedown = FALSE;
	      val->mouse_wheel_hack = FALSE;
	      val->abs_is_relspeed = FALSE;
	      val->is_absolute = FALSE;
	    }
	} /* relative valuators */

      tunDeviceRecForceXValuators (tun, nof_valuators);
    } /* valuators */

  TLOG ("Furt tady");

  if (TUN_DEVICE_HAS_KEYS (info))
    {
      int		nof_buttons, min_button, max_button;
      int		nof_keys, min_key, max_key;
      int		i;

      min_button = max_button = min_key = max_key = -1;
      nof_buttons = nof_keys = 0;

      for (i = 0; i < KEY_MAX; i++)
	if (TUN_DEVICE_TEST_KEY (info, i))
	  {
	    if (i < BTN_MISC)
	      { /* key */
		nof_keys ++;
		if (min_key == -1) min_key = i;
		max_key = i;
		TLOG ("[%s] Found key %d (%s)", local->name, i, 
		      tunGetKeyName (i));
	      }
	    else
	      { /* button */
		nof_buttons ++;
		if (min_button == -1) min_button = i;
		max_button = i;
		TLOG ("[%s] Found button %d (%s)", local->name, i,
		      tunGetKeyName (i)) ;
	      }
	  }

      if (nof_buttons)
	{
	  tun->nof_lbuttons = max_button - min_button + 1;
	  tun->first_lbutton = min_button;
	  
	  tun->lbut_to_xbut_tbl = t_new (short int, tun->nof_lbuttons);
	  for (i = 0; i < tun->nof_lbuttons; i++)
	    tun->lbut_to_xbut_tbl [i] = TUN_BUTTON_UNASSIGNED;
	}
    }
}
Пример #15
0
trigger *trigger_shock(unit * u)
{
  trigger *t = t_new(&tt_shock);
  t->data.v = (void *)u;
  return t;
}
Пример #16
0
static void
doit (void)
{
    struct taia stamp;
    struct taia deadline;
    int j = 0, r = 0, iolen = 0;

    for (;;)
    {
        taia_now (&stamp);
        taia_uint (&deadline, 120);
        taia_add (&deadline, &deadline, &stamp);

        iolen = 0;
        udp53io = io + iolen++;
        udp53io->fd = udp53;
        udp53io->events = IOPAUSE_READ;

        tcp53io = io + iolen++;
        tcp53io->fd = tcp53;
        tcp53io->events = IOPAUSE_READ;

        for (j = 0; j < MAXUDP; ++j)
        {
            if (u[j].active)
            {
                u[j].io = io + iolen++;
                query_io (&u[j].q, u[j].io, &deadline);
            }
        }

        for (j = 0; j < MAXTCP; ++j)
        {
            if (t[j].active)
            {
                t[j].io = io + iolen++;
                if (t[j].state == 0)
                    query_io (&t[j].q, t[j].io, &deadline);
                else
                {
                    if (taia_less (&t[j].timeout, &deadline))
                        deadline = t[j].timeout;
                    t[j].io->fd = t[j].tcp;
                    t[j].io->events = (t[j].state > 0) ?
                                            IOPAUSE_READ : IOPAUSE_WRITE;
                }
            }
        }
        iopause (io, iolen, &deadline, &stamp);

        for (j = 0; j < MAXUDP; ++j)
        {
            if (u[j].active)
            {
                r = query_get (&u[j].q, u[j].io, &stamp);
                if (r == -1)
                    u_drop (j);
                if (r == 1)
                    u_respond (j);
            }
        }
        for (j = 0; j < MAXTCP; ++j)
        {
            if (t[j].active)
            {
                if (t[j].io->revents)
                    t_timeout (j);
                if (t[j].state == 0)
                {
                    r = query_get (&t[j].q, t[j].io, &stamp);
                    if (r == -1)
                        t_drop (j);
                    if (r == 1)
                        t_respond (j);
                }
                else
                    if (t[j].io->revents || taia_less (&t[j].timeout, &stamp))
                        t_rw (j);
            }
        }

        if (udp53io && udp53io->revents)
                u_new();

        if (tcp53io && tcp53io->revents)
                t_new();
    }
}
Пример #17
0
trigger *trigger_unguard(building * b)
{
    trigger *t = t_new(&tt_unguard);
    t->data.v = (void *)b;
    return t;
}
Пример #18
0
static trigger *trigger_caldera(building * b)
{
  trigger *t = t_new(&tt_caldera);
  t->data.v = b;
  return t;
}
/* ------------------------------------------------------------------------- */
void MainWindow::on_method2_clicked()
{

    int index_prev;
    int index_now;
    QString ymlFile;

    vector<KeyPoint> imgpts1_tmp;
    vector<KeyPoint> imgpts2_tmp;

    imgpts.resize(filelist.size());

    on_PB_Sift_clicked();

    cout << endl << endl << endl << "Using Method 2:" <<endl;

    vector<DMatch> matches;
    cv::Matx34d P_0;
    cv::Matx34d P_1;

    P_0 = cv::Matx34d(1,0,0,0,
                      0,1,0,0,
                      0,0,1,0);

    cv::Mat_<double> t_prev = (cv::Mat_<double>(3,1) << 0, 0, 0);
    cv::Mat_<double> R_prev = (cv::Mat_<double>(3,3) << 0, 0, 0,
                               0, 0, 0,
                               0, 0, 0);
    cv::Mat_<double> R_prev_inv = (cv::Mat_<double>(3,3) << 0, 0, 0,
                                   0, 0, 0,
                                   0, 0, 0);
    cv::Mat_<double> t_now = (cv::Mat_<double>(3,1) << 0, 0, 0);
    cv::Mat_<double> R_now = (cv::Mat_<double>(3,3) << 0, 0, 0,
                              0, 0, 0,
                              0, 0, 0);
    cv::Mat_<double> t_new = (cv::Mat_<double>(3,1) << 0, 0, 0);
    cv::Mat_<double> R_new = (cv::Mat_<double>(3,3) << 0, 0, 0,
                              0, 0, 0,
                              0, 0, 0);

    reconstruct_first_two_view();
    std::cout << "Pmat[0]  = " << endl << Pmats[0]<<endl;
    std::cout << "Pmat[1]  = " << endl << Pmats[1]<<endl;
    for(index_now = 2; index_now<filelist.size(); index_now++)
    {
        cout << endl << endl << endl <<endl;
        cout << "dealing with " << filelist.at(index_now).fileName().toStdString() << endl;
        cout << endl;

        index_prev = index_now - 1;
        descriptors1.release();
        descriptors1 = descriptors2;
        descriptors2.release();

        ymlFile = ymlFileDir;
        ymlFile.append(filelist.at(index_now).fileName()).append(".yml");
        restore_descriptors_from_file(ymlFile.toStdString(),imgpts[index_now],descriptors2);

        matches.clear();
        //matching
        matching_fb_matcher(descriptors1,descriptors2,matches);
        matching_good_matching_filter(matches);

        imggoodpts1.clear();
        imggoodpts2.clear();
        P_0 = cv::Matx34d(1,0,0,0,
                          0,1,0,0,
                          0,0,1,0);
        P_1 = cv::Matx34d(1,0,0,0,
                          0,1,0,0,
                          0,0,1,0);
        outCloud.clear();

        if(FindCameraMatrices(K,Kinv,distcoeff,
                              imgpts[index_prev],imgpts[index_now],
                              imggoodpts1,imggoodpts2,
                              P_0,P_1,
                              matches,
                              outCloud))

        {//if can find camera matries
            R_prev(0,0) = Pmats[index_prev](0,0);
            R_prev(0,1) = Pmats[index_prev](0,1);
            R_prev(0,2) = Pmats[index_prev](0,2);
            R_prev(1,0) = Pmats[index_prev](1,0);
            R_prev(1,1) = Pmats[index_prev](1,1);
            R_prev(1,2) = Pmats[index_prev](1,2);
            R_prev(2,0) = Pmats[index_prev](2,0);
            R_prev(2,1) = Pmats[index_prev](2,1);
            R_prev(2,2) = Pmats[index_prev](2,2);
            t_prev(0) = Pmats[index_prev](0,3);
            t_prev(1) = Pmats[index_prev](1,3);
            t_prev(2) = Pmats[index_prev](2,3);


            R_now(0,0) = P_1(0,0);
            R_now(0,1) = P_1(0,1);
            R_now(0,2) = P_1(0,2);
            R_now(1,0) = P_1(1,0);
            R_now(1,1) = P_1(1,1);
            R_now(1,2) = P_1(1,2);
            R_now(2,0) = P_1(2,0);
            R_now(2,1) = P_1(2,1);
            R_now(2,2) = P_1(2,2);
            t_now(0) = P_1(0,3);
            t_now(1) = P_1(1,3);
            t_now(2) = P_1(2,3);

            invert(R_prev, R_prev_inv);

            t_new = t_prev + R_prev * t_now ;
            R_new = R_now * R_prev;

            //        //store estimated pose
            Pmats[index_now] = cv::Matx34d	(R_new(0,0),R_new(0,1),R_new(0,2),t_new(0),
                                             R_new(1,0),R_new(1,1),R_new(1,2),t_new(1),
                                             R_new(2,0),R_new(2,1),R_new(2,2),t_new(2));
            cout << "Pmats[index_now]:" << endl << Pmats[index_now]  << endl;

        }
        else
        {
            break;
        }
    }

    cout << endl;
    cout << endl;
    cout << endl;

    //visualization

    imgpts.clear();
    imgpts.resize(filelist.size());
    for( index_now = 1; index_now<filelist.size(); index_now++)
    {
        index_prev = index_now - 1;
        descriptors1.release();
        descriptors2.release();

        ymlFile = ymlFileDir;
        ymlFile.append(filelist.at(index_prev).fileName()).append(".yml");
        cout << ymlFile.toStdString()<< endl;
        restore_descriptors_from_file(ymlFile.toStdString(),imgpts[index_prev],descriptors1);

        ymlFile = ymlFileDir;
        ymlFile.append(filelist.at(index_now).fileName()).append(".yml");
        cout << ymlFile.toStdString()<< endl;
        restore_descriptors_from_file(ymlFile.toStdString(),imgpts[index_now],descriptors2);

        matches.clear();
        //matching
        matching_fb_matcher(descriptors1,descriptors2,matches);
        matching_good_matching_filter(matches);

        imgpts1_tmp.clear();
        imgpts2_tmp.clear();

        GetAlignedPointsFromMatch(imgpts[index_prev], imgpts[index_now], matches, imgpts1_tmp, imgpts2_tmp);

        cout << imgpts1_tmp.size() << endl;
        cout << imgpts1_tmp.size() << endl;

        outCloud.clear();
        std::vector<cv::KeyPoint> correspImg1Pt;
        double mean_proj_err = TriangulatePoints(imgpts1_tmp, imgpts2_tmp, K, Kinv,distcoeff, Pmats[index_prev], Pmats[index_now], outCloud, correspImg1Pt);

        std::vector<CloudPoint> outCloud_tmp;
        outCloud_tmp.clear();
        for (unsigned int i=0; i<outCloud.size(); i++)
        {
            if(outCloud[i].reprojection_error <= 3){
                //cout << "surving" << endl;
                outCloud[i].imgpt_for_img.resize(filelist.size());
                for(int j = 0; j<filelist.size();j++)
                {
                    outCloud[i].imgpt_for_img[j] = -1;
                }
                outCloud[i].imgpt_for_img[index_now] = matches[i].trainIdx;
                outCloud_tmp.push_back(outCloud[i]);
            }
        }
        outCloud.clear();
        outCloud= outCloud_tmp;

        cout << outCloud_tmp.size() << endl;

        for(unsigned int i=0;i<outCloud.size();i++)
        {
            outCloud_all.push_back(outCloud[i]);
        }

        outCloud_new = outCloud;

    }
    for( int i = 0; i<filelist.size(); i++)
        Pmats[i](1,3) =0;

    GetRGBForPointCloud(outCloud_all,pointCloudRGB);

    ui->widget->update(getPointCloud(),
                       getPointCloudRGB(),
                       getCameras());

    cout << "finished" <<endl <<endl;
}