int DexVirtualTracker::Update( void ) {
int i = 0;
int exit_status = 0;
/*
* In the real system we measure the actual position of the manipulandum
* using the call UpdateManipulandumPosition();
* Here we simulate the measured movement of the hand.
*/
double t_pos[3];
t_pos[X] = targets->targetPosition[targets->lastTargetOn][X] + 25.0;
t_pos[Y] = targets->targetPosition[targets->lastTargetOn][Y];
t_pos[Z] = targets->targetPosition[targets->lastTargetOn][Z];
switch ( simulated_movement ) {
case TARGETED_PROTOCOL: // The manipulandum moves magically toward the most recently lit target.
case COLLISION_PROTOCOL:
for ( i = 0; i < 3; i++ ) {
manipulandumPosition[i] += ATTRACTION_TIME_CONSTANT *
( t_pos[i] - manipulandumPosition[i] );
}
break;
case OSCILLATION_PROTOCOL: // The manipulandum oscillates around the center target.
if ( TimerElapsedTime( &oscillate_timer ) < 5.0 ) {
for ( i = 0; i < 3; i++ ) {
manipulandumPosition[i] += ATTRACTION_TIME_CONSTANT * 2 *
( t_pos[i] - manipulandumPosition[i] );
}
}
else if ( TimerElapsedTime( &oscillate_timer ) < 15.0 ){
manipulandumPosition[X] = targets->targetPosition[targets->nTargets / 2][X] + 25;
manipulandumPosition[Y] = targets->targetPosition[targets->nTargets / 2][Y] + 50.0 * sin( 2 * PI * TimerElapsedTime( &oscillate_timer ) );
manipulandumPosition[Z] = targets->targetPosition[targets->nTargets / 2][Z];
}
else {
for ( i = 0; i < 3; i++ ) {
manipulandumPosition[i] += ATTRACTION_TIME_CONSTANT * 2 *
( targets->targetPosition[targets->nTargets / 2][i] - manipulandumPosition[i] );
}
}
break;
}
if ( acquisition_on ) {
for ( int j = 0; j < 3; j++ ) recordedPosition[ nAcqFrames * 3 + j] = manipulandumPosition[j];
if ( nAcqFrames < nFrames ) nAcqFrames++;
}
return( 0 );
}
int main( int argc, char *argv[] ) {
int arg;
int use_tracker = FALSE;
int use_isense = FALSE;
int use_udp = FALSE;
int orientation_only = FALSE;
IsenseDataPacket iSenseData;
UDPDataPacket udpData;
int data_available = NO;
for ( arg = 1; arg < argc; arg++ ) {
if ( !strcmp( argv[arg], "-full" ) ) {
width = 640;
height = 480;
border = false;
fullscreen = true;
stereo = false;
}
else if ( !strcmp( argv[arg], "-hmd" ) ) {
width = 1280;
height = 480;
border = false;
fullscreen = true;
stereo = true;
// HMDScreen( HMD_STEREO );
}
else if ( !strcmp( argv[arg], "-svga" ) ) {
width = 2048;
height = 768;
border = false;
fullscreen = true;
stereo = true;
HMDScreen( HMD_STEREO );
}
else if ( !strcmp( argv[arg], "-nVisL" ) ) {
fprintf( stderr, "LowRes nVis\n" );
width = 2048;
height = 768;
border = false;
fullscreen = true;
stereo = true;
// HMDScreen( HMD_STEREO );
}
else if ( !strcmp( argv[arg], "-nVis" ) ) {
width = 2560;
height = 1024;
border = false;
fullscreen = true;
stereo = true;
// HMDScreen( HMD_STEREO );
}
else if ( !strcmp( argv[arg], "-noborder" ) ) border = FALSE;
else if ( !strcmp( argv[arg], "-tracker" ) ) use_tracker = TRUE;
else if ( !strcmp( argv[arg], "-isense" ) ) use_isense = TRUE;
else if ( !strcmp( argv[arg], "-udp" ) ) use_udp = TRUE;
else if ( !strcmp( argv[arg], "-ori" ) ) orientation_only = TRUE;
else if ( !strcmp( argv[arg], "-sensor" ) ) {
arg++;
if ( arg < argc ) sscanf( argv[arg], "%d", &viewpoint_sensor );
fprintf( stderr, "Using sensor %d.\n", viewpoint_sensor );
}
else closure_record = argv[arg];
}
fprintf( stderr, "Closure record: %s\n", closure_record );
/* Start up optical tracker. */
if ( use_tracker ) {
SetupTracker();
SensorSetHysteresis( 1, 2.0, 0.5 ); // mm and degrees
/*
* The simulated tracker goes through 7 phases.
* The first is stationary.
* The next three are translation movements along X,Y and Z, respectively.
* The last three are rotations around Z, Y and X, respectively.
* The following sets the amplitude of each phase.
* This call will have no effect on real tracker movements.
*/
SimulateSetMovement( viewpoint_sensor, sim_translation, sim_rotation );
/* Shift the nominal viewpoint up, then tilt the view back down to look at the target. */
viewpoint_position[Y] = nominal_head_height;
SimulateSetLocation( viewpoint_sensor, viewpoint_position );
viewpoint_orientation[Y][Y] = viewpoint_orientation[Z][Z] = cos( radians( nominal_head_tilt ) );
viewpoint_orientation[Y][Z] = sin( radians( nominal_head_tilt ) ) ;
viewpoint_orientation[Z][Y] = - viewpoint_orientation[Y][Z];
SimulateSetOrientation( viewpoint_sensor, viewpoint_orientation );
SimulateSetLocation( object_sensor, object_position );
SimulateSetMovement( object_sensor, sim_translation, sim_rotation );
}
if ( use_isense || use_udp ) {
if ( UDPTrackerInitClient( &trkr, NULL, DEFAULT_PORT ) ) {
MessageBox( NULL, "Error opening socket.", "Isense UDP", MB_OK );
use_isense = NO;
}
}
/*
* Define a viewing projection with:
* 45° vertical field-of-view - horizontal fov will be determined by window aspect ratio.
* 60 mm inter-pupilary distance - the units don't matter to OpenGL, but all the dimensions
* that I give for the model room here are in mm.
* 100.0 to 10000.0 depth clipping planes - making this smaller would improve the depth resolution.
*/
viewpoint = new Viewpoint( 6.0, 45.0, 10.0, 10000.0);
int x = 100, y = 100;
/*
* Create window.
*/
window = new OpenGLWindow();
window->Border = border; // Remove borders for an HMD display.
window->FullScreen = fullscreen;
if ( window->Create( NULL, argv[0], 0, 0, width, height ) ) {
/*
* Create sets the new window to be the active window.
* Setup the lights and materials for that window.
*/
glDefaultLighting();
glDefaultMaterial();
// wall_texture->Define();
}
window->Activate();
window->SetKeyboardCallback( keyboard_callback );
// Shade model
glEnable(GL_TEXTURE_2D); // Enable Texture Mapping ( NEW )
glEnable(GL_LIGHTING);
glShadeModel(GL_SMOOTH); // Enable Smooth Shading
glClearDepth(1.0f); // Depth Buffer Setup
glEnable(GL_DEPTH_TEST); // Enables Depth Testing
glDepthFunc(GL_LEQUAL); // The Type Of Depth Testing To Do
glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST); // Really Nice Perspective Calculations
create_objects();
TimerSet( &frame_timer, 10.0 );
frame_counter = 0;
for ( double angle = 0.0; true; angle += 5.0 ) {
if ( TimerTimeout( &frame_timer )) {
fprintf( stderr, "Frame rate: %f\n", (double) frame_counter / TimerElapsedTime( &frame_timer ) );
if ( use_isense ) {
UDPTrackerGetIsenseData( &trkr, &iSenseData );
printf("Isense Quarternion %7.3f %7.3f %7.3f %7.3f %.3f\n",
iSenseData.Station[0].orientation[0],
iSenseData.Station[0].orientation[1],
iSenseData.Station[0].orientation[2],
iSenseData.Station[0].orientation[3],
iSenseData.Station[0].timestamp );
}
TimerStart( &frame_timer );
frame_counter = 0;
}
if ( use_tracker ) {
data_available = !GetSensorPositionOrientation( viewpoint_sensor, YES, viewpoint_position, viewpoint_orientation );
if ( data_available ) {
if ( !orientation_only ) viewpoint->SetPosition( viewpoint_position );
viewpoint->SetOrientation( viewpoint_orientation );
}
}
else if ( use_isense ) {
data_available = !( UDPTrackerGetIsenseData( &trkr, &iSenseData ));
if ( data_available ) {
isense_to_matrix( iSenseData.Station[0].orientation, viewpoint_orientation );
isense_to_vector( iSenseData.Station[0].position, viewpoint_position );
scale_vector( 10.0, viewpoint_position, viewpoint_position );
if ( !orientation_only ) viewpoint->SetPosition( viewpoint_position );
viewpoint->SetOrientation( viewpoint_orientation );
}
}
else if ( use_udp ) {
data_available = !( UDPTrackerGetData( &trkr, &udpData ));
if ( data_available ) {
quaternion_to_matrix( udpData.Station[0].orientation, viewpoint_orientation );
copy_vector( udpData.Station[0].position, viewpoint_position );
scale_vector( 10.0, viewpoint_position, viewpoint_position );
if ( !orientation_only ) viewpoint->SetPosition( viewpoint_position );
viewpoint->SetOrientation( viewpoint_orientation );
}
}
else {
data_available = data_available;
}
if ( use_tracker ) {
data_available = !GetSensorPositionOrientation( object_sensor, YES, object_position, object_orientation );
object->SetPosition( object_position );
object->SetOrientation( object_orientation );
}
else if ( use_isense && data_available ) {
isense_to_matrix( iSenseData.Station[1].orientation, object_orientation );
isense_to_vector( iSenseData.Station[1].position, object_position );
scale_vector( 10.0, object_position, object_position );
object->SetOrientation( object_orientation );
if ( !orientation_only ) object->SetPosition( object_position );
}
else {
object->SetOrientation( angle, j_vector );
object->SetPosition( object_position );
}
window->Clear();
if ( stereo ) {
viewpoint->Apply( window, LEFT_EYE );
render();
viewpoint->Apply( window, RIGHT_EYE );
render();
}
else {
viewpoint->Apply( window, CYCLOPS );
render();
}
window->Swap();
if ( ! window->RunOnce() ) break;
frame_counter++;
}
window->Destroy();
if ( use_tracker ) KillTracker();
RevertScreen();
return( 0 );
}
