#include <stdio.h>
|
#include <gtk/gtk.h>
|
#include <gtk/gtkgl.h>
|
#include <GL/gl.h>
|
#include <GL/glu.h>
|
#include "Quaternion.h"
|
#include "struct.h"
|
#include "G4Trk.h"
|
#include "G4Hub.h"
|
#include <string.h>
|
#include <unistd.h>
|
#include <pthread.h>
|
#include "G4Display.h"
|
#include <gdk/gdkkeysyms.h>
|
#include "IncrDlg.h"
|
#include "FilterDlg.h"
|
#include "config.h"
|
|
|
int RunCollect=0;
|
pthread_t thread1;
|
pthread_mutex_t mutex=PTHREAD_MUTEX_INITIALIZER;
|
|
const char* g4_version=VERSION;
|
const char* g4_comment="Graphical Display Program to communicate with the Polhemus G4 Tracker";
|
|
|
int main(int argc,char* argv[]){
|
|
GtkWidget* win,*da,*fileDlg;
|
GdkGLConfig* glConfig;
|
CG4Trk trk;
|
REND_STRUCT rs;
|
char* cfgFile;
|
GtkFileFilter *all,*g4c;
|
|
|
memset(rs.viewTrans,0,sizeof(float)*3);
|
rs.srcScale=rs.senScale=0.5;
|
rs.counter=0;
|
|
gtk_init(&argc,&argv);
|
gtk_gl_init(&argc,&argv);
|
|
// show the images on the buttons
|
g_object_set(gtk_settings_get_default(),"gtk-button_images",TRUE,NULL);
|
|
win=gtk_window_new(GTK_WINDOW_TOPLEVEL);
|
gtk_window_set_title(GTK_WINDOW(win),"G4 Display Application");
|
|
da=gtk_drawing_area_new();
|
gtk_widget_set_size_request(da,900,700);
|
gtk_widget_set_events(da,GDK_EXPOSURE_MASK|GDK_KEY_PRESS_MASK);
|
gtk_container_add(GTK_CONTAINER(win),da);
|
gtk_window_set_position(GTK_WINDOW(win),GTK_WIN_POS_NONE);
|
gtk_widget_show(win);
|
|
glConfig=gdk_gl_config_new_by_mode((GdkGLConfigMode)(GDK_GL_MODE_RGB|GDK_GL_MODE_DEPTH|GDK_GL_MODE_DOUBLE));
|
gtk_widget_set_gl_capability(da,glConfig,NULL,TRUE,GDK_GL_RGBA);
|
|
gtk_widget_show_all(win);
|
gtk_window_maximize(GTK_WINDOW(win));
|
|
|
g_signal_connect(win,"destroy",G_CALLBACK(QuitApp),NULL);
|
g_signal_connect(da,"configure-event",G_CALLBACK(cb_configure),NULL);
|
g_signal_connect(da,"expose-event",G_CALLBACK(cb_expose),&rs);
|
g_signal_connect(win,"key-press-event",G_CALLBACK(cb_keyPress),&rs);
|
|
g_object_set_data(G_OBJECT(da),"tracker",&trk);
|
|
all=gtk_file_filter_new();
|
g4c=gtk_file_filter_new();
|
gtk_file_filter_set_name(all,"All Files");
|
gtk_file_filter_set_name(g4c,"G4 Source Configuration Files (*.g4c)");
|
gtk_file_filter_add_pattern(all,"*");
|
gtk_file_filter_add_pattern(g4c,"*.g4c");
|
fileDlg=gtk_file_chooser_dialog_new("Select a Source Configuration File",NULL,
|
GTK_FILE_CHOOSER_ACTION_OPEN,
|
GTK_STOCK_OPEN,GTK_RESPONSE_ACCEPT,
|
GTK_STOCK_CANCEL,GTK_RESPONSE_CANCEL,NULL);
|
gtk_file_chooser_add_filter(GTK_FILE_CHOOSER(fileDlg),g4c);
|
gtk_file_chooser_add_filter(GTK_FILE_CHOOSER(fileDlg),all);
|
|
|
int resp=gtk_dialog_run(GTK_DIALOG(fileDlg));
|
if (resp==GTK_RESPONSE_CANCEL){
|
gtk_widget_destroy(fileDlg);
|
return -2;
|
}
|
cfgFile=gtk_file_chooser_get_filename(GTK_FILE_CHOOSER(fileDlg));
|
gtk_widget_destroy(fileDlg);
|
resp=trk.Connect(cfgFile);
|
g_free(cfgFile);
|
if (!resp){
|
printf("Error Connecting to G4\n");
|
return -1;
|
}
|
|
usleep(500000);
|
rs.numSrc=trk.GetNumSrc();
|
rs.srcList=new float[rs.numSrc][6];
|
trk.GetSrcLoc(rs.srcList);
|
|
// rs.numHub=trk.GetActHubs();
|
rs.numHub=trk.UpdateHubs();
|
rs.hubList=new CG4Hub[rs.numHub];
|
int* hubIds=new int[rs.numHub];
|
trk.GetHubList(hubIds,rs.numHub);
|
int color_ind=0;
|
for (int i=0;i<rs.numHub;i++){
|
rs.hubList[i].SetId(hubIds[i]);
|
rs.hubList[i].SetColor(triad_colors[color_ind]);
|
color_ind++;
|
if (color_ind>=15)
|
color_ind=0;
|
}
|
delete[] hubIds;
|
|
rs.pTrk=&trk;
|
|
RunCollect=1;
|
// g_timeout_add(8,cb_timer,da);
|
g_timeout_add(REFRESH_RATE,cb_timer,da);
|
|
pthread_create(&thread1,NULL,Collect,&rs);
|
gtk_main();
|
|
delete[] rs.srcList;
|
delete[] rs.hubList;
|
|
return 0;
|
}
|
|
gboolean cb_configure(GtkWidget* w,GdkEventConfigure* e,gpointer g){
|
|
GdkGLDrawable* glDraw;
|
GdkGLContext* glContext;
|
|
glDraw=gtk_widget_get_gl_drawable(w);
|
glContext=gtk_widget_get_gl_context(w);
|
|
gdk_gl_drawable_gl_begin(glDraw,glContext);
|
|
glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
|
glViewport(0,0,(GLsizei)w->allocation.width,(GLsizei)w->allocation.height);
|
glMatrixMode(GL_PROJECTION);
|
glLoadIdentity();
|
gluPerspective(60.0,(float)w->allocation.width/(float)w->allocation.height,0.1,200.0);
|
glMatrixMode(GL_MODELVIEW);
|
glLoadIdentity();
|
|
gdk_gl_drawable_gl_end(glDraw);
|
return TRUE;
|
}
|
|
gboolean cb_expose(GtkWidget* w,GdkEventExpose* e,gpointer g){
|
|
GdkGLDrawable* glDraw;
|
GdkGLContext* glContext;
|
REND_STRUCT* prs=(REND_STRUCT*)g;
|
int refresh_cnt=(int)(1.0/(float)REFRESH_RATE*1000.0); // about every sec
|
CG4Trk* pTrk=(CG4Trk*)g_object_get_data(G_OBJECT(w),"tracker");
|
int numHubs;
|
|
if (!RunCollect)
|
return TRUE;
|
|
// periodically check for new hubs coming on line
|
if (++prs->counter>=refresh_cnt){
|
prs->counter=0;
|
if (prs->tot_hubs_on_system!=prs->numHub){
|
prs->numHub=numHubs=prs->tot_hubs_on_system=pTrk->UpdateHubs();
|
// numHubs=pTrk->UpdateHubs();
|
// if (numHubs!=prs->numHub){
|
int colorInd=0;
|
if (prs->hubList)
|
delete[] prs->hubList;
|
prs->hubList=new CG4Hub[numHubs];
|
int* hubIds=new int[numHubs];
|
pTrk->GetHubList(hubIds,numHubs);
|
for (int i=0;i<numHubs;i++){
|
prs->hubList[i].SetId(hubIds[i]);
|
prs->hubList[i].SetColor(triad_colors[colorInd++]);
|
// colorInd+=3;
|
if (colorInd>=15)
|
colorInd=0;
|
}
|
delete[] hubIds;
|
}
|
}
|
|
|
|
glDraw=gtk_widget_get_gl_drawable(w);
|
glContext=gtk_widget_get_gl_context(w);
|
|
gdk_gl_drawable_gl_begin(glDraw,glContext);
|
|
GLfloat lightPos[4]={-100.0f,500.0f,-100.0f,1.0f};
|
glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
|
|
glEnable(GL_DEPTH_TEST);
|
glEnable(GL_CULL_FACE);
|
glEnable(GL_LIGHTING);
|
glEnable(GL_LIGHT0);
|
glEnable(GL_COLOR_MATERIAL);
|
|
glLightfv(GL_LIGHT0,GL_POSITION,lightPos);
|
glColorMaterial(GL_FRONT,GL_AMBIENT_AND_DIFFUSE);
|
|
|
|
|
|
glLoadIdentity();
|
gluLookAt(0.0,0.0,30.0,0.0,0.0,0.0,0.0,10.0,30.0);
|
|
GLUquadricObj* quad=gluNewQuadric();
|
|
// Tracker coords x y z | y -z -x
|
// OpenGL coords -z x -y | x y z
|
|
float ang,vec[3];
|
prs->viewRot.GetAxisAngle(vec,ang);
|
|
glPushMatrix();
|
glTranslatef(prs->viewTrans[1],-prs->viewTrans[2],-prs->viewTrans[0]);
|
glRotatef(ang,vec[1],-vec[2],-vec[0]);
|
//glScalef(0.3,0.3,0.3);
|
// glScalef(5.0,5.0,5.0);
|
|
glPushMatrix();
|
|
RenderSources(quad,prs);
|
|
pthread_mutex_lock(&mutex);
|
RenderSensors(quad,prs);
|
pthread_mutex_unlock(&mutex);
|
|
glPopMatrix();
|
glPopMatrix();
|
|
gluDeleteQuadric(quad);
|
|
gdk_gl_drawable_swap_buffers(glDraw);
|
gdk_gl_drawable_gl_end(glDraw);
|
return TRUE;
|
}
|
|
gboolean cb_timer(gpointer g){
|
|
GtkWidget* w=GTK_WIDGET(g);
|
|
gdk_window_invalidate_rect(w->window,&w->allocation,FALSE);
|
gdk_window_process_updates(w->window,FALSE);
|
return TRUE;
|
}
|
|
void RenderSources(GLUquadricObj* quad,REND_STRUCT* prs){
|
|
for (int i=0;i<prs->numSrc;i++){
|
|
glPushMatrix();
|
glTranslatef(prs->srcList[i][1],-prs->srcList[i][2],-prs->srcList[i][0]); // ogl(x,y,z)->trk(y,-z,-x)
|
glColor3f(1.0,0.0,0.0);
|
glPushMatrix();
|
glScalef(prs->srcScale,prs->srcScale,prs->srcScale);
|
gluSphere(quad,1.0,50,25);
|
glPopMatrix();
|
glPopMatrix();
|
}
|
}
|
|
|
void RenderSensors(GLUquadricObj* quad,REND_STRUCT* prs){
|
|
for (int i=0;i<prs->numHub;i++){
|
if (prs->hubreadmap&(0x01<<i))
|
prs->hubList[i].Render(quad,prs->senScale);
|
}
|
|
|
}
|
|
void QuitApp(GtkWidget* w,gpointer g){
|
|
RunCollect=0;
|
pthread_join(thread1,NULL);
|
gtk_main_quit();
|
|
}
|
|
|
void* Collect(void* p){
|
|
REND_STRUCT* prs=(REND_STRUCT*)p;
|
|
while (RunCollect){
|
pthread_mutex_lock(&mutex);
|
prs->hubreadmap=0;
|
prs->tot_hubs_on_system=prs->pTrk->GetHubsPno(prs);
|
pthread_mutex_unlock(&mutex);
|
usleep(15000);
|
// usleep(8000);
|
}
|
|
return NULL;
|
}
|
|
gboolean cb_keyPress(GtkWidget* w,GdkEventKey* e,gpointer g){
|
|
int incr=1.0;
|
REND_STRUCT* prs=(REND_STRUCT*)g;
|
CQuaternion q;
|
|
// alt-Q
|
if ((e->state&GDK_MOD1_MASK)&&((e->keyval==GDK_Q)||(e->keyval==GDK_q)))
|
QuitApp(w,NULL);
|
|
// Reset -- Escape Key
|
else if (e->keyval==GDK_Escape){
|
memset(prs->viewTrans,0,sizeof(float)*3);
|
prs->viewRot.SetQuatVals(1.0,0.0,0.0,0.0);
|
prs->senScale=prs->srcScale=0.5;
|
}
|
|
// Directional Keys / Borsight
|
else {
|
|
if (e->state&GDK_SHIFT_MASK)
|
incr*=5.0;
|
|
switch (e->keyval){
|
case GDK_F2:
|
prs->viewTrans[0]+=incr;
|
break;
|
case GDK_F3:
|
prs->viewTrans[0]-=incr;
|
break;
|
case GDK_Right:
|
if (e->state & GDK_CONTROL_MASK)
|
prs->viewTrans[1]+=incr;
|
else if (e->state & GDK_MOD1_MASK){ // alt is pressed
|
q=CQuaternion(0.0,0.0,incr);
|
prs->viewRot*=q;
|
}
|
else{
|
q=CQuaternion(incr,0.0,0.0);
|
prs->viewRot*=q;
|
}
|
|
break;
|
case GDK_Left:
|
if(e->state & GDK_CONTROL_MASK)
|
prs->viewTrans[1]-=incr;
|
else if (e->state & GDK_MOD1_MASK){
|
q=CQuaternion(0.0,0.0,-incr);
|
prs->viewRot*=q;
|
}
|
else{
|
q=CQuaternion(-incr,0.0,0.0);
|
prs->viewRot*=q;
|
}
|
break;
|
case GDK_Up:
|
if (e->state & GDK_CONTROL_MASK)
|
prs->viewTrans[2]-=incr;
|
else{
|
q=CQuaternion(0.0,incr,0.0);
|
prs->viewRot*=q;
|
}
|
break;
|
case GDK_Down:
|
if (e->state & GDK_CONTROL_MASK)
|
prs->viewTrans[2]+=incr;
|
else {
|
q=CQuaternion(0.0,-incr,0.0);
|
prs->viewRot*=q;
|
}
|
break;
|
case GDK_B:
|
case GDK_b:
|
if (e->state & GDK_CONTROL_MASK){
|
if(e->state & GDK_SHIFT_MASK)
|
prs->pTrk->Boresight(false);
|
else
|
prs->pTrk->Boresight(true);
|
}
|
break;
|
|
case GDK_F:
|
case GDK_f:
|
OnFilter(prs->pTrk);
|
break;
|
|
case GDK_N:
|
case GDK_n:
|
OnIncr(prs->pTrk);
|
break;
|
|
case GDK_V:
|
case GDK_v:
|
OnVersion(w);
|
break;
|
|
case GDK_plus:
|
prs->srcScale+=0.5;
|
break;
|
case GDK_equal:
|
prs->senScale+=0.5;
|
break;
|
|
case GDK_underscore:
|
prs->srcScale-=0.5;
|
break;
|
case GDK_minus:
|
prs->senScale-=0.5f;
|
break;
|
|
}// end switch
|
|
}// end else
|
|
|
return FALSE;
|
}
|
|
void OnFilter(CG4Trk* pTrk){
|
|
// first get current filters to load dialog
|
float posFilt[4];
|
float oriFilt[4];
|
|
pTrk->GetFilterValues(posFilt,oriFilt);
|
FilterDlg dlg(posFilt,oriFilt);
|
int resp=dlg.present_dlg();
|
if (resp==GTK_RESPONSE_OK){
|
dlg.GetFilterData(posFilt,oriFilt);
|
pTrk->SetFilterValues(posFilt,oriFilt);
|
}
|
|
}
|
|
void OnIncr(CG4Trk* pTrk){
|
|
float p_incr,o_incr;
|
pTrk->GetSetIncr(p_incr,o_incr,FALSE);
|
IncrDlg dlg(p_incr,o_incr);
|
int resp=dlg.present_dlg();
|
if (resp==GTK_RESPONSE_OK){
|
dlg.get_increments(p_incr,o_incr);
|
pTrk->GetSetIncr(p_incr,o_incr,TRUE);
|
}
|
|
}
|
|
void OnVersion(GtkWidget* w){
|
|
const gchar* authors[]={"James C. Farr, Polhemus Inc.",NULL};
|
|
gtk_show_about_dialog(GTK_WINDOW(w),"authors",authors,"comments",g4_comment,"copyright","Copyright © Polhemus 2009-2013","program-name","g4display","version",g4_version,
|
"website","http://www.polhemus.com/",NULL);
|
|
}
|
