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