void handleKeypress(unsigned char key, int x, int y)
{
switch (key)
{ //Quit program
case 'q':
case 27:
{
optotrak.stopCollection();
Sleep(1000);
exit(0);
}
break;
case 'i':
infoDrawn=!infoDrawn;
break;
case ' ':
{
// Here we record the head shape - coordinates of eyes and markers, but centered in (0,0,0)
if ( headCalibrationDone==0 && allVisiblePatch )
{
headEyeCoords.init(markers[17].p,markers[18].p, markers[1].p,markers[2].p,markers[3].p,interoculardistance );
headCalibrationDone=1;
beepOk();
break;
}
// Second calibration, you must look a fixed fixation point
if ( headCalibrationDone==1 && allVisiblePatch)
{
headEyeCoords.init( headEyeCoords.getP1().p,headEyeCoords.getP2().p, markers[1].p, markers[2].p,markers[3].p,interoculardistance );
headCalibrationDone=2;
beepOk();
break;
}
if ( headCalibrationDone==2 && allVisiblePatch )
{ headEyeCoords.init( headEyeCoords.getP1().p,headEyeCoords.getP2().p, markers[1].p, markers[2].p,markers[3].p,interoculardistance );
beepOk();
break;
}
}
break;
// Enter key: press to make the final calibration
case 13:
{
if ( canCalibrate && headCalibrationDone == 2 && allVisiblePatch )
{
headEyeCoords.init( headEyeCoords.getP1().p,headEyeCoords.getP2().p, markers[1].p, markers[2].p,markers[3].p,interoculardistance );
headCalibrationDone=3;
infoDrawn=false;
for (int i=0; i<3; i++)
beepOk();
}
}
break;
}
}
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 drawInfo()
{
if ( !infoDrawn )
return;
GLText text;
text.init(SCREEN_WIDTH, SCREEN_HEIGHT,glWhite);
text.enterTextInputMode();
string strfactors;
for ( map<string,double>::iterator iter = factors.begin(); iter!=factors.end(); ++iter)
{
text.draw( (iter->first) + "= " + util::stringify<int>( (int )(iter->second)) );
}
switch ( headCalibrationDone )
{
case 0:
{
if ( allVisibleHead )
text.draw("OK! Press SPACEBAR");
else
text.draw("---");
}
break;
case 1:
case 2:
{
text.draw( "EL " + stringify<int>(eyeLeft.x() ) + " " + stringify<int>(eyeLeft.y() ) + " " + stringify<int>(eyeLeft.z()) );
text.draw( "ER " + stringify<int>(eyeRight.x() ) + " " + stringify<int>(eyeRight.y() ) + " " + stringify<int>(eyeRight.z()) );
text.draw("EC " + stringify<int>(cyclopeanEye.x())+" " + stringify<int>(cyclopeanEye.y())+" " + stringify<int>(cyclopeanEye.z()));
text.draw("Dist " + stringify<int>(cyclopeanEye.z()-focalDistance+60.0));
text.draw(" ");
text.draw( "PITCH " + stringify<int>(toDegrees(headEyeCoords.getPitch())));
text.draw( "YAW " + stringify<int>(toDegrees(headEyeCoords.getYaw())));
text.draw( "ROLL " + stringify<int>(toDegrees(headEyeCoords.getRoll())));
}
break;
}
text.leaveTextInputMode();
}
void writeContinuosDataFile()
{
ofstream outputfile;
outputfile.open("data.dat");
outputfile << "Yaw: " << toDegrees(headEyeCoords.getYaw()) << endl <<"Pitch: " << toDegrees(headEyeCoords.getPitch()) << endl;
outputfile << "EyeLeft: " << eyeLeft.transpose() << endl;
outputfile << "EyeRight: " << eyeRight.transpose() << endl << endl;
outputfile << "Factors:" << endl;
for (map<string,double>::iterator iter=factors.begin(); iter!=factors.end(); ++iter)
{
outputfile << "\t\t" << iter->first << "= " << iter->second << endl;
}
outputfile << "Trials done: " << trialNumber << endl;
if (trialNumber!=0)
outputfile << "Last response (right=0, left=1): " << response << endl;
outputfile.clear();
outputfile.seekp(0,ios::beg);
}
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 checkBounds()
{ if ( fingerCalibrationDone==3 && headCalibrationDone==3 )
{
switch ( trialMode )
{ /*
Vector3d trueIndex = indexCoords.getP1().p;
Vector3d trueThumb = thumbCoords.getP1().p;
case STIMULUSMODE:
{ // Check if the index tip stays inside the stimulus
Vector2d indexOnPlane = Vector2d(trueIndex.x(),trueIndex.z());
Vector2d hapticRodCenterOnPlane = Vector2d(hapticRodCenter.x(), hapticRodCenter.z()+10);
double stimHeight=0, stimRadius=0;
switch ( block.at("Phase") )
{ case 0:
case 2:
{ stimHeight = str2num<double>(parameters.find("TestStimulusHeight") );
stimRadius = str2num<double>(parameters.find("TestStimulusRadius") );
}
break;
case 1:
{ stimHeight = str2num<double>(parameters.find("AdaptStimulusHeight") );
stimRadius = str2num<double>(parameters.find("AdaptStimulusRadius") );
}
break;
}
bool yInside = (trueIndex.y()-(hapticRodCenter.y()+stimulusRodOffsetY))<(stimHeight/2) && (trueIndex.y()-(hapticRodCenter.y()+stimulusRodOffsetY))> (-stimHeight/2) ;
if ( ( (indexOnPlane - hapticRodCenterOnPlane).norm() <= stimRadius ) && yInside )
indexInside[0]=true;
else
indexInside[0]=false;
}
break;
*/
case HANDONSTARTMODE: //handsonstart
{ // Define a sphere around physicalRigidBodyTip in which the thumb must stay
bool indexVisible = isVisible(markers.at(7).p) && isVisible(markers.at(8).p) && isVisible(markers.at(9).p);
if ( indexVisible )
{ bool triggerCondition=false;
if ( parameters.find("Task") == "MSE" && (block.at("Phase") != 1) )
triggerCondition = (globalTimer.getElapsedTimeInMilliSec() < 1500);
else
triggerCondition=((indexCoords.getP1().p - physicalRigidBodyTip).norm() < ballRadius);
if ( triggerCondition ) //5 centimeters
{ indexInside[0]=true;
if ( indexInside[1]==false)
globalTimer.start();
}
else
{ indexInside[0]=false;
double distance = (indexCoords.getP1().p - physicalRigidBodyTip).norm();
// IMPORTANTE: controlla che il dito sia uscito ma esclude i casi in cui il dito esce da invisibile e diventa visibile fuori dalla palla... una soglia decente è 10 mm
if ( indexInside[1]==true && distance<(ballRadius+10) ) //è appena uscito
{ globalTimer.start();
trialMode=STIMULUSMODE;
}
}
}
}
break;
}
}
}
void handleKeypress(unsigned char key, int x, int y)
{ switch (key)
{ //Quit program
case 'q':
case 27:
{ cleanup();
exit(0);
}
break;
case ' ':
{ // Here we record the head shape - coordinates of eyes and markers, but centered in (0,0,0)
if ( headCalibrationDone==0 && allVisibleHead )
{ headEyeCoords.init(markers.at(17).p,markers.at(18).p, markers.at(1).p,markers.at(2).p,markers.at(3).p,interoculardistance );
headCalibrationDone=1;
beepOk();
break;
}
if ( headCalibrationDone==1 && allVisiblePatch )
{
headCalibrationDone=3;
beepOk();
}
}
break;
case 'i':
infoDraw=!infoDraw;
break;
case 'f':
case 'F':
{ // Here we record the finger tip physical markers
if ( allVisiblePlatform && (fingerCalibrationDone==0) )
{ platformIndex=markers.at(16).p;
platformThumb=markers.at(15).p;
fingerCalibrationDone=1;
beepOk();
break;
}
if ( (fingerCalibrationDone==1) && allVisibleFingers )
{ thumbCoords.init(platformThumb, markers.at(11).p, markers.at(12).p, markers.at(13).p);
indexCoords.init(platformIndex, markers.at(7).p, markers.at(8).p, markers.at(9).p );
fingerCalibrationDone=2;
beepOk();
break;
}
if ( fingerCalibrationDone==2 && allVisibleFingers )
{ infoDraw=false;
drawGLScene();
physicalRigidBodyTip = indexCoords.getP1().p;
fingerCalibrationDone=3;
switch ( block.at("Phase") )
{
case 0:
case 2:
{ factors = trial.getNext();
initStimulus(testStimHeight,testStimRadius);
moveScreenAbsolute(factors.at("Distances"),homeFocalDistance,SCREENSPEED);
initProjectionScreen(factors.at("Distances"));
visualRodCenter = Vector3d(0,0,factors.at("Distances"));
hapticRodCenter = rodAway; // keep it away
moveRod(rodAway,RODSPEED);
}
break;
case 1:
{ double zadaptmin = str2num<double>(parameters.find("AdaptZMin"));
double zadaptmax = str2num<double>(parameters.find("AdaptZMax"));
if (str2num<int>(parameters.find("AdaptMoveMonitor"))==1)
visualRodCenter = Vector3d(0,0,mathcommon::unifRand(zadaptmin,zadaptmax));
else
visualRodCenter = Vector3d(0,0,(zadaptmax+zadaptmin)/2);
if ( str2num<int>(parameters.find("AdaptHapticFeedback"))==1 )
hapticRodCenter = visualRodCenter - Vector3d(0,0,adaptOffsets.at(block1TrialNumber));
else
hapticRodCenter = rodAway;
initProjectionScreen( visualRodCenter.z() );
initStimulus(str2num<double>(parameters.find("AdaptStimulusHeight")),str2num<double>(parameters.find("AdaptStimulusRadius")));
moveScreenAbsoluteAsynchronous(visualRodCenter.z(),homeFocalDistance,SCREENSPEED);
moveRod(hapticRodCenter,RODSPEED);
}
break;
}
trialMode = HANDONSTARTMODE;
fingerDistance = (indexCoords.getP1().p-thumbCoords.getP1().p).norm();
beepTrial();
trialFrame=0;
globalTimer.start();
totalTime.start();
break;
}
}
break;
}
}
void drawInfo()
{ if ( infoDraw )
{ glDisable(GL_COLOR_MATERIAL);
glDisable(GL_BLEND);
glDisable(GL_LIGHTING);
GLText text;
if ( gameMode )
text.init(SCREEN_WIDTH,SCREEN_HEIGHT,glWhite,GLUT_BITMAP_HELVETICA_18);
else
text.init(640,480,glWhite,GLUT_BITMAP_HELVETICA_12);
text.enterTextInputMode();
switch ( headCalibrationDone )
{
case 0:
{ if ( allVisibleHead )
text.draw("==== Head Calibration OK ==== Press Spacebar to continue");
else
text.draw("Be visible with the head and glasses");
}
break;
case 1:
case 2:
{ if ( allVisiblePatch )
text.draw("Cyclopean(x,y,z)= " + stringify<int>((eyeRight.x()+eyeLeft.x())/2)+", "+ stringify<int>((eyeRight.y()+eyeLeft.y())/2)+", "+ stringify<int>((eyeRight.z()+eyeLeft.z())/2)+", " );
else
text.draw("Be visible with the patch");
}
break;
case 3: // When the head calibration is done then calibrate the fingers
{ switch ( fingerCalibrationDone )
{
case 0:
text.draw("Press F to record platform markers");
break;
case 1:
text.draw("Move index and thumb on platform markers to record ghost finger tips, then press F");
break;
case 2:
{
text.draw("Move index and thumb to rigidbody tip to define starting position, then press F");
}
break;
}
}
break;
}
if ( headCalibrationDone==3 && fingerCalibrationDone==3 )
{ text.draw("Phase 0 Trial number= " + stringify<int>(block0TrialNumber));
text.draw("Phase 1 Trial number= " + stringify<int>(block1TrialNumber));
text.draw("Phase 2 Trial number= " + stringify<int>(block2TrialNumber));
text.draw("Trial mode " + stringify<int>(trialMode ));
text.draw("DrawingTrialFrame= " + stringify<int>(drawingTrialFrame));
text.draw("IsDrawing?= " + stringify<int>(isDrawing));
text.draw("VisualRod= " + stringify<Eigen::Matrix<double,1,3> >(visualRodCenter.transpose()) );
text.draw("HapticRod= " + stringify<Eigen::Matrix<double,1,3> >(hapticRodCenter.transpose()) );
text.draw("AdaptOffsets= " + stringify< vector<double> >(adaptOffsets) );
text.draw("ThisAdaptOffset= " + stringify< double >(adaptOffsets.at(block1TrialNumber)) );
text.draw("Index= " + stringify< Eigen::Matrix<double,1,3> >(indexCoords.getP1().p ));
text.draw("Thumb= " + stringify< Eigen::Matrix<double,1,3> >(thumbCoords.getP1().p ));
text.draw("GlobTime= " + stringify<int>(globalTimer.getElapsedTimeInMilliSec()));
if ( block.at("Phase")!=1 )
{
text.draw("CurrentFactor= " + stringify<int>(factors.at("Distances")) );
text.draw("Remaining= " + stringify<int>( trial.getRemainingTrials() ));
}
}
text.leaveTextInputMode();
}
}
void handleKeypress(unsigned char key, int x, int y)
{ switch (key)
{ //Quit program
case 'q':
case 27:
{
plato_stop();
exit(0);
}
break;
case ' ':
{ // Here we record the head shape - coordinates of eyes and markers, but centered in (0,0,0)
if ( headCalibrationDone==0 && allVisibleHead )
{ headEyeCoords.init(markers.at(17).p,markers.at(18).p, markers.at(1).p,markers.at(2).p,markers.at(3).p,interoculardistance );
headCalibrationDone=1;
beepOk();
break;
}
if ( headCalibrationDone==1 && allVisiblePatch )
{
headCalibrationDone=3;
RoveretoMotorFunctions::moveObjectAbsolute( platformCalibration, rodTipHome, 3500);
beepOk();
}
}
break;
case 'i':
infoDraw=!infoDraw;
break;
case 'f':
case 'F':
{
if (headCalibrationDone < 3 )
break;
// Here we record the finger tip physical markers
if ( allVisiblePlatform && (fingerCalibrationDone==0) )
{ platformIndex=markers.at(16).p;
platformThumb=markers.at(15).p;
fingerCalibrationDone=1;
beepOk();
break;
}
// Calibrate the index first
if ( (fingerCalibrationDone==1) && allVisibleIndex )
{
indexCoords.init(platformIndex, markers.at(7).p, markers.at(8).p, markers.at(9).p );
rigidStartIndex.setRigidBody(markers.at(7).p-platformIndex,markers.at(8).p-platformIndex,markers.at(9).p-platformIndex);
fingerCalibrationDone=2;
beepOk();
break;
}
// Calibrate the thumb then
if ( (fingerCalibrationDone==2) && allVisibleThumb )
{
rigidStartThumb.setRigidBody(markers.at(11).p-platformThumb,markers.at(12).p-platformThumb,markers.at(13).p-platformThumb);
thumbCoords.init(platformThumb, markers.at(11).p, markers.at(12).p, markers.at(13).p);
fingerCalibrationDone=3;
beepOk();
break;
}
if ( fingerCalibrationDone==3 && allVisibleFingers )
{
fingerCalibrationDone=4;
infoDraw=false;
paintGL();
physicalRigidBodyTip = indexCoords.getP1().p;
trialMode = HANDONSTARTMODE;
fingerDistance = (indexCoords.getP1().p-thumbCoords.getP1().p).norm();
moveStimulusObject(visualStimCenter,3500);
beepTrial();
trialFrame=0;
drawingTrialFrame=0;
globalTimer.start();
plato_write(PLATO_LEFT_RIGHT_OPEN);
break;
}
}
break;
}
}
/**
* @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);
}
}
/**
* @brief drawInfo
*/
void drawInfo()
{ if ( infoDraw )
{ glDisable(GL_COLOR_MATERIAL);
glDisable(GL_BLEND);
glDisable(GL_LIGHTING);
GLText text;
if ( gameMode )
text.init(SCREEN_WIDTH,SCREEN_HEIGHT,glWhite,GLUT_BITMAP_HELVETICA_18);
else
text.init(640,480,glWhite,GLUT_BITMAP_HELVETICA_12);
text.enterTextInputMode();
text.draw("Marker5= " + stringify<Eigen::Matrix<double,1,3> >(markers.at(5).p.transpose()) );
switch ( headCalibrationDone )
{
case 0:
{ if ( allVisibleHead )
text.draw("==== Head Calibration OK ==== Press Spacebar to continue");
else
text.draw("Be visible with the head and glasses");
}
break;
case 1:
case 2:
{ if ( allVisiblePatch )
text.draw("Cyclopean(x,y,z)= " + stringify<int>((eyeRight.x()+eyeLeft.x())/2)+", "+ stringify<int>((eyeRight.y()+eyeLeft.y())/2)+", "+ stringify<int>((eyeRight.z()+eyeLeft.z())/2)+", " );
else
text.draw("Be visible with the patch");
}
break;
case 3: // When the head calibration is done then calibrate the fingers
{ switch ( fingerCalibrationDone )
{
case 0:
text.draw("Press F to record platform markers");
break;
case 1:
text.draw("Move INDEX on platform markers to record ghost finger tips, then press F");
break;
case 2:
text.draw("Move THUMB on platform markers to record ghost finger tips, then press F");
break;
case 3:
text.draw("Move BOTH INDEX AND THUMB to rigidbody tip, then press F");
break;
}
}
break;
}
if ( headCalibrationDone==3 && fingerCalibrationDone==4 )
{ text.draw("Trial mode " + stringify<int>(trialMode ));
text.draw("ThumbProjectedInside " + stringify<int>(thumbProjectedInside));
text.draw("DrawingTrialFrame= " + stringify<int>(drawingTrialFrame));
text.draw("TrialFrame= " + stringify<int>(trialFrame));
text.draw("IsDrawing?= " + stringify<int>(isDrawing));
text.draw("VisualRod= " + stringify<Eigen::Matrix<double,1,3> >(visualStimCenter.transpose()) );
text.draw("Index= " + stringify< Eigen::Matrix<double,1,3> >(indexCoords.getP1().p ));
text.draw("Thumb= " + stringify< Eigen::Matrix<double,1,3> >(thumbCoords.getP1().p ));
text.draw("GlobTime= " + stringify<int>(globalTimer.getElapsedTimeInMilliSec()));
text.draw("TrialTimer= " + stringify<int>(trialTimer.getElapsedTimeInMilliSec()));
text.draw("FingersTime= " + stringify<int>(fingersTimer.getElapsedTimeInMilliSec()));
}
//text.draw("Gain=" + stringify<double>(factors.getCurrent().at("Gain")));
text.leaveTextInputMode();
}
}
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();
}
void handleKeypress(unsigned char key, int x, int y)
{ switch (key)
{ //Quit program
case 'q':
case 27:
{ exit(0);
}
break;
case ' ':
{ // Here we record the head shape - coordinates of eyes and markers, but centered in (0,0,0)
if ( headCalibrationDone==0 && allVisibleHead )
{
headEyeCoords.init(markers.at(18).p-Vector3d(ChinRestWidth,0,0),markers.at(18).p, markers.at(1).p,markers.at(2).p,markers.at(3).p,interoculardistance );
headCalibrationDone=1;
beepOk();
break;
}
if ( headCalibrationDone==1 && allVisiblePatch )
{
headCalibrationDone=3;
beepOk();
}
}
break;
case 'i':
infoDraw=!infoDraw;
break;
case 'f':
case 'F':
{ // Here we record the finger tip physical markers
if ( allVisiblePlatform && (fingerCalibrationDone==0) )
{
platformIndex=markers.at(16).p;
platformThumb=markers.at(15).p;
fingerCalibrationDone=1;
beepOk();
break;
}
if ( (fingerCalibrationDone==1) && allVisibleFingers )
{
thumbCoords.init(platformThumb, markers.at(11).p, markers.at(12).p, markers.at(13).p);
indexCoords.init(platformIndex, markers.at(7).p, markers.at(8).p, markers.at(9).p );
fingerCalibrationDone=2;
// XXX sposto l'oggetto a home per permettere le dita di calibrarsi
// TriesteMotorFunctions::homeObject(3500); //// XXX commentato: A TS no servi
beepOk();
break;
}
if ( fingerCalibrationDone==2 && allVisibleFingers )
{ infoDraw=false;
drawGLScene();
physicalRigidBodyTip = indexCoords.getP1().p;
fingerCalibrationDone=3;
switch ( block.at("Phase") )
{
case 0:
{
cerr << "Key 721 case 0" << endl;
factors = trial.getNext();
deltaXRods = mathcommon::unifRand(str2num<double>(parameters.find("DeltaXMin")),str2num<double>(parameters.find("DeltaXMax")));
initStimulus(factors.at("StimulusHeight"),deltaXRods,factors.at("RelDepth"));
// moveMonitorAbsolute(factors.at("Distances"),homeFocalDistance,SCREENSPEED);
initProjectionScreen(factors.at("Distances"));
visualRodCenter = Vector3d(0,0,factors.at("Distances"));
hapticRodCenter = rodAway; // keep it away
// moveRod(rodAway,RODSPEED);
cerr << "Key 730 case 0" << endl;
}
beepOk();
break;
case 2:
{ factors = trial.getNext();
deltaXRods = mathcommon::unifRand(str2num<double>(parameters.find("DeltaXMin")),str2num<double>(parameters.find("DeltaXMax")));
initStimulus(factors.at("StimulusHeight"),deltaXRods,factors.at("RelDepth"));
// moveMonitorAbsolute(factors.at("Distances"),homeFocalDistance,SCREENSPEED);
initProjectionScreen(factors.at("Distances"));
visualRodCenter = Vector3d(0,0,factors.at("Distances"));
hapticRodCenter = rodAway; // keep it away
// moveRod(rodAway,RODSPEED);
}
break;
case 1:
{
cerr << "Key 735 case 1" << endl;
double zadaptmin = str2num<double>(parameters.find("AdaptZMin"));
double zadaptmax = str2num<double>(parameters.find("AdaptZMax"));
if (str2num<int>(parameters.find("AdaptMoveMonitor"))==1)
visualRodCenter = Vector3d(0,0,mathcommon::unifRand(zadaptmin,zadaptmax));
else
visualRodCenter = Vector3d(0,0,(zadaptmax+zadaptmin)/2);
if ( str2num<int>(parameters.find("AdaptHapticFeedback"))==1 )
hapticRodCenter = visualRodCenter - Vector3d(0,0,adaptOffsets.at(block1TrialNumber));
else
hapticRodCenter = rodAway;
initProjectionScreen( visualRodCenter.z() );
initStimulus(str2num<double>(parameters.find("AdaptStimulusHeight")),adaptStimRadius*2,adaptStimRadius*2);
moveMonitorAbsoluteAsynchronous(visualRodCenter.z(),homeFocalDistance,SCREENSPEED);
moveRod(hapticRodCenter,RODSPEED);
}
break;
}
cerr << "Key 756 " << endl;
trialMode = HANDONSTARTMODE;
cerr << "Key 758 " << endl;
fingerDistance = (indexCoords.getP1().p-thumbCoords.getP1().p).norm();
cerr << "Key 760 " << endl;
beepTrial();
cerr << "Key 762 " << endl;
trialFrame=0;
globalTimer.start();
totalTime.start();
break;
}
}
break;
}
}
