/*** 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 )
}
/**
* @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 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()
{
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);
}
void update(int value)
{
frameTimer.start();
// Read the experiment from file, if the file is finished exit suddenly
if ( inputStream.eof() )
{ exit(0);
}
if ( isReading )
{ // This reads a line (frame) in inputStream
readline(inputStream, trialNumber, headCalibration, trialMode, pointMatrix );
headEyeCoords.update(pointMatrix.col(0),pointMatrix.col(1),pointMatrix.col(2));
Affine3d active = headEyeCoords.getRigidStart().getFullTransformation();
eulerAngles.init( headEyeCoords.getRigidStart().getFullTransformation().rotation() );
eyeLeft = headEyeCoords.getLeftEye();
eyeRight= headEyeCoords.getRightEye();
//cerr << eyeRight.transpose() << endl;
cyclopeanEye = (eyeLeft+eyeRight)/2.0;
if ( trialMode == STIMULUSMODE )
stimulusFrames++;
if ( trialMode == FIXATIONMODE )
stimulusFrames=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 );
double lineOfSightRightDistanceToFocalPlane = lineOfSightRight.intersection(focalPlane);
//double lenghtOnZ = (active*(center-eyeCalibration )+eyeRight).z();
projPointEyeRight = lineOfSightRightDistanceToFocalPlane *(directionOfSight)+ (eyeRight);
// 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);
projPointEyeRight=secondProjection ;
// Compute the translation to move the eye in order to avoid share components
Vector3d posAlongLineOfSight = (headEyeCoords.getRigidStart().getFullTransformation().rotation())*(eyeRight -eyeCalibration);
// GENERATION OF PASSIVE MODE.
// HERE WE MOVE THE SCREEN TO FACE THE OBSERVER's EYE
if ( passiveMode )
{ initProjectionScreen(0, headEyeCoords.getRigidStart().getFullTransformation()*Translation3d(center));
}
else
initProjectionScreen(focalDistance, Affine3d::Identity());
if ( trialMode == STIMULUSMODE )
{
// IMPORTANT Reset the previous status of transformations
objectActiveTransformation[0].setIdentity();
objectActiveTransformation[1].setIdentity();
// PLANE 0 Transformation QUELLO CHE STA SOTTO
alpha = atan( eyeRight.x()/abs(projPointEyeRight.z()) );
if ( overallTilt )
{
instantPlaneSlant = alphaMultiplier*alpha+toRadians(-factors.at("DeltaSlant")-factors.at("StillPlaneSlant"));
AngleAxis<double> aa0( instantPlaneSlant,Vector3d::UnitY());
objectActiveTransformation[0]*=aa0;
double planesYOffset = factors.at("PlanesCentersYDistance")*(whichPlaneDrawUp ? 1 : -1);
objectActiveTransformation[0].translation() = Vector3d(0,planesYOffset,focalDistance);
// PLANE 1 Transformation QUELLO CHE STA SOPRA
AngleAxis<double> aa1(-toRadians(factors.at("StillPlaneSlant")),Vector3d::UnitY());
objectActiveTransformation[1]*=aa1;
objectActiveTransformation[1].translation() = Vector3d(0,-planesYOffset,focalDistance);
}
else
{
instantPlaneSlant = alphaMultiplier*alpha+toRadians(factors.at("DeltaSlant")+factors.at("StillPlaneSlant"));
AngleAxis<double> aa0( instantPlaneSlant,Vector3d::UnitY());
objectActiveTransformation[0]*=aa0;
double planesYOffset = factors.at("PlanesCentersYDistance")*(whichPlaneDrawUp ? 1 : -1);
objectActiveTransformation[0].translation() = Vector3d(0,planesYOffset,focalDistance);
// PLANE 1 Transformation QUELLO CHE STA SOPRA
AngleAxis<double> aa1(toRadians(factors.at("StillPlaneSlant")),Vector3d::UnitY());
objectActiveTransformation[1]*=aa1;
objectActiveTransformation[1].translation() = Vector3d(0,-planesYOffset,focalDistance);
}
objectPassiveTransformation[0] = ( cam.getModelViewMatrix()*objectActiveTransformation[0] );
objectPassiveTransformation[1] = ( cam.getModelViewMatrix()*objectActiveTransformation[1] );
//cout << toDegrees(instantPlaneSlant) << endl;
// **************** COMPUTE THE OPTIC FLOWS **************************
// 1) Project the points to screen by computing their coordinates on focalPlane in passive (quite complicate, see the specific method)
// *********** FOR THE MOVING PLANE *************
vector<Vector3d> projPointsMovingPlane = stimDrawer[0].projectStimulusPoints(objectActiveTransformation[0],headEyeCoords.getRigidStart().getFullTransformation(),cam,focalDistance, screen, eyeCalibration,passiveMode,false);
// 2) Get the angles formed by stimulus and observer
// updating with the latest values
Vector3d oldAlphaMoving = flowsAnglesAlphaMoving,oldBetaMoving=flowsAnglesBetaMoving;
// alpha is the "pitch" angle, beta is the "yaw" angle
// Here me must use the points 4,5,8 of the stimulus
flowsAnglesAlphaMoving(0) = ( atan2(projPointsMovingPlane[4].x(), abs(focalDistance) ) );
flowsAnglesAlphaMoving(1) = ( atan2(projPointsMovingPlane[5].x(), abs(focalDistance) ) );
flowsAnglesAlphaMoving(2) = ( atan2(projPointsMovingPlane[8].x(), abs(focalDistance) ) );
flowsAnglesBetaMoving(0) = ( atan2(projPointsMovingPlane[4].y(), abs(focalDistance) ) );
flowsAnglesBetaMoving(1) = ( atan2(projPointsMovingPlane[5].y(), abs(focalDistance) ) );
flowsAnglesBetaMoving(2) = ( atan2(projPointsMovingPlane[8].y(), abs(focalDistance) ) );
// 3) Fill the matrix of derivatives
MatrixXd angVelocitiesMoving(6,1);
angVelocitiesMoving(0) = flowsAnglesAlphaMoving(0)-oldAlphaMoving(0);
angVelocitiesMoving(1) = flowsAnglesBetaMoving(0)-oldBetaMoving(0);
angVelocitiesMoving(2) = flowsAnglesAlphaMoving(1)-oldAlphaMoving(1);
angVelocitiesMoving(3) = flowsAnglesBetaMoving(1)-oldBetaMoving(1);
angVelocitiesMoving(4) = flowsAnglesAlphaMoving(2)-oldAlphaMoving(2);
angVelocitiesMoving(5) = flowsAnglesBetaMoving(2)-oldBetaMoving(2);
angVelocitiesMoving /= ((double)TIMER_MS/(double)1000);
// 4) Fill the coefficient matrix, to solve the linear system
MatrixXd coeffMatrixMoving(6,6);
coeffMatrixMoving <<
1, flowsAnglesAlphaMoving(0), flowsAnglesBetaMoving(0), 0, 0, 0,
0, 0, 0, 1,flowsAnglesAlphaMoving(0),flowsAnglesBetaMoving(0),
1, flowsAnglesAlphaMoving(1), flowsAnglesBetaMoving(1), 0, 0, 0,
0, 0, 0, 1,flowsAnglesAlphaMoving(1),flowsAnglesBetaMoving(1),
1, flowsAnglesAlphaMoving(2), flowsAnglesBetaMoving(2), 0, 0, 0,
0, 0, 0, 1,flowsAnglesAlphaMoving(2),flowsAnglesBetaMoving(2)
;
// 5) Solve the linear system by robust fullPivHouseholderQR decomposition (see Eigen for details http://eigen.tuxfamily.org/dox/TutorialLinearAlgebra.html )
MatrixXd velocitiesMoving = coeffMatrixMoving.colPivHouseholderQr().solve(angVelocitiesMoving);
// 6) Write the output to file flowsFileMoving
flowsFileMoving << fixed << trialNumber << "\t" << //1
stimulusFrames << " " <<
factors.at("DeltaSlant")<< " " <<
factors.at("StillPlaneSlant") << " " <<
overallTilt << " " <<
projPointsMovingPlane[4].transpose() << " " <<
projPointsMovingPlane[5].transpose() << " " <<
projPointsMovingPlane[8].transpose() << " " <<
angVelocitiesMoving.transpose() << " " <<
velocitiesMoving.transpose() << endl;
// ********************* FLOWS FOR THE STILL PLANE **************
// Here we must repeat the same things for the still plane
vector<Vector3d> projPointsStillPlane = stimDrawer[1].projectStimulusPoints(objectActiveTransformation[1],headEyeCoords.getRigidStart().getFullTransformation(),cam,focalDistance, screen, eyeCalibration,passiveMode,false);
// 2) Get the angles formed by stimulus and observer
// updating with the latest values
Vector3d oldAlphaStill = flowsAnglesAlphaStill,oldBetaStill=flowsAnglesBetaStill;
// alpha is the "pitch" angle, beta is the "yaw" angle
// Here me must use the points 4,5,8 of the stimulus
flowsAnglesAlphaStill(0) = ( atan2(projPointsStillPlane[4].x(), abs(focalDistance) ) );
flowsAnglesAlphaStill(1) = ( atan2(projPointsStillPlane[5].x(), abs(focalDistance) ) );
flowsAnglesAlphaStill(2) = ( atan2(projPointsStillPlane[8].x(), abs(focalDistance) ) );
flowsAnglesBetaStill(0) = ( atan2(projPointsStillPlane[4].y(), abs(focalDistance) ) );
flowsAnglesBetaStill(1) = ( atan2(projPointsStillPlane[5].y(), abs(focalDistance) ) );
flowsAnglesBetaStill(2) = ( atan2(projPointsStillPlane[8].y(), abs(focalDistance) ) );
// 3) Fill the matrix of derivatives
MatrixXd angVelocitiesStill(6,1);
angVelocitiesStill(0) = flowsAnglesAlphaStill(0)-oldAlphaStill(0);
angVelocitiesStill(1) = flowsAnglesBetaStill(0)-oldBetaStill(0);
angVelocitiesStill(2) = flowsAnglesAlphaStill(1)-oldAlphaStill(1);
angVelocitiesStill(3) = flowsAnglesBetaStill(1)-oldBetaStill(1);
angVelocitiesStill(4) = flowsAnglesAlphaStill(2)-oldAlphaStill(2);
angVelocitiesStill(5) = flowsAnglesBetaStill(2)-oldBetaStill(2);
angVelocitiesStill /= ((double)TIMER_MS/(double)1000);
// 4) Fill the coefficient matrix, to solve the linear system
MatrixXd coeffMatrixStill(6,6);
coeffMatrixStill <<
1, flowsAnglesAlphaStill(0), flowsAnglesBetaStill(0), 0, 0, 0,
0, 0, 0, 1,flowsAnglesAlphaStill(0),flowsAnglesBetaStill(0),
1, flowsAnglesAlphaStill(1), flowsAnglesBetaStill(1), 0, 0, 0,
0, 0, 0, 1,flowsAnglesAlphaStill(1),flowsAnglesBetaStill(1),
1, flowsAnglesAlphaStill(2), flowsAnglesBetaStill(2), 0, 0, 0,
0, 0, 0, 1,flowsAnglesAlphaStill(2),flowsAnglesBetaStill(2)
;
// 5) Solve the linear system by robust fullPivHouseholderQR decomposition (see Eigen for details http://eigen.tuxfamily.org/dox/TutorialLinearAlgebra.html )
MatrixXd velocitiesStill = coeffMatrixStill.colPivHouseholderQr().solve(angVelocitiesStill);
// 6) Write the output to file flowsFileStill
flowsFileStill << fixed << trialNumber << "\t" << // 1
stimulusFrames << " " << // 2
factors.at("DeltaSlant")<< " " << // 3
factors.at("StillPlaneSlant") << " " << // 4
overallTilt << " " <<
projPointsStillPlane[4].transpose() << " " << // 5,6,7
projPointsStillPlane[5].transpose() << " " << // 8,9,10
projPointsStillPlane[8].transpose() << " " << // 11,12,13
angVelocitiesStill.transpose() << " " << // 14, 15, 16, 17, 18, 19
velocitiesStill.transpose() << endl; // 20, 21, 22, 23, 24, 25
// **************** END OF OPTIC FLOWS COMPUTATION
}
/*
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 << "Factors:" << endl;
for (map<string,double>::iterator iter=factors.begin(); iter!=factors.end(); ++iter)
{ outputfile << "\t\t" << iter->first << "= " << iter->second << endl;
}
*/
}
if ( trialMode == PROBEMODE )
isReading=false;
glutPostRedisplay();
glutTimerFunc(TIMER_MS, update, 0);
}
/**
* @brief drawStimulus
*/
void drawStimulus()
{
//////////////////////////////
// Update stimuli
double oscillationAmplitude=50.0;
double oscillationPeriod=100.0;
static double timeFrame=0.0;
timeFrame+=1.0;
if ( !trial.getCurrent().at("StimulusAnchored") )
{
// FISSO
glPushMatrix();
glLoadIdentity();
Vector3d translationFactor(0,0,focalDistance);
double instantPlaneSlant = trial.getCurrent().at("Slant");
switch ( (int) trial.getCurrent().at("Tilt") )
{
case 0:
{
glTranslated( translationFactor.x(), translationFactor.y(), translationFactor.z());
glRotated( -instantPlaneSlant ,0,1,0);
glScaled(1/sin(toRadians( -90-trial.getCurrent().at("Slant"))),1,1); //backprojection phase
break;
}
case 90:
{
glTranslated( translationFactor.x(), translationFactor.y(), translationFactor.z());
glRotated( -instantPlaneSlant ,1,0,0);
glScaled(1,1/sin(toRadians( -90-trial.getCurrent().at("Slant") )),1); //backprojection phase
break;
}
case 180:
{
glTranslated( translationFactor.x(), translationFactor.y(), translationFactor.z());
glRotated( -instantPlaneSlant ,0,1,0);
glScaled(1/sin(toRadians( -90-trial.getCurrent().at("Slant") )),1,1); //backprojection phase
break;
}
case 270:
{
glTranslated( translationFactor.x(), translationFactor.y(), translationFactor.z());
glRotated( -instantPlaneSlant ,1,0,0);
glScaled(1,1/sin(toRadians( -90-trial.getCurrent().at("Slant") )),1); //backprojection phase
break;
}
}
glGetDoublev(GL_MODELVIEW_MATRIX,objectActiveTransformation.data());
stimDrawer.draw();
glPopMatrix();
}
else
{
// ANCORATO
glPushMatrix();
glLoadMatrixd( (headEyeCoords.getRigidStart().getFullTransformation()*Translation3d(Vector3d(0,0,focalDistance))).data() );
glGetDoublev(GL_MODELVIEW_MATRIX,objectActiveTransformation.data());
stimDrawer.draw();
glPopMatrix();
}
// Draw the occlusor
if ( useCircularOcclusor )
{
double maskRadius=parameters.get("CircularOcclusorRadius");
glPushMatrix();
glLoadIdentity();
Vector3d projCoords;
if ( trial.getCurrent().at("StimulusAnchored") )
projCoords << project( (headEyeCoords.getRigidStart().getFullTransformation()*Translation3d(Vector3d(0,0,focalDistance))).translation() );
else
projCoords << project( Vector3d(0,0,focalDistance) );
circleMask( projCoords.x(), projCoords.y(), maskRadius );
glPopMatrix();
}
bool passiveMode=false;
stimDrawer.projectStimulusPoints( objectActiveTransformation, headEyeCoords.getRigidStart().getFullTransformation(), cam, focalDistance, screen, eyeCalibration, passiveMode, true);
}
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 update(int value)
{
// Timing things
if ( trialMode != PROBEMODE )
{
oldvariable = variable;
variable = -factors["Onset"]*mathcommon::trianglewave( timeFrame , factors["StimulusDuration"]/(TIMER_MS*factors["FollowingSpeed"]) );
timeFrame+=1;
bool isInside = ((projPoint - Vector3d(0,0,focalDistance) ).norm()) <= (circleRadius);
// permette di avanzare se siamo in stimulusMode e lo stimolo ha fatto un semiciclo ( una passata da dx a sx o da su a giù )
bool nextMode = ( sumOutside == 0 ) && (trialMode==STIMULUSMODE);
if ( isInside && ( sumOutside > stimulusEmiCycles ) || (nextMode) )
{
sumOutside=-1;
advanceTrial();
//cerr << "stim time= " << stimulusTimer.getElapsedTimeInMilliSec() << endl;
}
}
// Simulate head translation
// Coordinates picker
markers[1] = Vector3d(0,0,0);
markers[2] = Vector3d(0,10,0);
markers[3] = Vector3d(0,0,10);
Vector3d translation(0,0,0);
switch ( (int) factors["Anchored"] )
{
case 0:
translation = Vector3d((circleRadius+1)*variable,0,0);
break;
case 1:
translation = Vector3d((circleRadius+1)*variable,0,0);
break;
case 2:
translation = Vector3d(0,(circleRadius+1)*variable,0);
break;
}
markers[1]+=translation;
markers[2]+=translation;
markers[3]+=translation;
headEyeCoords.update(markers[1],markers[2],markers[3]);
eyeLeft = headEyeCoords.getLeftEye();
eyeRight = headEyeCoords.getRightEye();
fixationPoint = (headEyeCoords.getRigidStart().getFullTransformation() * ( Vector3d(0,0,focalDistance) ) );
// Projection of view normal on the focal plane
pline = Eigen::ParametrizedLine<double,3>::Through(eyeRight,fixationPoint);
projPoint = pline.intersection(focalPlane)*((fixationPoint - eyeRight).normalized()) + eyeRight;
checkBounds();
glutPostRedisplay();
glutTimerFunc(TIMER_MS, update, 0);
}
void update(int value)
{ // Read the experiment from file, if the file is finished exit suddenly
if ( inputStream.eof() )
{ cleanup();
exit(0);
}
if ( isReading )
{ // This reads a line (frame) in inputStream
readline(inputStream, trialNumber, headCalibration, trialMode, pointMatrix );
headEyeCoords.update(pointMatrix.col(0),pointMatrix.col(1),pointMatrix.col(2));
Affine3d active = headEyeCoords.getRigidStart().getFullTransformation();
eulerAngles.init( headEyeCoords.getRigidStart().getFullTransformation().rotation() );
eyeLeft = headEyeCoords.getLeftEye();
eyeRight= headEyeCoords.getRightEye();
cyclopeanEye = (eyeLeft+eyeRight)/2.0;
if ( trialMode == STIMULUSMODE )
stimulusFrames++;
if ( trialMode == FIXATIONMODE )
stimulusFrames=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 share components
Vector3d posAlongLineOfSight = (headEyeCoords.getRigidStart().getFullTransformation().rotation())*(eyeRight -eyeCalibration);
// GENERATION OF PASSIVE MODE.
// HERE WE MOVE THE SCREEN TO FACE THE OBSERVER's EYE
if ( passiveMode )
{
initProjectionScreen(0, headEyeCoords.getRigidStart().getFullTransformation()*Translation3d(center));
}
else
initProjectionScreen(focalDistance, Affine3d::Identity());
objectPassiveTransformation = ( cam.getModelViewMatrix()*objectActiveTransformation );
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 << "Factors:" << endl;
for (map<string,double>::iterator iter=factors.begin(); iter!=factors.end(); ++iter)
{
outputfile << "\t\t" << iter->first << "= " << iter->second << endl;
}
}
if ( trialMode == PROBEMODE )
isReading=false;
glutPostRedisplay();
glutTimerFunc(TIMER_MS, update, 0);
}
void drawFixation()
{
switch ( headCalibrationDone )
{
case 1:
// Fixed stimulus
glColor3fv(glWhite);
glDisable(GL_BLEND);
glPointSize(5);
glBegin(GL_POINTS);
glVertex3d(0,0,focalDistance);
glEnd();
glPointSize(1);
break;
case 2:
// Fixed stimulus + projected points
glColor3fv(glWhite);
glDisable(GL_BLEND);
glPointSize(5);
glBegin(GL_POINTS);
glVertex3d(0,0,focalDistance);
glColor3fv(glRed);
glVertex3dv(projPointEyeRight.data());
glColor3fv(glBlue);
glVertex3d(eyeRight.x(),eyeRight.y(),focalDistance);
glEnd();
glPointSize(1);
// Draw the calibration circle
glColor3fv(glWhite);
break;
case 3:
{
// DRAW THE FIXATION POINT
double eyeToCenterAngleX= toDegrees(atan(eyeRight.x()/(-focalDistance-eyeRight.z()) ));
double eyeToCenterAngleY= toDegrees(atan(eyeRight.y()/(-focalDistance-eyeRight.z()) ));
double projPointAngleX = toDegrees( atan( (projPointEyeRight.x()-eyeRight.x())/abs(projPointEyeRight.z())));
double maxAllowedTranslationYaw = str2num<double>(parameters.find("MaxAllowedTranslationYaw"));
Vector3d stimulusCenter(0,0,0);
Matrix3d objrotation = Matrix3d::Identity();
// IMPORTANT Reset the previous status of transformation
objectActiveTransformation.setIdentity();
switch ( (int) factors["Rotation"] )
{
case 2:
{
objrotation = (AngleAxis<double>(eulerAngles.getYaw()*factors["FollowingSpeed"], Vector3d::UnitY())
*AngleAxis<double>(eulerAngles.getPitch(), Vector3d::UnitX())).toRotationMatrix();
instantPlaneSlant = toDegrees(eulerAngles.getYaw())*factors["RotationSpeed"]+factors["Slant"];
stimulusCenter = objrotation*Vector3d(0,0,focalDistance)+headEyeCoords.getRigidStart().getFullTransformation().translation();
objectActiveTransformation.linear()=objrotation;
}
break;
case 1:
{
objrotation = (AngleAxis<double>(eulerAngles.getYaw(), Vector3d::UnitY())
*AngleAxis<double>(eulerAngles.getPitch()*factors["FollowingSpeed"], Vector3d::UnitX())).toRotationMatrix();
instantPlaneSlant = toDegrees(eulerAngles.getPitch())*factors["RotationSpeed"]+factors["Slant"];
stimulusCenter = objrotation*Vector3d(0,0,focalDistance)+headEyeCoords.getRigidStart().getFullTransformation().translation();
objectActiveTransformation.linear()=objrotation;
}
break;
case 0:
{
objrotation = (AngleAxis<double>(eulerAngles.getYaw(), Vector3d::UnitY())
*AngleAxis<double>(eulerAngles.getPitch(), Vector3d::UnitX())).toRotationMatrix();
//instantPlaneSlant = eyeRight.x()*factors["RotationSpeed"]/10+factors["Slant"];
instantPlaneSlant = toDegrees( atan(eyeRight.x()/abs(focalDistance+eyeRight.z()) ) )*factors["RotationSpeed"]+factors["Slant"];
stimulusCenter = headEyeCoords.getRigidStart().getFullTransformation().linear()*Vector3d(eyeRight.x()*factors["FollowingSpeed"],eyeRight.y(),eyeRight.z()+focalDistance);
objectActiveTransformation.linear() = objrotation;
}
break;
}
objectActiveTransformation.translation() = stimulusCenter;
Vector3d fixationPointTmp = objectActiveTransformation.translation();
glPushMatrix();
glTranslated(fixationPointTmp.x(),fixationPointTmp.y(),fixationPointTmp.z());
glutSolidSphere(1,10,10);
glPopMatrix();
break;
}
}
}
void drawRedDotsPlane()
{ // Draw the stimulus ( red-dots plane )
glDisable(GL_COLOR_MATERIAL);
glDisable(GL_BLEND);
glDisable(GL_LIGHTING);
Matrix3d objrotation ;
Vector3d stimulusCenter;
double instantPlaneSlant=0;
// IMPORTANT Reset the previous status of transformation
objectActiveTransformation.setIdentity();
switch ( (int) factors["Rotation"] )
{
case 2:
{
objrotation = (AngleAxis<double>(eulerAngles.getYaw()*factors["FollowingSpeed"], Vector3d::UnitY())
*AngleAxis<double>(eulerAngles.getPitch(), Vector3d::UnitX())).toRotationMatrix();
instantPlaneSlant = toDegrees(eulerAngles.getYaw())*factors["RotationSpeed"]+factors["Slant"];
stimulusCenter = objrotation*Vector3d(0,0,focalDistance)+headEyeCoords.getRigidStart().getFullTransformation().translation();
objectActiveTransformation.linear()=objrotation;
}
break;
case 1:
{
objrotation = (AngleAxis<double>(eulerAngles.getYaw(), Vector3d::UnitY())
*AngleAxis<double>(eulerAngles.getPitch()*factors["FollowingSpeed"], Vector3d::UnitX())).toRotationMatrix();
instantPlaneSlant = toDegrees(eulerAngles.getPitch())*factors["RotationSpeed"]+factors["Slant"];
stimulusCenter = objrotation*Vector3d(0,0,focalDistance)+headEyeCoords.getRigidStart().getFullTransformation().translation();
objectActiveTransformation.linear()=objrotation;
}
break;
case 0:
{
objrotation = (AngleAxis<double>(eulerAngles.getYaw(), Vector3d::UnitY())
*AngleAxis<double>(eulerAngles.getPitch(), Vector3d::UnitX())).toRotationMatrix();
//instantPlaneSlant = eyeRight.x()*factors["RotationSpeed"]/10+factors["Slant"];
instantPlaneSlant = toDegrees( atan(eyeRight.x()/abs(focalDistance+eyeRight.z()) ) )+factors["Slant"];
stimulusCenter = headEyeCoords.getRigidStart().getFullTransformation().linear()*Vector3d(eyeRight.x()*factors["FollowingSpeed"],eyeRight.y(),eyeRight.z()+focalDistance);
objectActiveTransformation.linear() = objrotation;
}
break;
}
objectActiveTransformation.translation() = stimulusCenter;
//cerr << instantPlaneSlant << endl;
glPushMatrix(); // PUSH MATRIX
glLoadIdentity();
glMultMatrixd(objectActiveTransformation.data());
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;
}
stimDrawer.draw();
glPopMatrix(); // POP MATRIX
}
