virtual void easedOutCallback(hkaDefaultAnimationControl* control, hkReal deltaTime) { m_env->m_textDisplay->outputText3D( "Eased Out", m_pos(0), m_pos(1), m_pos(2), 0xffffffff, 30); // Ease back in 0.5 second control->easeIn(0.5f); }
virtual void loopUnderflowCallback(hkaDefaultAnimationControl* control, hkReal deltaTime, hkUint32 underflows) { m_env->m_textDisplay->outputText3D( "Underflow", m_pos(0), m_pos(1), m_pos(2), 0xffffffff, 30); // Ease out this animation in 0.5 sec control->easeOut(0.5f); }
int m_atoi(const char* nptr) { bool isNegative = false; int i = 0; int mul = strlen(nptr) - 1; if (index(nptr, '.')) mul = m_pos(nptr, '.') - 1; if (*nptr == '-') isNegative = true; else i += (*nptr - 48) * pow(10, mul); nptr++; mul--; while (*nptr) { if (*nptr == '.') break; i += (*nptr - 48) * pow(10, mul); nptr++; mul--; } if (isNegative) return 0 - i; return i; }
int CSeedManager::OutputOriginalData( CtrlParam* pctrl ) { int ret = TRUE; //开始解压数据 MergeSeedCell *pcurt; pcurt = m_list->NextElement(NULL); m_position = 0; //进度条 int pos = 0; m_position = 0; if(m_range != 0) m_range(m_list->GetElementCnt()); if(m_text != 0) { if(pctrl->type == EXPORT_MSEED) m_text((L"正在合并数据到MSEED"), FALSE); if(pctrl->type == EXPORT_BIN) m_text((L"正在合并数据到SEED"), FALSE); else m_text((L"正在合并数据"), FALSE); } //开始解压 while( pcurt != NULL) { if(m_pos != 0)//进度条更新 m_pos(pos++); pcurt->seed->SetDefaultFormat(m_defFormat); pcurt->seed->SetLogFile(m_plog); //重新加载并分析文件, if(FALSE == pcurt->seed->resume()) { ret = FALSE; break; } if (-1 == m_exp->ExportData((int*)pcurt->sinfo->lpAddr, ARRAY_DATA, pcurt->sinfo->iRecordLen, pctrl) ) { if(m_plog != NULL) m_plog->FormatAndAppendLog(error_seed, erro_data_export, ""); ret = FALSE; break; } //准备下一个seed文件 pcurt->seed->suspend();////////////////////////////////释放文件 pcurt = m_list->NextElement(pcurt); //释放前面已经处理过的seed文件 //m_list->DelFirstElement();/////////////////////////////// m_position ++;//记录进度 } return ret; }
int alloc_big_data (void) { long i,j; double val ; if ((m_big = malloc((x_div*3*y_div*3)*sizeof(long))) == NULL) { fprintf(msgout,"Out of memory!\n"); return(-1) ; } for (j=1; j<= y_div; j++) { for (i=1; i<=x_div; i++) { val = m_fld[m_pos(i,j)] ; m_big[m_posbig(i,j)] = val ; m_big[m_posbig(i+x_div,j)] = val ; m_big[m_posbig(i+2*x_div,j)] = val ; m_big[m_posbig(i,j+y_div)] = val ; m_big[m_posbig(i+x_div,j+y_div)] = val ; m_big[m_posbig(i+2*x_div,j+y_div)] = val ; m_big[m_posbig(i,j+2*y_div)] = val ; m_big[m_posbig(i+x_div,j+2*y_div)] = val ; m_big[m_posbig(i+2*x_div,j+2*y_div)] = val ; } } #if 0 fprintf(msgout,"\nbig data:\n"); for (j=1; j<= y_div*3; j++) { for (i=1; i<=x_div*3; i++) { #if 1 fprintf(msgout," %li", m_big[m_posbig(i,j)]); #else fprintf(msgout," %li", m_posbig(i,j)); #endif } fprintf(msgout,"\n"); } #endif return(0); }
void QualisysDriver::handleSubject(const int& sub_idx) { boost::unique_lock<boost::shared_mutex> write_lock(mtx); // Name of the subject string subject_name(port_protocol.Get6DOFBodyName(sub_idx)); // Pose of the subject float x, y, z, roll, pitch, yaw; prt_packet->Get6DOFEulerBody( sub_idx, x, y, z, roll, pitch, yaw); write_lock.unlock(); // If the subject is lost if(isnan(x) || isnan(y) || isnan(z) || isnan(roll) || isnan(pitch) || isnan(yaw)) { subjects[subject_name]->disable(); return; } // Qualisys sometimes flips 180 degrees around the x axis //if(roll > 90) // roll -= 180; //else if(roll < -90) // roll += 180; // Convert the msgs to Eigen type Eigen::Quaterniond m_att; tf::quaternionTFToEigen( tf::createQuaternionFromRPY(roll*deg2rad, pitch*deg2rad, yaw*deg2rad), m_att); // Convert mm to m Eigen::Vector3d m_pos(x/1000.0, y/1000.0, z/1000.0); // Re-enable the object if it is lost previously if (subjects[subject_name]->getStatus() == Subject::LOST) { subjects[subject_name]->enable(); } // Compute the timestamp // double time = ros::Time::now().toSec(); if(start_time_local_ == 0) { start_time_local_ = ros::Time::now().toSec(); start_time_packet_ = prt_packet->GetTimeStamp() / 1e6; } const double packet_time = prt_packet->GetTimeStamp() / 1e6; const double time = start_time_local_ + (packet_time - start_time_packet_); // Feed the new measurement to the subject subjects[subject_name]->processNewMeasurement(time, m_att, m_pos); // Publish tf if requred if (publish_tf && subjects[subject_name]->getStatus() == Subject::TRACKED) { Quaterniond att = subjects[subject_name]->getAttitude(); Vector3d pos = subjects[subject_name]->getPosition(); tf::Quaternion att_tf; tf::Vector3 pos_tf; tf::quaternionEigenToTF(att, att_tf); tf::vectorEigenToTF(pos, pos_tf); tf::StampedTransform stamped_transform = tf::StampedTransform(tf::Transform(att_tf, pos_tf), ros::Time::now(), fixed_frame_id, subject_name); write_lock.lock(); tf_publisher.sendTransform(stamped_transform); write_lock.unlock(); } return; }
virtual void controlDeletedCallback(hkaAnimationControl* control) { m_env->m_textDisplay->outputText3D( "Deleted", m_pos(0), m_pos(1), m_pos(2), 0xffffffff, 30); }
virtual void easedInCallback(hkaDefaultAnimationControl* control, hkReal deltaTime) { // Animation has Eased in so we'll reverse the play direction m_env->m_textDisplay->outputText3D( "Eased In", m_pos(0), m_pos(1), m_pos(2), 0xffffffff, 30); }
int CSeedManager::OutputDcmpData( CtrlParam* pctrl ) { int ret = TRUE; //开始解压数据 MergeSeedCell *pcurt; CtrlParam loc; memcpy(&loc, pctrl, sizeof(CtrlParam)); pcurt = m_list->NextElement(NULL); //进度条 int pos = 0; m_position = 0; if(m_range != 0) m_range(m_list->GetElementCnt()); if(m_text != 0) { if(pctrl->type == EXPORT_TXT) m_text((L"正在合并数据到TXT"), FALSE); if(pctrl->type == EXPORT_BIN) m_text((L"正在合并数据到BIN"), FALSE); else m_text((L"正在合并数据"), FALSE); } //开始解压 while( pcurt != NULL) { if(m_pos != 0)//进度条更新 m_pos(pos++); pcurt->seed->SetDefaultFormat(m_defFormat); pcurt->seed->SetLogFile(m_plog); //重新加载并分析文件, if(FALSE == pcurt->seed->resume()) { ret = FALSE; break; } //按照要求对数据处理,解压抽取数据 if(FALSE == pcurt->seed->DcmpExtrStationDataByTime(pcurt->index, &loc.tstart, &loc.tend, loc.numerator, loc.denominator) ) continue; //如果提取到了数据,则写入 if (pcurt->sinfo->iDataCnt > 0) { //补数 if (-1 == AddLostData(&pcurt->sinfo->datatime, &loc.tstart, *pcurt->sinfo->pData, &loc)) { if(m_plog != NULL) m_plog->FormatAndAppendLog(error_seed, erro_data_export, ""); ret = FALSE; break; } memcpy(&loc.tstart, &pcurt->sinfo->datatime, sizeof(SYSTEMTIME)); if (-1 == m_exp->ExportData(pcurt->sinfo->pData, ARRAY_DATA, pcurt->sinfo->iDataCnt, &loc) ) { if(m_plog != NULL) m_plog->FormatAndAppendLog(error_seed, erro_data_export, ""); ret = FALSE; break; } //计算下一个seed文件的第一个数据的开始时间 memcpy(&loc.tstart, &pcurt->sinfo->datatime, sizeof(SYSTEMTIME)); SystemTimeAddFraction(&loc.tstart, &loc.tstart, pcurt->sinfo->dataFreq.denominator*pcurt->sinfo->iDataCnt, pcurt->sinfo->dataFreq.numerator); } //准备下一个seed文件 pcurt->seed->suspend();////////////////////////////////释放文件 pcurt->seed->FreeStationData();///////////////////////////释放解压得到的数据缓冲 pcurt = m_list->NextElement(pcurt); //释放前面已经处理过的seed文件 //m_list->DelFirstElement();/////////////////////////////// m_position ++;//记录进度 } //补数 if ( -1 == AddLostData(&loc.tend, &loc.tstart, 0, &loc)) { if(m_plog != NULL) m_plog->FormatAndAppendLog(error_seed, erro_data_export, ""); ret = FALSE; } return ret; }
int main (int argc, char *argv[]) { int i, j, pos ; double a,b,f, dx,dy ; long mat ; msgout = stderr ; fr = stdin ; if (argc > 1) { if ((fr = fopen(argv[1],"r")) == NULL) { fprintf(stderr,"Tensor Scale Analysis:\n Use: %s input_file output_file fea_file\n",argv[0]); exit (-1) ; } } if (read_data (fr) != 0) { exit (-1); } if ((y_div <24)&&(x_div<70)) { fprintf(msgout,"\ninput data:\n"); for (j=0; j<y_div; j++) { for (i=0; i<x_div; i++) { #if 1 fprintf(msgout," %li", m_fld[m_pos(i+1,j+1)]); #else fprintf(msgout," %li", m_pos(i+1,j+1)); #endif } fprintf(msgout,"\n"); } } if (alloc_big_data () != 0) { exit (-1); } if (alloc_line_coords () != 0) { exit (-1); } if (alloc_results () != 0) { exit (-1); } dx = x_siz / ((double) x_div) ; dy = y_siz / ((double) y_div) ; pos = 0 ; for (j=0; j<y_div; j++) { for (i=0; i<x_div; i++) { compute_abf( x_siz + 0.5*dx + ((double)i)*dx, y_siz + 0.5*dy + ((double)j)*dy, &ts_mat[pos], &ts_a[pos], &ts_b[pos], &ts_f[pos]); pos++ ; } } compute_abf(3*x_siz/2, 3*y_siz/2, &mat, &a, &b, &f); fprintf(msgout," A=%e B=%e Angle=%e\n",a,b,f); if (argc > 2) { if ((fw = fopen(argv[2],"w")) == NULL) { fw = stdout ; } printf("[[%s]] (%li)\n", argv[2], ts_len); } else { fw = stdout ; } comp_global(&ksi, &h, 0); printf("ksi = %e. h =%e\n", ksi, h); write_data(fw) ; if (argc > 3) { if ((fwe = fopen(argv[3],"w")) == NULL) { fwe = stdout ; } export_data_ufem(fwe); fclose(fwe); } printf("--------------------\n"); compute_g_abf(stdout) ; fclose(fw); fclose(fr); return(0); }