Vector3d getEyeProjectionPoint()
{
Eigen::Hyperplane<double,3> focalPlane = Eigen::Hyperplane<double,3>::Through ( Vector3d(1,0,focalDistance), Vector3d(0,1,focalDistance),Vector3d(0,0,focalDistance) );
// Projection of view normal on the focal plane
Vector3d directionOfSight = (headEyeCoords.getRigidStart().getFullTransformation().rotation()*Vector3d(0,0,-1)).normalized();
Eigen::ParametrizedLine<double,3> lineOfSightRight = Eigen::ParametrizedLine<double,3>::Through( eyeRight , eyeRight+directionOfSight );
double dist = lineOfSightRight.intersection(focalPlane);
return (dist*(directionOfSight)+ (eyeRight));
}
Affine3d getPassiveMatrix()
{
Screen screenPassive;
screenPassive.setWidthHeight(SCREEN_WIDE_SIZE, SCREEN_WIDE_SIZE*SCREEN_HEIGHT/SCREEN_WIDTH);
screenPassive.setOffset(alignmentX,alignmentY);
screenPassive.setFocalDistance(0);
screenPassive.transform(headEyeCoords.getRigidStart().getFullTransformation()*Translation3d(Vector3d(0,0,focalDistance)));
VRCamera camPassive;
camPassive.init(screenPassive);
camPassive.setDrySimulation(true);
camPassive.setEye(eyeRight);
return ( camPassive.getModelViewMatrix()*objectActiveTransformation );
}
void idle()
{ // Set the time during which the stimulus is drawn, it depents on the phase (adaption or test)
double drawStimTime = 0;
if ( block.at("Phase") == 1 )
drawStimTime = str2num<double>(parameters.find("AdaptStimulusDuration"));
else
drawStimTime = str2num<double>(parameters.find("TestStimulusDuration"));
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);
eyeLeft = headEyeCoords.getLeftEye().p;
eyeRight = headEyeCoords.getRightEye().p;
checkBounds();
indexInside[1]=indexInside[0];
// Controlla i frames occlusi, se il dito di test è fuori dalla sfera
// ed ha superato una percentuale di tempo maggiore di
// TestPercentOccludedFrames rispetto al tempo di presentazione dello
// stimolo allora fa un beep
if ( headCalibrationDone==3 && fingerCalibrationDone==3 && !indexInside[0] && (int) block.at("Phase")!=1 && isDrawing )
{
bool visibleFinger=true;
switch ( str2num<int>(parameters.find("TestFinger")) )
{
case 0:
visibleFinger=isVisible(thumbCoords.getP1().p);
break;
case 1:
visibleFinger=isVisible(indexCoords.getP1().p);
break;
case 2:
visibleFinger=isVisible(thumbCoords.getP1().p) && isVisible(indexCoords.getP1().p);
break;
}
// XXX scommentare per far si che anche il polso venga controllato per missing frames
//visibleFinger = visibleFinger && isVisible(markers.at(6).p);
if ( !visibleFinger )
{ occludedFrames++;
if ( str2num<int>(parameters.find("AudioFeedback") ) )
boost::thread invisibleBeep( beepInvisible);
}
trialOk=true; // mentre disegna lo stimolo
}
// Qui durante la fase di delay dopo la scomparsa dello stimolo controlla che il numero
// di frames occlusi non abbia superato il limite consentito
if ( isDrawing==0 && headCalibrationDone==3 && (block.at("Phase")!=1) && !indexInside[0] )
{
double percentOccludedFrames = ((double)occludedFrames/(double)drawingTrialFrame )*100.0;
trialOk=(percentOccludedFrames < str2num<double>(parameters.find("TestPercentOccludedFrames")));
}
// Calcola le coordinate delle dita offsettate durante la fase di adattamento
if ( block.at("Phase") == 1 )
{ double fingerOffset = adaptOffsets.at(block1TrialNumber) ;
switch ( str2num<int>(parameters.find("AdaptFinger")) )
{
case 0: //index
visualThumb = thumbCoords.getP1().p + Vector3d(0,0,fingerOffset);
break;
case 1: //thumb
visualIndex = indexCoords.getP1().p + Vector3d(0,0,fingerOffset);
break;
case 2:
{ visualThumb = thumbCoords.getP1().p + Vector3d(0,0,fingerOffset);
visualIndex = indexCoords.getP1().p + Vector3d(0,0,fingerOffset);
}
break;
}
drawingTrialFrame=0;
occludedFrames=0;
}
if ( headCalibrationDone==3 && fingerCalibrationDone==3 && trialMode==STIMULUSMODE )
{
if ( globalTimer.getElapsedTimeInMilliSec() <= drawStimTime )
{ isDrawing=1;
delayedTimer.start();
}
else
{ // non disegna nulla
if ( delayedTimer.getElapsedTimeInMilliSec() < str2num<int>(parameters.find("DelayTime")) )
isDrawing=0;
else
{ delayedTimer.start();
advanceTrial();
}
}
}
if ( (int) block.at("Phase") != 1 && isDrawing==1 )
{ drawingTrialFrame++;
}
#define WRITE
int thisBlockTrialNumber=0;
switch ( block.at("Phase") )
{
case 0:
thisBlockTrialNumber=block0TrialNumber;
break;
case 1:
thisBlockTrialNumber=block1TrialNumber;
break;
case 2:
thisBlockTrialNumber=block2TrialNumber;
break;
}
if ( !isMovingRod )
{
RowVector3d junk(9999,9999,9999);
// Write to file
if ( headCalibrationDone==3 && fingerCalibrationDone==3 )
{
if ( totalFrame==0)
{
markersFile << fixed << setprecision(3) <<
"SubjectName\tAdaptFinger\tTestFinger\tAdaptOffset\tAdaptHapticFeedback\tFingerDist\tTotalTrial\tPhase\tTrialNumber\tTrialFrame\tdT\tTotTime\tIndexInside\tIsDrawing\tTrialOk\tVisualStimX\tVisualStimY\tVisualStimZ\tHapticRodX\tHapticRodY\tHapticRodZ\tfRelDepth\tfDistances\tDeltaXRods\tEyeLeftXraw\tEyeLeftYraw\tEyeLeftZraw\tEyeRightXraw\tEyeRightYraw\tEyeRightZraw\tWristXraw\tWristYraw\tWristZraw\tThumbXraw\tThumbYraw\tThumbZraw\tIndexXraw\tIndexYraw\tIndexZraw" << endl;
totalFrame++;
}
markersFile << fixed << setprecision(3) <<
parameters.find("SubjectName") << "\t" <<
parameters.find("AdaptFinger") << "\t" <<
parameters.find("TestFinger") << "\t" <<
adaptOffsets.at(block1TrialNumber) << "\t" <<
parameters.find("AdaptHapticFeedback") << "\t" <<
fingerDistance << "\t" <<
totalTrialNumber << "\t" <<
block.at("Phase") << "\t" <<
thisBlockTrialNumber << "\t" <<
trialFrame << "\t" <<
deltaT*1000 << "\t" <<
totalTime.getElapsedTimeInMilliSec() << "\t" <<
indexInside[0] << "\t" <<
isDrawing << "\t" <<
trialOk << "\t" <<
visualRodCenter.transpose() << "\t" <<
hapticRodCenter.transpose() << "\t" <<
((block.at("Phase")!=1) ? factors.at("RelDepth") : 9999) << "\t" <<
((block.at("Phase")!=1) ? factors.at("Distances") : 9999) << "\t" <<
deltaXRods << "\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(thumbCoords.getP1().p) ? thumbCoords.getP1().p.transpose() : junk ) << "\t" <<
( isVisible(indexCoords.getP1().p) ? indexCoords.getP1().p.transpose() : junk ) << endl;
}
trialFrame++;
}
#ifdef SAVEDATADAT
eulerAngles.init( headEyeCoords.getRigidStart().getFullTransformation().rotation() );
// Write datafile to real-time streaming of data
ofstream outputfile;
outputfile.open("data.dat");
outputfile << "Subject Name: " << parameters.find("SubjectName") << endl;
outputfile << "AdaptHapticFeedback: " << parameters.find("AdaptHapticFeedback") << endl;
outputfile << "AdaptFinger: " << parameters.find("AdaptFinger") << endl;
outputfile << "TestFinger: " << parameters.find("TestFinger") << endl;
outputfile << "IndexInside: " << indexInside[0] << endl;
//outputfile << "Yaw: " << toDegrees(eulerAngles.getYaw()) << endl <<"Pitch: " << toDegrees(eulerAngles.getPitch()) << endl << "Roll: " << toDegrees(eulerAngles.getRoll()) << endl;
outputfile << "EyeLeft: " << headEyeCoords.getLeftEye().p.transpose() << endl;
outputfile << "EyeRight: " << headEyeCoords.getRightEye().p.transpose() << endl << endl;
outputfile << "Thumb: " << thumbCoords.getP1().p.transpose() << endl;
outputfile << "Index: " << indexCoords.getP1().p.transpose() << endl;
outputfile << "Wrist: " << markers.at(6).p.transpose() << endl;
outputfile << "IsDrawing: " << isDrawing << " TrialOk: " << trialOk << " OccludedFrames: " << occludedFrames << " DrawingTrialFrames: " << drawingTrialFrame << endl;
outputfile << "HapticRodCenter: " << hapticRodCenter.transpose() << endl;
outputfile << "VisualRodCenter: " << visualRodCenter.transpose() << endl;
outputfile << "Markers(5): " << markers.at(5).p.transpose() << endl;
outputfile << "AdaptOffset: " << adaptOffsets.at(block1TrialNumber) << endl;
outputfile << "Phase: " << block.at("Phase") << endl;
outputfile << "TrialNumber: " << thisBlockTrialNumber << endl;
outputfile << "Total trials: " << totalTrialNumber << endl;
#endif
frameTimer.start();
}
void idle()
{
if (trialNumber >= maxTotalTrials )
exit(0);
double elapsedFrameTime = totalTimer.getTimeIntervalInMilliSec();
optotrak.updateMarkers(elapsedFrameTime);
markers = optotrak.getAllMarkers();
headEyeCoords.update(markers[1],markers[2],markers[3],TIMER_MS);
allVisiblePatch = markers[1].isVisible() && markers[2].isVisible()
&& markers[3].isVisible();
allVisibleHead = markers[17].isVisible() && markers[18].isVisible() && allVisibleHead;
eyeLeft = headEyeCoords.getLeftEye().p;
eyeRight = headEyeCoords.getRightEye().p;
cyclopeanEye = (eyeLeft+eyeRight)/2.0;
projPointEyeRight = getEyeProjectionPoint();
checkBounds(nOscillationsFixation,
eyeRight.x(),
trialMode,
headCalibrationDone,
minOscTime,
maxOscTime,
maxXOscillation,
translationTimer,
beepOk,
tweeter,
woofer,tweeter);
if ( trialMode == STIMULUSMODE )
deltaT+=TIMER_MS;
else
deltaT=0;
if (headCalibrationDone == 3 && trialMode != PROBEMODE )
{
// Questo rende conto del fatto che lo stimolo appare solo quando l'occhio è quasi in centro
int actualTrialMode = trialMode;
if ( trialMode == STIMULUSMODE && ( eyeRight.x()) > centerTolerance )
actualTrialMode=FIXATIONMODE;
markersFile << fixed << trialNumber << " " << actualTrialMode << " " ;
markersFile << fixed << setprecision(3) << eyeRight.transpose() << " " << eyeLeft.transpose() << " " << toDegrees(headEyeCoords.getPitch()) << " " << toDegrees(headEyeCoords.getYaw()) << " " << toDegrees(headEyeCoords.getRoll()) << " " ;
markersFile << fixed << setprecision(0)<<
factors["OmegaY"] << " " <<
factors["Binocular"] << " " <<
factors["Tilt"] << " " <<
factors["Slant"] << " " <<
totalTimer.getElapsedTimeInMilliSec() << endl;
//objectPassiveTransformation.setIdentity();
if ( actualTrialMode == STIMULUSMODE )
{
objectPassiveTransformation = getPassiveMatrix();
matrixFile << setw(6) << left <<
trialNumber << " " ;
for ( int i=0; i<3; i++)
matrixFile << objectPassiveTransformation.matrix().row(i) << " " ;
matrixFile << endl;
}
if ( actualTrialMode == STIMULUSMODE )
{
vector< Vector3d> projPoints = stimDrawer.projectStimulusPoints(objectActiveTransformation,headEyeCoords.getRigidStart().getFullTransformation(),cam,focalDistance,screen,Vector3d(0,0,0),false,false);
MatrixXd a1toa6 = stimDrawer.computeOpticFlow(projPoints, focalDistance, elapsedFrameTime/1000);
flowsFile << trialNumber << " " << a1toa6.transpose() << endl;
}
}
writeContinuosDataFile();
}
