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; }
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; }
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; }
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; }
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; }
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; }
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(); } };
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; } }
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; }
trigger *trigger_clonedied(unit * u) { trigger *t = t_new(&tt_clonedied); t->data.v = (void *)u; return t; }
trigger *trigger_killunit(unit * u) { trigger *t = t_new(&tt_killunit); t->data.v = (void *)u; return t; }
trigger *trigger_xmasgate(building * b) { trigger *t = t_new(&tt_xmasgate); t->data.v = b; return t; }
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; } } }
trigger *trigger_shock(unit * u) { trigger *t = t_new(&tt_shock); t->data.v = (void *)u; return t; }
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(); } }
trigger *trigger_unguard(building * b) { trigger *t = t_new(&tt_unguard); t->data.v = (void *)b; return t; }
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; }