void EDS_2x3PlaneFingers::_updateData()
{
if (_isStopped() == true)
{
Sleep(50);
// writeData
if(mOutputFile.is_open())
{
double abs_ObjPosX = mObjPosX+mGraspingObj->getOriginPosition().x();
double abs_ObjPosY = mObjPosY+mGraspingObj->getOriginPosition().y();
mOutputFile << abs_ObjPosX << ", " << abs_ObjPosY << ", ";
int i,j;
for(i=0; i<2; i++)
for(j=0; j<3; j++)
mOutputFile << mTheta[i][j] << ", ";
for(i=0; i<2; i++)
for(j=0; j<3; j++)
{
mOutputFile << mContactPos[i][j].x() << ", ";
mOutputFile << mContactPos[i][j].y() << ", ";
mOutputFile << mContactPos[i][j].z() << ", ";
}
for(i=0; i<2; i++)
for(j=0; j<3; j++)
{
mOutputFile << mContactNormal[i][j].x() << ", ";
mOutputFile << mContactNormal[i][j].y() << ", ";
mOutputFile << mContactNormal[i][j].z() << ", ";
}
// use '0' as tail of this record
mOutputFile << 0 << '\n';
}
// re-position the object
if (mObjPosX <= 80 && mObjPosY <= 40)
{
mObjPosX += 4;
if (mObjPosX > 80)
{
mObjPosX = 0;
mObjPosY += 4;
}
mGraspingObj->setPosition(osg::Vec3(mObjPosX, mObjPosY, 0) * mHand->getHandScale());
mGraspingObj->makeTransform();
}
_makeDataZero();
return;
}
// 分界线以上是当一次试验完成时，对所有数据的清零操作
//////////////////////////////////////////////////////////////////////////
// 分界线以下是试验进行过程中，对所有数据的一般更新操作
for (int i=0; i<2; i++)
{
for (int j=0; j<3; j++)
{
if (mCollided[i][j]==0 && mReachLimit[i][j]==0)
{
Part* pt = mHand->getFingerFromVector(i)->getKnuckleAt(j);
double CurrentPos = pt->getAttitude().x();
// 当前角位置大于90°，则认为到达极限位置
if(fabs(CurrentPos) >= 90)
mReachLimit[i][j] = 1;
// 否则，更新角位置
else
mTheta[i][j] += pt->getDeltaAttitudePerFrame().x();
break; // 用于控制指节运动的先后，现在的情况下，前一个指节停止运动后，后一个才会开始运动
}
}
}
}
