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);
}
