void initCalibration()
{
// Now wind the input points file while the head calibration isn't done
while ( true )
{ // Here we load the variables needed to keep track of the experiment status
readline(inputStream, trialNumber, headCalibration, trialMode, pointMatrix );
//First phase of calibration (equivalent when spacebar is pressed first time )
if ( (headCalibration== 1) && (headCalibrationDone==0 ))
{ headEyeCoords.init(pointMatrix.col(3),pointMatrix.col(4), pointMatrix.col(0),pointMatrix.col(1),pointMatrix.col(2),interoculardistance );
headCalibrationDone=headCalibration;
//cerr << headCalibrationDone << endl;
}
// Second phase of calibration (equivalent when space bar is pressed second time )
if ( (headCalibration== 2) && (headCalibrationDone==1 ))
{ headEyeCoords.init( headEyeCoords.getP1(),headEyeCoords.getP2(), pointMatrix.col(0),pointMatrix.col(1),pointMatrix.col(2),interoculardistance );
eyeCalibration=headEyeCoords.getRightEye();
headCalibrationDone=headCalibration;
}
// Third and final phase of calibration ( equivalent when spacebar is pressed third time )
if ((headCalibration==3))
{ headEyeCoords.init( headEyeCoords.getP1(),headEyeCoords.getP2(), pointMatrix.col(0),pointMatrix.col(1),pointMatrix.col(2),interoculardistance );
eyeCalibration=headEyeCoords.getRightEye();
headCalibrationDone=3;
break; // exit the while cycle
}
// simulates the update of head and eyes positions
if ( headCalibration==headCalibrationDone)
headEyeCoords.update(pointMatrix.col(0),pointMatrix.col(1),pointMatrix.col(2));
}
}
/**
* @brief paintGL
*/
void paintGL()
{
if (stereo)
{ glDrawBuffer(GL_BACK);
// Draw left eye view
glDrawBuffer(GL_BACK_LEFT);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glClearColor(0.0,0.0,0.0,1.0);
cam.setEye(headEyeCoords.getRightEye());
drawInfo();
drawTrial();
// Draw right eye view
glDrawBuffer(GL_BACK_RIGHT);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glClearColor(0.0,0.0,0.0,1.0);
cam.setEye(headEyeCoords.getLeftEye());
drawInfo();
drawTrial();
glutSwapBuffers();
}
else
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glClearColor(0.0,0.0,0.0,1.0);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
cam.setEye(headEyeCoords.getRightEye());
drawInfo();
drawTrial();
glutSwapBuffers();
}
}
void handleKeypress(unsigned char key, int x, int y)
{ switch (key)
{ //Quit program
case 'q':
case 27:
{ cleanup();
exit(0);
}
break;
case 'i':
interoculardistance+=1;
headEyeCoords.setInterOcularDistance(interoculardistance);
break;
case 'I':
interoculardistance-=1;
headEyeCoords.setInterOcularDistance(interoculardistance);
break;
case 'w':
modelZ-=1;
break;
case 's':
modelZ+=1;
break;
}
}
void initVariables()
{
totalTimer.start();
interoculardistance = str2num<double>(parameters.find("IOD"));
trial.init(parameters);
factors = trial.getNext(); // Initialize the factors in order to start from trial 1
useCircularMask = util::str2num<int>( parameters.find("CircularMask")) == 1 ;
circularMaskRadius = util::str2num<int>(parameters.find("CircularMaskRadius"));
if (useCircularMask)
glEnable(GL_STENCIL_TEST);
else
glDisable(GL_STENCIL_TEST);
fixationDurationInSeconds = util::str2num<double>(parameters.find("AdaptationDurationInSeconds"));
// Imposta stimolo e drawer
redDotsPlane.setNpoints(util::str2num<int>(parameters.find("NumStimulusPoints")));
redDotsPlane.setDimensions(
util::str2num<int>(parameters.find("StimulusEdgeLength")),
util::str2num<int>(parameters.find("StimulusEdgeLength")),0.1);
redDotsPlane.compute();
stimDrawer.initList(&redDotsPlane,glRed);
resetPointStrip();
stimulusDurationInMilliSeconds = util::str2num<double>(parameters.find("StimulusDuration"));
initialAdaptationTimeInSeconds = util::str2num<double>(parameters.find("InitialAdaptationTime"));
initialAdaptationFlowIncrement = util::str2num<double>(parameters.find("InitialAdaptationFlowIncrement"));
stimMotion=SINUSOIDAL_MOTION;
trialMode = INITIALADAPTATION;
headEyeCoords.init(Vector3d(interoculardistance/2,0,0),Vector3d(interoculardistance/2,0,0), Vector3d(0,0,0),Vector3d(0,10,0),Vector3d(0,0,10),interoculardistance );
eyeCalibration=headEyeCoords.getRightEye();
}
void initVariables()
{
interoculardistance = str2num<double>(parameters.find("IOD"));
stimulusEmiCycles= atoi(parameters.find("StimulusEmiCycles").c_str());
trial.init(parameters);
factors = trial.getNext(); // Initialize the factors in order to start from trial 1
if ( atoi(parameters.find("DrawOccluder").c_str()) ==1 )
{
redDotsPlane.setNpoints(500); //XXX controllare densita di distribuzione dei punti
redDotsPlane.setDimensions(200,200,0.1);
}
else
{
redDotsPlane.setNpoints(75); //XXX controllare densita di distribuzione dei punti
redDotsPlane.setDimensions(50,50,0.1);
}
//redDotsPlane.setSlantTilt( factors["Slant"], (int) factors["Tilt"] );
redDotsPlane.compute();
stimDrawer.setStimulus(&redDotsPlane);
stimDrawer.initList(&redDotsPlane);
/** Bound check things **/
signsX.push_back(false);
signsX.push_back(false);
signsY.push_back(false);
signsY.push_back(false);
headEyeCoords.init(Vector3d(-32.5,0,0),Vector3d(32.5,0,0), Vector3d(0,0,0),Vector3d(0,10,0),Vector3d(0,0,10),interoculardistance );
eyeCalibration=headEyeCoords.getRightEye();
model.load("../data/objmodels/occluder.obj");
}
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;
}
break;
case 13:
whichColor=(bool)(rand()%2);
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=3;
beepOk();
RoveretoMotorFunctions::homeObjectAsynchronous(3500);
break;
}
}
break;
case '+':
{
mirrorAngle+=10;
mirrorPlane = Eigen::Hyperplane<double,3>(Vector3d(sin(toRadians(mirrorAngle)),0,cos(toRadians(mirrorAngle))),focalDistance);
cout << mirrorAngle << endl;
}
break;
case '-':
{
mirrorAngle-=10;
mirrorPlane = Eigen::Hyperplane<double,3>(Vector3d(sin(toRadians(mirrorAngle)),0,cos(toRadians(mirrorAngle))),focalDistance);
cout << mirrorAngle << endl;
}
break;
}
}
void update(int value)
{ markers = optotrak.getAllMarkers();
headEyeCoords.update(markers[1].p,markers[2].p,markers[3].p);
eyeLeft = headEyeCoords.getLeftEye();
eyeRight = headEyeCoords.getRightEye();
glutPostRedisplay();
glutTimerFunc(TIMER_MS, update, 0);
}
/*** Online operations ***/
void online_fingers()
{
allVisibleReferenceMarkers = isVisible(markers.at(calibration1).p) && isVisible(markers.at(calibration2).p) && isVisible(markers.at(calibration3).p) && isVisible(markers.at(calibration4).p);
allVisibleIndex = isVisible(markers.at(ind1).p) && isVisible(markers.at(ind2).p) && isVisible(markers.at(ind3).p);
allVisibleThumb = isVisible(markers.at(thu1).p) && isVisible(markers.at(thu2).p) && isVisible(markers.at(thu3).p);
allVisibleFingers = allVisibleIndex && allVisibleThumb;
allVisibleCalibrationMarkers = isVisible(markers.at(indexcal).p) && isVisible(markers.at(thumbcal).p);
if(allVisibleCalibrationMarkers && fingerCalibrationDone == 0)
fingerCalibrationDone = 1;
// fingers coordinates, fingersOccluded and framesOccluded
if ( fingerCalibrationDone >= 3)
{
if(allVisibleIndex)
{
indexCoords.update(markers.at(ind1).p, markers.at(ind2).p, markers.at(ind3).p );
#ifndef SIMULATION
ind = indexCoords.getP1();
#endif
}
if(allVisibleThumb)
{
thumbCoords.update(markers.at(thu1).p, markers.at(thu2).p, markers.at(thu3).p );
#ifndef SIMULATION
thu = thumbCoords.getP1();
#endif
}
}
// what the program checks online during the grasp
if (fingerCalibrationDone==5 )
{
if(experiment)
{
// Write to responseFile
markersFile << fixed <<
parameters.find("SubjectName") << "\t" <<
trialNumber << "\t" <<
timer.getElapsedTimeInMilliSec() << "\t" << //time
frameN << "\t" << //frameN
ind.transpose() << "\t" << //indexXraw, indexYraw, indexZraw
thu.transpose() << "\t" << //thumbXraw, thumbYraw, thumbZraw
fingersOccluded << "\t" << //fingersOccluded
framesOccluded << "\t" << //framesOccluded
iGrasped <<
endl;
}
}
}
void handleKeypress(unsigned char key, int x, int y)
{ switch (key)
{ //Quit program
case 'q':
case 27:
{ cleanup();
exit(0);
}
break;
case '4':
alignmentX-=0.5;
initProjectionScreen(focalDistance,Affine3d::Identity());
break;
case '6':
alignmentX+=0.5;
initProjectionScreen(focalDistance,Affine3d::Identity());
break;
case '2':
alignmentY+=0.5;
initProjectionScreen(focalDistance,Affine3d::Identity());
break;
case '8':
alignmentY-=0.5;
initProjectionScreen(focalDistance,Affine3d::Identity());
break;
case '+':
scaleZ+=0.01;
break;
case '-':
scaleZ-=0.01;
break;
/* case 'a':
rotZ+=0.5;
break;
case 'z':
rotZ-=0.5;
break;
*/
case ' ':
{
if ( headCalibrationDone==0 && allVisibleHead && allVisibleModel )
{
headEyeCoords.init(markers[17],markers[18], markers[1],markers[2],markers[3],interoculardistance );
modelCoordinates.init(markers[14],markers[10],markers[19],markers[20]);
headCalibrationDone=1;
break;
}
}
break;
}
}
void calibration_head(int phase)
{
switch (phase)
{
case 1:
{
headEyeCoords.init(markers[3].p-Vector3d(70,0,0),markers[3].p, markers[10].p,markers[11].p,markers[12].p,interoculardistance );
} break;
case 2:
{
headEyeCoords.init( headEyeCoords.getP1(),headEyeCoords.getP2(), markers[10].p, markers[11].p,markers[12].p,interoculardistance );
} break;
}
}
/**
* @brief idle
*/
void idle()
{
// Update markers
optotrak.updateMarkers();
markers = optotrak.getAllMarkers();
allVisiblePatch = isVisible(markers[1].p) && isVisible(markers[2].p) && isVisible(markers[3].p);
allVisibleHead = isVisible(markers[17].p) && isVisible(markers[18].p) && allVisiblePatch;
headEyeCoords.update(markers.at(1).p,markers.at(2).p,markers.at(3).p);
if ( trialTimer.getElapsedTimeInMilliSec() >= parameters.get("WaitTime") && trialMode==BLACK_MODE )
{
frame=0.0;
trialMode++;
trialTimer.start();
}
if ( trialTimer.getElapsedTimeInMilliSec() >= parameters.get("ProbeTime") && trialMode==PROBE_MODE )
{
frame=0.0;
trialMode++;
trialTimer.start();
}
if ( trialTimer.getElapsedTimeInMilliSec() >= parameters.get("StimulusTime") && trialMode==STIMULUS_MODE )
{
Beep(660,660);
frame=0.0;
trialMode=RESPONSE_MODE;
trialTimer.start();
}
markersFile << fixed << trialNumber << " " << headCalibrationDone << " " << trialMode << " " ;
markersFile << setprecision(3) <<
( isVisible(markers[1].p) ? markers[1].p.transpose() : junk ) << " " <<
( isVisible(markers[2].p) ? markers[2].p.transpose() : junk ) << " " <<
( isVisible(markers[3].p) ? markers[3].p.transpose() : junk ) << " " <<
( isVisible(markers[17].p) ? markers[17].p.transpose() : junk ) << " " <<
( isVisible(markers[18].p) ? markers[18].p.transpose() : junk ) << " " <<
( isVisible(markers[18].p) ? markers[18].p.transpose() : junk ) << " " <<
( isVisible(headEyeCoords.getLeftEye()) ? headEyeCoords.getLeftEye().transpose() : junk ) << " " <<
( isVisible(headEyeCoords.getRightEye()) ? headEyeCoords.getRightEye().transpose() : junk ) << " " ;
markersFile << setprecision(1) <<
trial.getCurrent().at("ZWidth") << " " <<
trial.getCurrent().at("Slant") << " " <<
trial.getCurrent().at("Tilt") << " " <<
trial.getCurrent().at("StimulusAnchored") << " " <<
endl;
}
/*** FUNCTIONS FOR TRIAL MODE DRAWING ***/
void drawCube()
{
glColor3fv(glGreen50);
glPushMatrix();
glLoadIdentity();
glTranslated(projPointEyeRight.x(), projPointEyeRight.y(), projPointEyeRight.z());
glutWireCube(20);
glPopMatrix();
// YELLOW CUBE :
// Fixed in 0,0,-418.5 in active, moving in passive
glColor3fv(glYellow);
glPushMatrix();
glLoadIdentity();
glTranslated(0,0,focalDistance);
glutWireCube(20);
glPopMatrix();
// BLUE CUBE:
// Moving in active, following the eye projection, fixed but rotating in passive
glColor3fv(glBlue);
Affine3d passive = (headEyeCoords.getRigidStart().getFullTransformation())*Translation3d(Vector3d(0,0,focalDistance)-eyeCalibration);
Vector3d cubePassive = passive*Vector3d(0,0,0);
glPushMatrix();
glLoadIdentity();
glTranslated(cubePassive.x(),cubePassive.y(), cubePassive.z());
glutWireCube(20);
glPopMatrix();
}
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 idle()
{
Timer frameTimer; frameTimer.start();
// Timing things
timeFrame+=1;
double oscillationPeriod = factors.at("StimulusDuration")*TIMER_MS;
switch (stimMotion)
{
case SAWTOOTH_MOTION:
periodicValue = oscillationAmplitude*mathcommon::sawtooth(timeFrame,oscillationPeriod);
break;
case TRIANGLE_MOTION:
periodicValue = oscillationAmplitude*mathcommon::trianglewave(timeFrame,oscillationPeriod);
break;
case SINUSOIDAL_MOTION:
periodicValue = oscillationAmplitude*sin(3.14*timeFrame/(oscillationPeriod));
break;
default:
SAWTOOTH_MOTION;
}
timingFile << totalTimer.getElapsedTimeInMilliSec() << " " << periodicValue << endl;
// Simulate head translation
// Coordinates picker
markers[1] = Vector3d(0,0,0);
markers[2] = Vector3d(0,10,0);
markers[3] = Vector3d(0,0,10);
headEyeCoords.update(markers[1],markers[2],markers[3]);
eyeLeft = headEyeCoords.getLeftEye();
eyeRight = headEyeCoords.getRightEye();
Vector3d fixationPoint = (headEyeCoords.getRigidStart().getFullTransformation() * ( Vector3d(0,0,focalDistance) ) );
// Projection of view normal on the focal plane
Eigen::ParametrizedLine<double,3> pline = Eigen::ParametrizedLine<double,3>::Through(eyeRight,fixationPoint);
projPoint = pline.intersection(focalPlane)*((fixationPoint - eyeRight).normalized()) + eyeRight;
stimTransformation.matrix().setIdentity();
stimTransformation.translation() <<0,0,focalDistance;
Timer sleepTimer;
sleepTimer.sleep((TIMER_MS - frameTimer.getElapsedTimeInMilliSec())/2);
}
/*** GRASP ***/
void calibration_fingers(int phase)
{
switch (phase)
{
case 1:
{
index_marker = markers.at(indexcal).p;
thumb_marker = markers.at(thumbcal).p;
}
case 2:
{
indexCoords.init(index_marker, markers.at(ind1).p, markers.at(ind2).p, markers.at(ind3).p);
thumbCoords.init(thumb_marker, markers.at(thu1).p, markers.at(thu2).p, markers.at(thu3).p);
} break;
}
}
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);
}
// Questa funzione e' quella che in background fa tutti i conti matematici, quindi qui devi inserire
// 1) Scrittura su file continua delle coordinate che vuoi salvare
// 2) Estrazione delle coordinate a partire dai corpi rigidi precedentemente definiti vedi ad esempio
// come e' fatto per eyeLeft e eyeRight oppure per thumb ed index
void idle()
{
timeFile << globalTimer.getElapsedTimeInMilliSec() << endl;
optotrak->updateMarkers();
markers = optotrak->getAllMarkers();
// Coordinates picker
allVisiblePatch = isVisible(markers[5].p) && isVisible(markers[6].p) && isVisible(markers[7].p);
headEyeCoords.update(markers[5],markers[6],markers[7]);
eyeLeft = headEyeCoords.getLeftEye();
eyeRight = headEyeCoords.getRightEye();
eyeRight = Vector3d(interoculardistance/2,0,0);
eyeLeft = -eyeRight;
thumb = thumbCoords.getP1();
index = indexCoords.getP1();
}
void drawRedDotsPlane()
{ // Draw the stimulus ( red-dots plane )
glDisable(GL_COLOR_MATERIAL);
glDisable(GL_BLEND);
glDisable(GL_LIGHTING);
// IMPORTANT Reset the previous status of transformation
objectActiveTransformation.setIdentity();
objectActiveTransformation.translation() = projPointEyeRight + translationFactor;
if ((int)factors["Translation"]==-1 || (int)factors["Translation"]==-2 )
objectActiveTransformation.linear().setIdentity();
else
objectActiveTransformation.linear() = (AngleAxis<double>(eulerAngles.getYaw(), Vector3d::UnitY())*AngleAxis<double>(eulerAngles.getPitch(), Vector3d::UnitX())).toRotationMatrix();
glPushMatrix(); // PUSH MATRIX
glLoadIdentity();
glMultMatrixd(objectActiveTransformation.data());
Vector3d posAlongLineOfSight = (headEyeCoords.getRigidStart().getFullTransformation().rotation())*(eyeRight-eyeCalibration);
double argslant = acos( cos(toRadians(factors["Slant"]))*(focalDistance-posAlongLineOfSight.z() )/((focalDistance )));
instantPlaneSlant = toDegrees(argslant);
switch ( (int) factors["Tilt"] )
{
case 0:
glRotated( instantPlaneSlant ,0,1,0);
//objectActiveTransformation*=AngleAxisd( toRadians(-instantPlaneSlant), Vector3d::UnitY() );
glScaled(1/sin(toRadians( -90-factors["Slant"])),1,1); //backprojection phase
break;
case 90:
glRotated( -instantPlaneSlant ,1,0,0);
//objectActiveTransformation*=AngleAxisd( toRadians(-instantPlaneSlant), Vector3d::UnitX() );
glScaled(1,1/sin(toRadians( -90-factors["Slant"] )),1); //backprojection phase
break;
case 180:
glRotated( -instantPlaneSlant ,0,1,0);
//objectActiveTransformation*=AngleAxisd( toRadians(-instantPlaneSlant), Vector3d::UnitY() );
glScaled(1/sin(toRadians( -90-factors["Slant"] )),1,1); //backprojection phase
break;
case 270:
glRotated( instantPlaneSlant ,1,0,0);
//objectActiveTransformation*=AngleAxisd( toRadians(-instantPlaneSlant), Vector3d::UnitX() );
glScaled(1,1/sin(toRadians( -90-factors["Slant"] )),1); //backprojection phase
break;
}
glGetDoublev(GL_MODELVIEW_MATRIX,objectActiveTransformation.data());
BoundChecker stimBoundariesActive(&cam, &redDotsPlane);
BoundChecker stimBoundariesPassive(&camPassive, &redDotsPlane);
stimOutside = ( stimBoundariesActive.checkOutside(objectActiveTransformation) || stimBoundariesPassive.checkOutside(objectActiveTransformation));
stimDrawer.draw();
glPopMatrix(); // POP MATRIX
}
void idle()
{
double deltaT=1E-2;
optotrak.updateMarkers();
markers = optotrak.getAllMarkers();
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);
headEyeCoords.update(markers.at(1).p,markers.at(2).p,markers.at(3).p);
// update thumb coordinates
thumbCoords.update(markers.at(11).p,markers.at(12).p,markers.at(13).p);
// update index coordinates
indexCoords.update(markers.at(7).p, markers.at(8).p, markers.at(9).p);
eyeLeft = headEyeCoords.getLeftEye();
eyeRight = headEyeCoords.getRightEye();
realThumb = thumbCoords.getP1();
realIndex = indexCoords.getP1();
reflectedIndex = getReflected(realIndex,mirrorPlane);
reflectedThumb = getReflected(realThumb,mirrorPlane);
}
// Questa funzione e' quella che in background fa tutti i conti matematici, quindi qui devi inserire
// 1) Scrittura su file continua delle coordinate che vuoi salvare
// 2) Estrazione delle coordinate a partire dai corpi rigidi precedentemente definiti vedi ad esempio
// come e' fatto per eyeLeft e eyeRight oppure per thumb ed index
void idle()
{
timeFile << globalTimer.getElapsedTimeInMilliSec() << endl;
eyeLeft = headEyeCoords.getLeftEye();
eyeRight = headEyeCoords.getRightEye();
eyeRight = Vector3d(interoculardistance/2,0,0);
eyeLeft = -eyeRight;
thumb = thumbCoords.getP1();
index = indexCoords.getP1();
dz = zedge + (jitter/2);
dx = xedge+jitterX;
r = sqrt(dx*dz/4 + dz*dz/4);
}
void calibration_fingers(int phase)
{
switch (phase)
{
case 1:
{
platformIndex=markers[1].p;
platformThumb=markers[2].p;
}
break;
case 2:
{
indexCoords.init(platformIndex, markers.at(13).p, markers.at(14).p, markers.at(16).p );
thumbCoords.init(platformThumb, markers.at(15).p, markers.at(17).p, markers.at(18).p );
}
break;
}
}
/**
* @brief drawFixation
*/
void drawFixation()
{
double circleRadius = parameters.get("MaxCircleRadius"); // millimeters
double zBoundary = parameters.get("MaxZOscillation"); // millimeters
// Projection of view normal on the focal plane
Vector3d directionOfSight = (headEyeCoords.getRigidStart().getFullTransformation().linear()*Vector3d(0,0,-1)).normalized();
Eigen::ParametrizedLine<double,3> lineOfSightRight = Eigen::ParametrizedLine<double,3>::Through( headEyeCoords.getRightEye() , headEyeCoords.getRightEye()+directionOfSight );
Eigen::Hyperplane<double,3> focalPlane = Eigen::Hyperplane<double,3>::Through( Vector3d(1,0,focalDistance), Vector3d(0,1,focalDistance),Vector3d(0,0,focalDistance) );
double lineOfSightRightDistanceToFocalPlane = lineOfSightRight.intersection(focalPlane);
Vector3d opticalAxisToFocalPlaneIntersection = lineOfSightRightDistanceToFocalPlane *(directionOfSight)+ (headEyeCoords.getRightEye());
switch ( headCalibrationDone )
{
case 1:
{
// STIM_FIXED stimulus at (0,0,focalDistance)
glPushAttrib(GL_POINT_BIT);
glColor3fv(glRed);
glPointSize(5);
glBegin(GL_POINTS);
glVertex3d(0,0,focalDistance);
glVertex3d(headEyeCoords.getRightEye().x(),headEyeCoords.getRightEye().y(),focalDistance);
glEnd();
glPopAttrib();
break;
}
case 2:
{
// STIM_FIXED stimulus + projected points
glPushAttrib( GL_ALL_ATTRIB_BITS );
glPointSize(5);
glLineWidth(2);
glBegin(GL_POINTS);
glColor3fv(glRed);
glVertex3d(0,0,focalDistance);
glColor3fv(glBlue);
glVertex3dv(opticalAxisToFocalPlaneIntersection.data());
glColor3fv(glWhite);
glVertex3d(headEyeCoords.getRightEye().x(),headEyeCoords.getRightEye().y(),focalDistance);
glEnd();
double r2EyeRight = pow(headEyeCoords.getRightEye().x(),2)+pow(headEyeCoords.getRightEye().y(),2);
// Draw the calibration circle
if ( pow(opticalAxisToFocalPlaneIntersection.x(),2)+pow(opticalAxisToFocalPlaneIntersection.y(),2) <= circleRadius*circleRadius && abs(headEyeCoords.getRightEye().z()) < zBoundary && r2EyeRight<circleRadius*circleRadius )
{
readyToStart=true;
drawCircle(circleRadius,0,0,focalDistance,glGreen);
}
else
{
drawCircle(circleRadius,0,0,focalDistance,glRed);
}
glPopAttrib();
break;
}
}
}
void calibration_fingers(int phase)
{
switch (phase)
{
case 1:
{
if(allVisibleObject)
{
upperPinMarker=markers[1].p;
upperPin.init(upperPinMarker, markers.at(8).p, markers.at(11).p, markers.at(12).p );
}
} break;
case 2:
{
if(allVisibleObject)
{
lowerPinMarker=markers[2].p;
lowerPin.init(lowerPinMarker, markers.at(8).p, markers.at(11).p, markers.at(12).p );
}
} break;
case 3:
{
indexCoords.init(upperPin.getP1(), markers.at(13).p, markers.at(14).p, markers.at(16).p );
thumbCoords.init(lowerPin.getP1(), markers.at(15).p, markers.at(17).p, markers.at(18).p );
} break;
}
}
// Questa funzione e' quella che in background fa tutti i conti matematici, quindi qui devi inserire
// 1) Scrittura su file continua delle coordinate che vuoi salvare
// 2) Estrazione delle coordinate a partire dai corpi rigidi precedentemente definiti vedi ad esempio
// come e' fatto per eyeLeft e eyeRight oppure per thumb ed index
void idle()
{
timeFile << globalTimer.getElapsedTimeInMilliSec() << endl;
//optotrak->updateMarkers();
//markers = optotrak->getAllMarkers();
// Coordinates picker
//allVisiblePatch = isVisible(markers[5].p) && isVisible(markers[6].p) && isVisible(markers[7].p);
//headEyeCoords.update(markers[5],markers[6],markers[7]);
eyeLeft = headEyeCoords.getLeftEye();
eyeRight = headEyeCoords.getRightEye();
eyeRight = Vector3d(interoculardistance/2,0,0);
eyeLeft = -eyeRight;
thumb = thumbCoords.getP1();
index = indexCoords.getP1();
dz = zedge + (jitter/2);
dx = xedge+jitterX;
r = sqrt(dx*dz/4 + dz*dz/4);
}
// Questa funzione e' quella che in background fa tutti i conti matematici, quindi qui devi inserire
// 1) Scrittura su file continua delle coordinate che vuoi salvare
// 2) Estrazione delle coordinate a partire dai corpi rigidi precedentemente definiti vedi ad esempio
// come e' fatto per eyeLeft e eyeRight oppure per thumb ed index
void idle()
{
//optotrak->updateMarkers();
//markers = optotrak->getAllMarkers();
// Coordinates picker
//allVisiblePatch = isVisible(markers[5].p) && isVisible(markers[6].p) && isVisible(markers[7].p);
//headEyeCoords.update(markers[5],markers[6],markers[7]);
eyeLeft = headEyeCoords.getLeftEye();
eyeRight = headEyeCoords.getRightEye();
eyeRight = Vector3d(interoculardistance/2,0,0);
eyeLeft = -eyeRight;
thumb = thumbCoords.getP1();
index = indexCoords.getP1();
dz = zedge + jitter;
dx = xedge+jitterX;
r = sqrt(dx*dz/4 + dz*dz/4);
frame++;
}
void initVariables()
{
totalTimer.start();
interoculardistance = str2num<double>(parameters.find("IOD"));
trial.init(parameters);
factors = trial.getNext(); // Initialize the factors in order to start from trial 1
// Imposta stimolo e drawer
redDotsPlane.setNpoints(75);
redDotsPlane.setDimensions(50,50,0.1);
redDotsPlane.compute();
stimDrawer.initList(&redDotsPlane,glRed);
// Imposta striscia del fixation e drawer
stripPlane.setNpoints(N_STRIP_POINTS);
stripPlane.setDimensions(STRIP_WIDTH,STRIP_HEIGHT,0.01);
stripPlane.compute();
stripDrawer.initList(&stripPlane,glRed);
stimMotion=SINUSOIDAL_MOTION;
headEyeCoords.init(Vector3d(-32.5,0,0),Vector3d(32.5,0,0), Vector3d(0,0,0),Vector3d(0,10,0),Vector3d(0,0,10),interoculardistance );
eyeCalibration=headEyeCoords.getRightEye();
}
void recordHeadEyeRelativePositions()
{ bool allVisible=false;
for (int i=0; i<10; i++)
{ optotrak.updateMarkers();
markers=optotrak.getAllMarkers();
allVisible= isVisible(markers[17].p) && isVisible(markers[18].p) && isVisible(markers[1].p) && isVisible(markers[2].p) && isVisible(markers[3].p) ;
cerr << "Move the head such that all markers are visible: Trial num " << i << endl;
cin.ignore( std::numeric_limits <std::streamsize> ::max(), '\n' );
cerr << allVisible << endl;
cerr << markers[17].p.transpose() << " " << markers[18].p.transpose() << endl;
if ( allVisible )
{ headEyeCoords.init(markers[17].p,markers[18].p,
markers[1].p,markers[2].p,markers[3].p,interoculardistance);
break;
}
}
}
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();
}
/**
* @brief drawInfo
*/
void drawInfo()
{
if (infoDraw==false)
return;
GLText text(SCREEN_WIDTH,SCREEN_HEIGHT,glWhite);
text.enterTextInputMode();
switch ( headCalibrationDone )
{
case 0:
{
if ( allVisibleHead )
text.draw("PRESS SPACEBAR TO CALIBRATE");
else
text.draw("BE VISIBLE...");
break;
}
case 1:
{
break;
}
}
text.draw("HeadCalibration="+util::stringify<int>(headCalibrationDone));
text.draw("TrialMode=" + util::stringify<int>(trialMode));
text.draw("ReadyToStart="+util::stringify<int>(readyToStart) );
text.draw("RightEye="+util::stringify< Eigen::Matrix<int,1,3> >(headEyeCoords.getRightEye().transpose().cast<int>()) );
/*
if ( !trial.getCurrent().empty() )
{
for ( map<std::string,std::string>::iterator iter = trial.getCurrentAsMapToString().begin(); iter!=trial.getCurrentAsMapToString().end();++iter )
{
text.draw(iter->first + "= " + iter->second);
}
}
*/
text.leaveTextInputMode();
}
void idle()
{
#ifndef SIMULATION
optotrak->updateMarkers();
markers = optotrak->getAllMarkers();
#endif
// Visibility check
allVisiblePlatform = isVisible(markers[1].p) && isVisible(markers[2].p);
allVisibleIndex = isVisible(markers[13].p) && isVisible(markers[14].p) && isVisible(markers[16].p);
allVisibleThumb = isVisible(markers[15].p) && isVisible(markers[17].p) && isVisible(markers[18].p);
allVisibleFingers = allVisibleIndex && allVisibleThumb;
allVisiblePatch = isVisible(markers[10].p) && isVisible(markers[11].p) && isVisible(markers[12].p);
allVisibleHead = allVisiblePatch && isVisible(markers[9].p);
// mirror alignment check
mirrorAlignment = asin(
abs((markers[6].p.z()-markers[7].p.z()))/
sqrt(
pow(markers[6].p.x()-markers[7].p.x(), 2) +
pow(markers[6].p.z()-markers[7].p.z(), 2)
)
)*180/M_PI;
// screen Y alignment check
screenAlignmentY = asin(
abs((markers[19].p.y()-markers[21].p.y()))/
sqrt(
pow(markers[19].p.x()-markers[21].p.x(), 2) +
pow(markers[19].p.y()-markers[21].p.y(), 2)
)
)*180/M_PI;
// screen Z alignment check
screenAlignmentZ = asin(
abs(markers[19].p.z()-markers[20].p.z())/
sqrt(
pow(markers[19].p.x()-markers[20].p.x(), 2) +
pow(markers[19].p.z()-markers[20].p.z(), 2)
)
)*180/M_PI*
abs(markers[19].p.x()-markers[20].p.x())/
(markers[19].p.x()-markers[20].p.x());
// vertical translation (due to physical stimulus being lower than usual settings)
Vtranslation = whereTop.y()-(stimulus_height[whichStimulus]/2);
// eye coordinates
eyeRight = Vector3d(interoculardistance/2,Vtranslation,0);
eyeLeft = Vector3d(-interoculardistance/2,Vtranslation,0);
// fingers coordinates
if ( allVisibleIndex )
{
indexCoords.update(markers[13].p, markers[14].p, markers[16].p );
thumbCoords.update(markers[15].p, markers[17].p, markers[18].p );
fingersOccluded = 0;
}else
{
fingersOccluded=1;
framesOccluded++;
}
// what the program checks online during the actual experiment
// if it's haptic condition
if(condition == 1)
{
if (fingerCalibrationDone==3 )
{
// frames counting
frameN++;
#ifndef SIMULATION
// index coordinates
if(allVisibleIndex)
ind = indexCoords.getP1();
// thumb coordinates
if(allVisibleThumb)
thu = thumbCoords.getP1();
#endif
if(ind.y() > whereTop.y() - 5 && ind.y() < whereTop.y() + 5 && ind.z() < absDepth + 20 && markers[14].v.norm() < 35)
indexOnPlace = true;
else
{
indexOnPlace = false;
timer.start();
}
// Write to responseFile
responseFile << fixed <<
parameters.find("SubjectName") << "\t" <<
interoculardistance << "\t" <<
trialNumber << "\t" <<
timer.getElapsedTimeInMilliSec() << "\t" << //time
frameN << "\t" << //frameN
ind.transpose() << "\t" << //indexXraw, indexYraw, indexZraw
thu.transpose() << "\t" << //thumbXraw, thumbYraw, thumbZraw
eyeRight.transpose() << "\t" << //eyeRXraw, eyeRYraw, eyeRZraw
eyeLeft.transpose() << "\t" << //eyeLXraw, eyeLYraw, eyeLZraw
fingersOccluded << "\t" << //fingersOccluded
framesOccluded << "\t" << //framesOccluded
absDepth <<"\t" <<
trial.getCurrent().second->getCurrentStaircase()->getID() << "\t" <<
trial.getCurrent().second->getCurrentStaircase()->getState() << "\t" <<
trial.getCurrent().second->getCurrentStaircase()->getInversions() << "\t" <<
trial.getCurrent().second->getCurrentStaircase()->getAscending() << "\t" <<
trial.getCurrent().second->getCurrentStaircase()->getStepsDone() << "\t" <<
stimulus_height[first_interval] << "\t" <<
stimulus_height[second_interval] << "\t" <<
markers[4].p.y() - markers[3].p.y() << "\t" <<
responsekey <<
endl;
}
}
}