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; // 用于控制指节运动的先后,现在的情况下,前一个指节停止运动后,后一个才会开始运动 } } } }