void initOptotrak()
{ optotrak.setTranslation(calibration);
optotrak.setMarkersToSave(vlist_of<unsigned int>(1)(2)(3)(4)(17)(18));
if ( optotrak.init(18,NULL,NULL,NULL) != 0)
{ cleanup();
exit(0);
}
boost::thread optothread( &Optotrak::startRecording, &optotrak);
optothread.detach();
}
void drawInfo()
{
glPushMatrix();
glClearColor(0.0,0.0,0.0,1.0);
glMatrixMode (GL_PROJECTION);
glPushMatrix ();
glLoadIdentity ();
gluOrtho2D(0, SCREEN_WIDTH, 0, SCREEN_HEIGHT);
glMatrixMode (GL_MODELVIEW);
glPushMatrix();
glLoadIdentity();
markers = optotrak.getAllPoints();
switch ( headCalibrationDone )
{
case 0:
{ allVisibleHead = isVisible(markers[17]) && isVisible(markers[18]) && isVisible(markers[1]) && isVisible(markers[2]) && isVisible(markers[3]) ;
if ( allVisibleHead )
glClearColor(0.0,1.0,0.0,1.0); //green light
else
glClearColor(1.0,0.0,0.0,1.0); //red light
}
break;
case 1:
case 2:
//case 3:
{
glPrintText(10, SCREEN_HEIGHT-40,SCREEN_WIDTH,SCREEN_HEIGHT, "EL " + stringify<int>(eyeLeft.x() ) + " " + stringify<int>(eyeLeft.y() ) + " " + stringify<int>(eyeLeft.z()) );
glPrintText(10, SCREEN_HEIGHT-60,SCREEN_WIDTH,SCREEN_HEIGHT, "ER " + stringify<int>(eyeRight.x() ) + " " + stringify<int>(eyeRight.y() ) + " " + stringify<int>(eyeRight.z()) );
glPrintText(10, SCREEN_HEIGHT-80,SCREEN_WIDTH,SCREEN_HEIGHT, "EC" + stringify<int>(cyclopeanEye.x())+" " + stringify<int>(cyclopeanEye.y())+" " + stringify<int>(cyclopeanEye.z()));
glPrintText(10, SCREEN_HEIGHT-100,SCREEN_WIDTH,SCREEN_HEIGHT, "Dist " + stringify<int>(cyclopeanEye.z()-focalDistance));
glPrintText(10, SCREEN_HEIGHT-120,SCREEN_WIDTH,SCREEN_HEIGHT, "PITCH " + stringify<int>(toDegrees(eulerAngles.getPitch())));
glPrintText(10, SCREEN_HEIGHT-140,SCREEN_WIDTH,SCREEN_HEIGHT, "YAW " + stringify<int>(toDegrees(eulerAngles.getYaw())));
glPrintText(10, SCREEN_HEIGHT-160,SCREEN_WIDTH,SCREEN_HEIGHT, "ROLL " + stringify<int>(toDegrees(eulerAngles.getRoll())));
glPrintText(10, SCREEN_HEIGHT-180,SCREEN_WIDTH,SCREEN_HEIGHT, "Press SPACEBAR to calibrate again or ENTER to confirm calibration.");
glPrintText(10, SCREEN_HEIGHT-200,SCREEN_WIDTH,SCREEN_HEIGHT, "Delta " + stringify<int>(eyeRight.z()- eyeCalibration.z()));
Vector3d angles = headEyeCoords.getRigidStart().getFullTransformation().rotation().eulerAngles(0,1,2);
glPrintText(10, SCREEN_HEIGHT-220,SCREEN_WIDTH,SCREEN_HEIGHT, "YAW " + stringify<int>(toDegrees(eulerAngles.getYaw())));
glPrintText(10, SCREEN_HEIGHT-240,SCREEN_WIDTH,SCREEN_HEIGHT, "PITCH " + stringify<int>(toDegrees(eulerAngles.getPitch())));
if ( !passiveMode )
glPrintText(10, SCREEN_HEIGHT-260,SCREEN_WIDTH,SCREEN_HEIGHT, "Active");
else
glPrintText(10, SCREEN_HEIGHT-260,SCREEN_WIDTH,SCREEN_HEIGHT, "Passive");
glPrintText(10, SCREEN_HEIGHT-280,SCREEN_WIDTH,SCREEN_HEIGHT, "OBJ " + stringify<int>(projPointEyeRight.x() ) + " " + stringify<int>(projPointEyeRight.y() ) + " " + stringify<int>(projPointEyeRight.z()) );
glPrintText(10, SCREEN_HEIGHT-300,SCREEN_WIDTH,SCREEN_HEIGHT,"Slant= " + stringify<int>(factors["Slant"]) + " " + stringify<int>((instantPlaneSlant)));
glPrintText(10, SCREEN_HEIGHT-320,SCREEN_WIDTH,SCREEN_HEIGHT, "GlassesL" + stringify<int>(markers[17].x() ) + " " + stringify<int>(markers[17].y() ) + " " + stringify<int>(markers[17].z()) );
glPrintText(10, SCREEN_HEIGHT-340,SCREEN_WIDTH,SCREEN_HEIGHT, "GlassesR" + stringify<int>(markers[18].x() ) + " " + stringify<int>(markers[18].y() ) + " " + stringify<int>(markers[18].z()) );
}
break;
}
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
glPopMatrix();
glPopMatrix();
// end if ( headCalibrationDone )
}
void update(int value)
{ optotrak.updatePoints();
markers = optotrak.getAllPoints();
headEyeCoords.update(markers[1],markers[2],markers[3]);
modelCoordinates.update(markers[10],markers[19],markers[20]);
if ( isVisible(markers[20]+markers[19]+markers[10]) )
{
rigidCurrent.setRigidBody(markers[20], markers[19],modelCoordinates.getFinger() );
rigidAux.computeTransformation(rigidCurrent,true); // true the scaling matrix
}
eyeLeft = headEyeCoords.getLeftEye();
eyeRight = headEyeCoords.getRightEye();
glutPostRedisplay();
glutTimerFunc(TIMER_MS, update, 0);
}
void checkBounds()
{
double maxOscTime = str2num<double>(parameters.find("MaxOscillationTime"));
double minOscTime = str2num<double>(parameters.find("MinOscillationTime"));
double maxHeadShift = str2num<double>(parameters.find("MaxHeadShift"));
if ( (headCalibrationDone > 2) && (trialMode!=PROBEMODE) )
{
conditionInside = abs(eyeRight.z()-eyeCalibration.z()) < maxHeadShift;
if ( !( conditionInside || (!wasInside) ) )
{
signs.pop_back();
signs.push_front( (eyeRight.z()-eyeCalibration.z()) > 0 );
rythmMaker.stop();
if ( signs.front() != signs.back() )
{
if ( trialMode == STIMULUSMODE )
{
boost::thread beepOkThread(beepOk);
beepOkThread.detach();
sumOutside++;
}
double timeElapsed = rythmMaker.getElapsedTimeInMilliSec();
if ( (timeElapsed > minOscTime ) && (timeElapsed < maxOscTime ) && (trialMode==FIXATIONMODE) )
{
boost::thread beepOkThread(beepOk);
beepOkThread.detach();
if ( factors["Onset"]*optotrak.getAllVelocities()[3].z() > 0 )
sumOutside++;
}
if ( (timeElapsed < minOscTime ) && ( trialMode==FIXATIONMODE ) )
{
boost::thread tweeterThread(tweeter);
tweeterThread.detach();
sumOutside=0;
}
if ( (timeElapsed > maxOscTime ) && ( trialMode==FIXATIONMODE ) )
{
boost::thread wooferThread(woofer);
wooferThread.detach();
sumOutside=0;
}
}
rythmMaker.start();
}
wasInside = conditionInside;
}
if ( (stimOutside) && (trialMode==STIMULUSMODE) )
{
outsideBeep();
stimOutside=false;
trialMode=CALIBRATIONMODE;
sumOutside=0;
}
}
/*************************** FUNCTIONS ***********************************/
void cleanup()
{ // Close all the file streams
markersFile.close();
anglesFile.close();
trajFile.close();
matrixFile.close();
stimFile.close();
// Stop the optotrak
optotrak.stopCollection();
}
void drawInfo()
{
markers = optotrak.getAllPoints();
if ( headCalibrationDone==0 )
{
allVisibleHead = isVisible(markers[17]) && isVisible(markers[18]) && isVisible(markers[1]) && isVisible(markers[2]) && isVisible(markers[3]) ;
allVisibleModel = (markers[10]+markers[14]+markers[19]+markers[20]).norm() < 1E20;
if ( allVisibleHead )
glClearColor(0.0,1.0,0.0,1.0); //green light
else
glClearColor(1.0,0.0,0.0,1.0); //red light
}
GLText text;
text.init(SCREEN_WIDTH,SCREEN_HEIGHT,glWhite,GLUT_BITMAP_HELVETICA_18);
text.enterTextInputMode();
text.draw("Head is visible? " + stringify<int>(allVisibleHead));
text.draw("Full model is visible? " + stringify<bool>(allVisibleModel));
text.draw( stringify<double>((markers[14]-modelCoordinates.getFinger()).norm()));
text.leaveTextInputMode();
}
void update(int value)
{
// Conta i cicli di presentazione dello stimolo
if ( (sumOutside > str2num<int>(parameters.find("StimulusCycles")) ) && (trialMode == STIMULUSMODE) )
{
sumOutside=0;
trialMode++;
trialMode=trialMode%4;
}
if (conditionInside && (sumOutside*2 > str2num<int>(parameters.find("FixationCycles"))) && (trialMode ==FIXATIONMODE ) )
{
sumOutside=0;
trialMode++;
trialMode=trialMode%4;
stimulusDuration.start();
}
if ( trialMode == STIMULUSMODE )
stimulusFrames++;
if ( trialMode == FIXATIONMODE )
stimulusFrames=0;
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(center));
camPassive.init(screenPassive);
camPassive.setDrySimulation(true);
camPassive.setEye(eyeRight);
objectPassiveTransformation = ( camPassive.getModelViewMatrix()*objectActiveTransformation );
// Coordinates picker
markers = optotrak.getAllPoints();
if ( isVisible(markers[1]) && isVisible(markers[2]) && isVisible(markers[3]) )
headEyeCoords.update(markers[1],markers[2],markers[3]);
Affine3d active = headEyeCoords.getRigidStart().getFullTransformation();
eulerAngles.init( headEyeCoords.getRigidStart().getFullTransformation().rotation() );
eyeLeft = headEyeCoords.getLeftEye();
eyeRight = headEyeCoords.getRightEye();
cyclopeanEye = (eyeLeft+eyeRight)/2.0;
// Projection of view normal on the focal plane
Vector3d directionOfSight = (active.rotation()*Vector3d(0,0,-1)).normalized();
Eigen::ParametrizedLine<double,3> lineOfSightRight = Eigen::ParametrizedLine<double,3>::Through( eyeRight , eyeRight+directionOfSight );
Eigen::ParametrizedLine<double,3> lineOfSightLeft = Eigen::ParametrizedLine<double,3>::Through( eyeLeft, eyeLeft+directionOfSight );
double lineOfSightRightDistanceToFocalPlane = lineOfSightRight.intersection(focalPlane);
double lineOfSightLeftDistanceToFocalPlane = lineOfSightLeft.intersection(focalPlane);
//double lenghtOnZ = (active*(center-eyeCalibration )+eyeRight).z();
projPointEyeRight = lineOfSightRightDistanceToFocalPlane *(directionOfSight)+ (eyeRight);
projPointEyeLeft= lineOfSightLeftDistanceToFocalPlane * (directionOfSight) + (eyeLeft);
// second projection the fixation point computed with z non constant but perfectly parallel to projPointEyeRight
lineOfSightRightDistanceToFocalPlane= (( active.rotation()*(center)) - eyeRight).norm();
Vector3d secondProjection = lineOfSightRightDistanceToFocalPlane *(directionOfSight)+ (eyeRight);
if ( !zOnFocalPlane )
projPointEyeRight=secondProjection ;
// Compute the translation to move the eye in order to avoid shear components
Vector3d posAlongLineOfSight = (headEyeCoords.getRigidStart().getFullTransformation().rotation())*(eyeRight -eyeCalibration);
switch ( (int)factors["Translation"] )
{
case -1:
case -2:
translationFactor.setZero();
if ( trialMode == STIMULUSMODE )
projPointEyeRight=center;
break;
case 0:
translationFactor.setZero();
break;
case 1:
translationFactor = factors["TranslationConstant"]*Vector3d(posAlongLineOfSight.z(),0,0);
break;
case 2:
translationFactor = factors["TranslationConstant"]*Vector3d(0,posAlongLineOfSight.z(),0);
break;
}
if ( passiveMode )
initProjectionScreen(0,headEyeCoords.getRigidStart().getFullTransformation()*Translation3d(Vector3d(0,0,focalDistance)));
else
initProjectionScreen(focalDistance,Affine3d::Identity());
checkBounds();
/**** Save to file part ****/
// Markers file save the used markers and time-depending experimental variable to a file
// (Make sure that in passive experiment the list of variables has the same order)
markersFile << trialNumber << " " << headCalibrationDone << " " << trialMode << " " ;
markersFile <<markers[1].transpose() << " " << markers[2].transpose() << " " << markers[3].transpose() << " " << markers[17].transpose() << " " << markers[18].transpose() << " " ;
markersFile << factors["Tilt"] << " " <<
factors["Slant"] << " " <<
factors["Translation"] << " " <<
factors["Onset"] << " " <<
factors["TranslationConstant"] <<
endl;
ofstream outputfile;
outputfile.open("data.dat");
outputfile << "Subject Name: " << parameters.find("SubjectName") << endl;
outputfile << "Passive matrix:" << endl << objectPassiveTransformation.matrix() << endl;
outputfile << "Yaw: " << toDegrees(eulerAngles.getYaw()) << endl <<"Pitch: " << toDegrees(eulerAngles.getPitch()) << endl;
outputfile << "EyeLeft: " << headEyeCoords.getLeftEye().transpose() << endl;
outputfile << "EyeRight: " << headEyeCoords.getRightEye().transpose() << endl << endl;
outputfile << "Slant: " << instantPlaneSlant << endl;
outputfile << "(Width,Height) [px]: " << getPlaneDimensions().transpose() << " " << 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 << "Trial remaining: " << trial.getRemainingTrials()+1 << endl;
outputfile << "Last response: " << probeAngle << endl;
// Here we save plane projected width and height
// now rewind the file
outputfile.clear();
outputfile.seekp(0,ios::beg);
// Write down frame by frame the trajectories and angles of eyes and head
if ( trialMode == STIMULUSMODE && headCalibrationDone > 2 )
{
trajFile << setw(6) << left <<
trialNumber << " " <<
stimulusFrames << " " <<
eyeRight.transpose() << endl;
anglesFile << setw(6) << left <<
trialNumber << " " <<
stimulusFrames << " " <<
toDegrees(eulerAngles.getPitch()) << " " <<
toDegrees(eulerAngles.getRoll()) << " " <<
toDegrees(eulerAngles.getYaw()) << " " <<
instantPlaneSlant << endl;
matrixFile << setw(6) << left <<
trialNumber << " " <<
stimulusFrames << " " ;
for (int i=0; i<3; i++)
matrixFile << objectPassiveTransformation.matrix().row(i) << " " ;
matrixFile << endl;
// Write the 13 special extremal points on stimFile
stimFile << setw(6) << left <<
trialNumber << " " <<
stimulusFrames << " " ;
double winx=0,winy=0,winz=0;
for (PointsRandIterator iRand = redDotsPlane.specialPointsRand.begin(); iRand!=redDotsPlane.specialPointsRand.end(); ++iRand)
{ Point3D *p=(*iRand);
Vector3d v = objectActiveTransformation*Vector3d( p->x, p->y, p->z);
gluProject(v.x(),v.y(),v.z(), (&cam)->getModelViewMatrix().data(), (&cam)->getProjectiveMatrix().data(), (&cam)->getViewport().data(), &winx,&winy,&winz);
stimFile << winx << " " << winy << " " << winz << " ";
}
stimFile << endl;
}
glutPostRedisplay();
glutTimerFunc(TIMER_MS, update, 0);
}
void cleanup()
{ optotrak.stopCollection();
}
