/**
* @brief idle
*/
void idle()
{
double deltaT = (double)TIMER_MS;
optotrak.updateMarkers(deltaT);
markers = optotrak.getAllMarkers();
// Coordinates picker
allVisiblePlatform = isVisible(markers.at(15).p) && isVisible(markers.at(16).p);
allVisibleThumb = isVisible(markers.at(11).p) && isVisible(markers.at(12).p) && isVisible(markers.at(13).p);
allVisibleIndex = isVisible(markers.at(7).p) && isVisible(markers.at(8).p) && isVisible(markers.at(9).p);
allVisibleFingers = allVisibleThumb && allVisibleIndex;
allVisiblePatch = isVisible(markers.at(1).p) && isVisible(markers.at(2).p) && isVisible(markers.at(3).p);
allVisibleHead = allVisiblePatch && isVisible(markers.at(17).p) && isVisible(markers.at(18).p);
//if ( allVisiblePatch )
headEyeCoords.update(markers.at(1).p,markers.at(2).p,markers.at(3).p,deltaT);
// update thumb coordinates
thumbCoords.update(markers.at(11).p,markers.at(12).p,markers.at(13).p,deltaT);
// update index coordinates
indexCoords.update(markers.at(7).p, markers.at(8).p, markers.at(9).p,deltaT);
// Compute the coordinates of visual thumb
thumb = thumbCoords.getP1().p;
index = indexCoords.getP1().p;
eyeLeft = headEyeCoords.getLeftEye().p;
eyeRight = headEyeCoords.getRightEye().p;
rigidCurrentThumb.setRigidBody(markers.at(11).p,markers.at(12).p,markers.at(13).p);
rigidCurrentIndex.setRigidBody(markers.at(7).p,markers.at(8).p,markers.at(9).p);
rigidStartThumb.computeTransformation(rigidCurrentThumb);
rigidStartIndex.computeTransformation(rigidCurrentIndex);
if (headCalibrationDone==3 && fingerCalibrationDone==4)
{
if ( !allVisibleIndex || !allVisibleThumb || !isVisible(markers.at(6).p))
boost::thread invisibleBeep( beepInvisible);
Vector2d thumbProjection = cam.computeProjected(thumb);
thumbProjectedInside = ( thumbProjection.x() >= 0 && thumbProjection.x() <= SCREEN_WIDTH ) && ( thumbProjection.y() >= 0 && thumbProjection.y() <= SCREEN_HEIGHT );
Vector2d indexProjection = cam.computeProjected(index);
indexProjectedInside = ( indexProjection.x() >= 0 && indexProjection.x() <= SCREEN_WIDTH ) && ( indexProjection.y() >= 0 && indexProjection.y() <= SCREEN_HEIGHT );
trialFrame++;
// Work on fingers coordinates
double wThumb=0.5, wIndex=0.5;
Vector3d m = (wThumb*thumb+wIndex*index)/(wThumb+wIndex);
Vector3d d = thumb-index;
visualThumb = thumb;
visualIndex = index;
double clamp= 2*factors.getCurrent().at("StimulusRadius");
double gain = 1.0;//in quello di Bob era invece: factors.getCurrent().at("Gain");
if ( d.norm() > clamp )
{
visualThumb = m + 0.5*d.normalized()*(gain*d.blueNorm() - clamp*(gain-1));
visualIndex = m - 0.5*d.normalized()*(gain*d.blueNorm() - clamp*(gain-1));
}
// Check when to advance trial
double stimRadius = factors.getCurrent().at("StimulusRadius");
double radiusTolerance = util::str2num<double>(parameters.find("RadiusTolerance"));
fingNow = fingersInSphericalVolume(thumb,index, visualStimCenter, stimRadius-radiusTolerance, stimRadius+radiusTolerance, selectedFinger );
// SubjectName\tFingerDist\tTrialNumber\tTrialFrame\tTotTime\tVisualStimX\tVisualStimY\tVisualStimZ\tfStimulusRadius\tfDistances\tfGain\tEyeLeftXraw\tEyeLeftYraw\tEyeLeftZraw\tEyeRightXraw\tEyeRightYraw\tEyeRightZraw\tWristXraw\tWristYraw\tWristZraw\tThumbXraw\tThumbYraw\tThumbZraw\tIndexXraw\tIndexYraw\tIndexZraw\tVisualThumbXraw\tVisualThumbYraw\tVisualThumbZraw\tVisualIndexXraw\tVisualIndexYraw\tVisualIndexZraw\tIsDrawing\tIsThumbProjected\tIsIndexProjected
if (!isSaving)
return;
RowVector3d junk(9999,9999,9999);
markersFile << fixed << setprecision(3) <<
parameters.find("SubjectName") << "\t" <<
fingerDistance << "\t" <<
totalTrialNumber << "\t" <<
trialFrame << "\t" <<
globalTimer.getElapsedTimeInMilliSec() << "\t" <<
visualStimCenter.x() << "\t" <<
visualStimCenter.y() << "\t" <<
visualStimCenter.z() << "\t" <<
factors.getCurrent().at("StimulusRadius") << "\t" <<
factors.getCurrent().at("Distances") << "\t" <<
factors.getCurrent().at("Gain") << "\t" <<
factors.getCurrent().at("DisappearRadius") << "\t" <<
( isVisible(eyeLeft) ? eyeLeft.transpose() : junk ) << "\t" <<
( isVisible(eyeRight) ? eyeRight.transpose() : junk ) << "\t" <<
( isVisible(markers.at(6).p) ? markers.at(6).p.transpose() : junk ) << "\t" <<
( isVisible(thumb) ? thumb.transpose() : junk ) << "\t" <<
( isVisible(index) ? index.transpose() : junk ) << "\t" <<
( isVisible(visualThumb) ? visualThumb.transpose() : junk ) << "\t" <<
( isVisible(visualIndex) ? visualIndex.transpose() : junk ) << "\t" <<
isDrawing << "\t" <<
thumbProjectedInside << "\t" <<
indexProjectedInside << "\t" <<
fingNow <<
endl;
// Switch to this function if you want cylindrical stimulus
//fingersInCylindricalVolume( thumb,index, Vector3d(0,0,factors.getCurrent().at("Distances")), util::str2num<double>(parameters.find("StimulusHeight")), stimRadius,stimRadius+20, selectedFinger );
if (trialMode == STIMULUSMODE )
{
if ( !fingNow ) // always starts counting the time if the finger is outside
{
fingersTimer.start();
}
// only if the finger is inside the volume the counter has counted more that 1 second
if ( fingersTimer.getElapsedTimeInMilliSec() > util::str2num<double>(parameters.find("LeaningTime")) )
{
plato_write(PLATO_LEFT_RIGHT_CLOSED);
advanceTrial();
plato_write(PLATO_LEFT_RIGHT_OPEN);
}
trialTimer.start(); // continue keeping it starting during stimulus
drawingTrialFrame++;
invisibleIndexFrames+=!allVisibleIndex;
invisibleThumbFrames+=!allVisibleThumb;
invisibleWristFrames+=!isVisible(markers.at(6).p);
}
else
{
if ( trialTimer.getElapsedTimeInMilliSec() > util::str2num<double>(parameters.find("IntervalTime")) )
{
boost::thread trialBeep( beepTrial );
trialMode=STIMULUSMODE;
}
}
isDrawing = (trialMode == STIMULUSMODE);
}
}
