~streib/g4display.git

Janis Streib

30.03.22 48e3f3ec6672ba89420338fdfa76018e46750c9e

commit \| author \| age
69b66f	1	#include <stdio.h>
JS	2	#include <gtk/gtk.h>
	3	#include <gtk/gtkgl.h>
	4	#include <GL/gl.h>
	5	#include <GL/glu.h>
	6	#include "Quaternion.h"
	7	#include "struct.h"
	8	#include "G4Trk.h"
	9	#include "G4Hub.h"
	10	#include <string.h>
	11	#include <unistd.h>
	12	#include <pthread.h>
	13	#include "G4Display.h"
	14	#include <gdk/gdkkeysyms.h>
	15	#include "IncrDlg.h"
	16	#include "FilterDlg.h"
	17	#include "config.h"
	18
	19
	20	int RunCollect=0;
	21	pthread_t thread1;
	22	pthread_mutex_t mutex=PTHREAD_MUTEX_INITIALIZER;
	23
	24	const char* g4_version=VERSION;
	25	const char* g4_comment="Graphical Display Program to communicate with the Polhemus G4 Tracker";
	26
	27
	28	int main(int argc,char* argv[]){
	29
	30	GtkWidget* win,da,fileDlg;
	31	GdkGLConfig* glConfig;
	32	CG4Trk trk;
	33	REND_STRUCT rs;
	34	char* cfgFile;
	35	GtkFileFilter all,g4c;
	36
	37
	38	memset(rs.viewTrans,0,sizeof(float)*3);
	39	rs.srcScale=rs.senScale=0.5;
	40	rs.counter=0;
	41
	42	gtk_init(&argc,&argv);
	43	gtk_gl_init(&argc,&argv);
	44
	45	// show the images on the buttons
	46	g_object_set(gtk_settings_get_default(),"gtk-button_images",TRUE,NULL);
	47
	48	win=gtk_window_new(GTK_WINDOW_TOPLEVEL);
	49	gtk_window_set_title(GTK_WINDOW(win),"G4 Display Application");
	50
	51	da=gtk_drawing_area_new();
	52	gtk_widget_set_size_request(da,900,700);
	53	gtk_widget_set_events(da,GDK_EXPOSURE_MASK\|GDK_KEY_PRESS_MASK);
	54	gtk_container_add(GTK_CONTAINER(win),da);
	55	gtk_window_set_position(GTK_WINDOW(win),GTK_WIN_POS_NONE);
	56	gtk_widget_show(win);
	57
	58	glConfig=gdk_gl_config_new_by_mode((GdkGLConfigMode)(GDK_GL_MODE_RGB\|GDK_GL_MODE_DEPTH\|GDK_GL_MODE_DOUBLE));
	59	gtk_widget_set_gl_capability(da,glConfig,NULL,TRUE,GDK_GL_RGBA);
	60
	61	gtk_widget_show_all(win);
	62	gtk_window_maximize(GTK_WINDOW(win));
	63
	64
	65	g_signal_connect(win,"destroy",G_CALLBACK(QuitApp),NULL);
	66	g_signal_connect(da,"configure-event",G_CALLBACK(cb_configure),NULL);
	67	g_signal_connect(da,"expose-event",G_CALLBACK(cb_expose),&rs);
	68	g_signal_connect(win,"key-press-event",G_CALLBACK(cb_keyPress),&rs);
	69
	70	g_object_set_data(G_OBJECT(da),"tracker",&trk);
	71
	72	all=gtk_file_filter_new();
	73	g4c=gtk_file_filter_new();
	74	gtk_file_filter_set_name(all,"All Files");
	75	gtk_file_filter_set_name(g4c,"G4 Source Configuration Files (*.g4c)");
	76	gtk_file_filter_add_pattern(all,"*");
	77	gtk_file_filter_add_pattern(g4c,"*.g4c");
	78	fileDlg=gtk_file_chooser_dialog_new("Select a Source Configuration File",NULL,
	79	GTK_FILE_CHOOSER_ACTION_OPEN,
	80	GTK_STOCK_OPEN,GTK_RESPONSE_ACCEPT,
	81	GTK_STOCK_CANCEL,GTK_RESPONSE_CANCEL,NULL);
	82	gtk_file_chooser_add_filter(GTK_FILE_CHOOSER(fileDlg),g4c);
	83	gtk_file_chooser_add_filter(GTK_FILE_CHOOSER(fileDlg),all);
	84
	85
	86	int resp=gtk_dialog_run(GTK_DIALOG(fileDlg));
	87	if (resp==GTK_RESPONSE_CANCEL){
	88	gtk_widget_destroy(fileDlg);
	89	return -2;
	90	}
	91	cfgFile=gtk_file_chooser_get_filename(GTK_FILE_CHOOSER(fileDlg));
	92	gtk_widget_destroy(fileDlg);
	93	resp=trk.Connect(cfgFile);
	94	g_free(cfgFile);
	95	if (!resp){
	96	printf("Error Connecting to G4\n");
	97	return -1;
	98	}
	99
	100	usleep(500000);
	101	rs.numSrc=trk.GetNumSrc();
	102	rs.srcList=new float[rs.numSrc][6];
	103	trk.GetSrcLoc(rs.srcList);
	104
	105	// rs.numHub=trk.GetActHubs();
	106	rs.numHub=trk.UpdateHubs();
	107	rs.hubList=new CG4Hub[rs.numHub];
	108	int* hubIds=new int[rs.numHub];
	109	trk.GetHubList(hubIds,rs.numHub);
	110	int color_ind=0;
	111	for (int i=0;i<rs.numHub;i++){
	112	rs.hubList[i].SetId(hubIds[i]);
	113	rs.hubList[i].SetColor(triad_colors[color_ind]);
	114	color_ind++;
	115	if (color_ind>=15)
	116	color_ind=0;
	117	}
	118	delete[] hubIds;
	119
	120	rs.pTrk=&trk;
	121
	122	RunCollect=1;
	123	// g_timeout_add(8,cb_timer,da);
	124	g_timeout_add(REFRESH_RATE,cb_timer,da);
	125
	126	pthread_create(&thread1,NULL,Collect,&rs);
	127	gtk_main();
	128
	129	delete[] rs.srcList;
	130	delete[] rs.hubList;
	131
	132	return 0;
	133	}
	134
	135	gboolean cb_configure(GtkWidget* w,GdkEventConfigure* e,gpointer g){
	136
	137	GdkGLDrawable* glDraw;
	138	GdkGLContext* glContext;
	139
	140	glDraw=gtk_widget_get_gl_drawable(w);
	141	glContext=gtk_widget_get_gl_context(w);
	142
	143	gdk_gl_drawable_gl_begin(glDraw,glContext);
	144
	145	glClear(GL_COLOR_BUFFER_BIT\|GL_DEPTH_BUFFER_BIT);
	146	glViewport(0,0,(GLsizei)w->allocation.width,(GLsizei)w->allocation.height);
	147	glMatrixMode(GL_PROJECTION);
	148	glLoadIdentity();
	149	gluPerspective(60.0,(float)w->allocation.width/(float)w->allocation.height,0.1,200.0);
	150	glMatrixMode(GL_MODELVIEW);
	151	glLoadIdentity();
	152
	153	gdk_gl_drawable_gl_end(glDraw);
	154	return TRUE;
	155	}
	156
	157	gboolean cb_expose(GtkWidget* w,GdkEventExpose* e,gpointer g){
	158
	159	GdkGLDrawable* glDraw;
	160	GdkGLContext* glContext;
	161	REND_STRUCT* prs=(REND_STRUCT*)g;
	162	int refresh_cnt=(int)(1.0/(float)REFRESH_RATE*1000.0); // about every sec
	163	CG4Trk* pTrk=(CG4Trk*)g_object_get_data(G_OBJECT(w),"tracker");
	164	int numHubs;
	165
	166	if (!RunCollect)
	167	return TRUE;
	168
	169	// periodically check for new hubs coming on line
	170	if (++prs->counter>=refresh_cnt){
	171	prs->counter=0;
	172	if (prs->tot_hubs_on_system!=prs->numHub){
	173	prs->numHub=numHubs=prs->tot_hubs_on_system=pTrk->UpdateHubs();
	174	// numHubs=pTrk->UpdateHubs();
	175	// if (numHubs!=prs->numHub){
	176	int colorInd=0;
	177	if (prs->hubList)
	178	delete[] prs->hubList;
	179	prs->hubList=new CG4Hub[numHubs];
	180	int* hubIds=new int[numHubs];
	181	pTrk->GetHubList(hubIds,numHubs);
	182	for (int i=0;i<numHubs;i++){
	183	prs->hubList[i].SetId(hubIds[i]);
	184	prs->hubList[i].SetColor(triad_colors[colorInd++]);
	185	// colorInd+=3;
	186	if (colorInd>=15)
	187	colorInd=0;
	188	}
	189	delete[] hubIds;
	190	}
	191	}
	192
	193
	194
	195	glDraw=gtk_widget_get_gl_drawable(w);
	196	glContext=gtk_widget_get_gl_context(w);
	197
	198	gdk_gl_drawable_gl_begin(glDraw,glContext);
	199
	200	GLfloat lightPos[4]={-100.0f,500.0f,-100.0f,1.0f};
	201	glClear(GL_COLOR_BUFFER_BIT\|GL_DEPTH_BUFFER_BIT);
	202
	203	glEnable(GL_DEPTH_TEST);
	204	glEnable(GL_CULL_FACE);
	205	glEnable(GL_LIGHTING);
	206	glEnable(GL_LIGHT0);
	207	glEnable(GL_COLOR_MATERIAL);
	208
	209	glLightfv(GL_LIGHT0,GL_POSITION,lightPos);
	210	glColorMaterial(GL_FRONT,GL_AMBIENT_AND_DIFFUSE);
	211
	212
	213
	214
	215
	216	glLoadIdentity();
	217	gluLookAt(0.0,0.0,30.0,0.0,0.0,0.0,0.0,10.0,30.0);
	218
	219	GLUquadricObj* quad=gluNewQuadric();
	220
	221	// Tracker coords x y z \| y -z -x
	222	// OpenGL coords -z x -y \| x y z
	223
	224	float ang,vec[3];
	225	prs->viewRot.GetAxisAngle(vec,ang);
	226
	227	glPushMatrix();
	228	glTranslatef(prs->viewTrans[1],-prs->viewTrans[2],-prs->viewTrans[0]);
	229	glRotatef(ang,vec[1],-vec[2],-vec[0]);
	230	//glScalef(0.3,0.3,0.3);
	231	// glScalef(5.0,5.0,5.0);
	232
	233	glPushMatrix();
	234
	235	RenderSources(quad,prs);
	236
	237	pthread_mutex_lock(&mutex);
	238	RenderSensors(quad,prs);
	239	pthread_mutex_unlock(&mutex);
	240
	241	glPopMatrix();
	242	glPopMatrix();
	243
	244	gluDeleteQuadric(quad);
	245
	246	gdk_gl_drawable_swap_buffers(glDraw);
	247	gdk_gl_drawable_gl_end(glDraw);
	248	return TRUE;
	249	}
	250
	251	gboolean cb_timer(gpointer g){
	252
	253	GtkWidget* w=GTK_WIDGET(g);
	254
	255	gdk_window_invalidate_rect(w->window,&w->allocation,FALSE);
	256	gdk_window_process_updates(w->window,FALSE);
	257	return TRUE;
	258	}
	259
	260	void RenderSources(GLUquadricObj* quad,REND_STRUCT* prs){
	261
	262	for (int i=0;i<prs->numSrc;i++){
	263
	264	glPushMatrix();
	265	glTranslatef(prs->srcList[i][1],-prs->srcList[i][2],-prs->srcList[i][0]); // ogl(x,y,z)->trk(y,-z,-x)
	266	glColor3f(1.0,0.0,0.0);
	267	glPushMatrix();
	268	glScalef(prs->srcScale,prs->srcScale,prs->srcScale);
	269	gluSphere(quad,1.0,50,25);
	270	glPopMatrix();
	271	glPopMatrix();
	272	}
	273	}
	274
	275
	276	void RenderSensors(GLUquadricObj* quad,REND_STRUCT* prs){
	277
	278	for (int i=0;i<prs->numHub;i++){
	279	if (prs->hubreadmap&(0x01<<i))
	280	prs->hubList[i].Render(quad,prs->senScale);
	281	}
	282
	283
	284	}
	285
	286	void QuitApp(GtkWidget* w,gpointer g){
	287
	288	RunCollect=0;
	289	pthread_join(thread1,NULL);
	290	gtk_main_quit();
	291
	292	}
	293
	294
	295	void* Collect(void* p){
	296
	297	REND_STRUCT* prs=(REND_STRUCT*)p;
	298
	299	while (RunCollect){
	300	pthread_mutex_lock(&mutex);
	301	prs->hubreadmap=0;
	302	prs->tot_hubs_on_system=prs->pTrk->GetHubsPno(prs);
	303	pthread_mutex_unlock(&mutex);
	304	usleep(15000);
	305	// usleep(8000);
	306	}
	307
	308	return NULL;
	309	}
	310
	311	gboolean cb_keyPress(GtkWidget* w,GdkEventKey* e,gpointer g){
	312
	313	int incr=1.0;
	314	REND_STRUCT* prs=(REND_STRUCT*)g;
	315	CQuaternion q;
	316
	317	// alt-Q
	318	if ((e->state&GDK_MOD1_MASK)&&((e->keyval==GDK_Q)\|\|(e->keyval==GDK_q)))
	319	QuitApp(w,NULL);
	320
	321	// Reset -- Escape Key
	322	else if (e->keyval==GDK_Escape){
	323	memset(prs->viewTrans,0,sizeof(float)*3);
	324	prs->viewRot.SetQuatVals(1.0,0.0,0.0,0.0);
	325	prs->senScale=prs->srcScale=0.5;
	326	}
	327
	328	// Directional Keys / Borsight
	329	else {
	330
	331	if (e->state&GDK_SHIFT_MASK)
	332	incr*=5.0;
	333
	334	switch (e->keyval){
	335	case GDK_F2:
	336	prs->viewTrans[0]+=incr;
	337	break;
	338	case GDK_F3:
	339	prs->viewTrans[0]-=incr;
	340	break;
	341	case GDK_Right:
	342	if (e->state & GDK_CONTROL_MASK)
	343	prs->viewTrans[1]+=incr;
	344	else if (e->state & GDK_MOD1_MASK){ // alt is pressed
	345	q=CQuaternion(0.0,0.0,incr);
	346	prs->viewRot*=q;
	347	}
	348	else{
	349	q=CQuaternion(incr,0.0,0.0);
	350	prs->viewRot*=q;
	351	}
	352
	353	break;
	354	case GDK_Left:
	355	if(e->state & GDK_CONTROL_MASK)
	356	prs->viewTrans[1]-=incr;
	357	else if (e->state & GDK_MOD1_MASK){
	358	q=CQuaternion(0.0,0.0,-incr);
	359	prs->viewRot*=q;
	360	}
	361	else{
	362	q=CQuaternion(-incr,0.0,0.0);
	363	prs->viewRot*=q;
	364	}
	365	break;
	366	case GDK_Up:
	367	if (e->state & GDK_CONTROL_MASK)
	368	prs->viewTrans[2]-=incr;
	369	else{
	370	q=CQuaternion(0.0,incr,0.0);
	371	prs->viewRot*=q;
	372	}
	373	break;
	374	case GDK_Down:
	375	if (e->state & GDK_CONTROL_MASK)
	376	prs->viewTrans[2]+=incr;
	377	else {
	378	q=CQuaternion(0.0,-incr,0.0);
	379	prs->viewRot*=q;
	380	}
	381	break;
	382	case GDK_B:
	383	case GDK_b:
	384	if (e->state & GDK_CONTROL_MASK){
	385	if(e->state & GDK_SHIFT_MASK)
	386	prs->pTrk->Boresight(false);
	387	else
	388	prs->pTrk->Boresight(true);
	389	}
	390	break;
	391
	392	case GDK_F:
	393	case GDK_f:
	394	OnFilter(prs->pTrk);
	395	break;
	396
	397	case GDK_N:
	398	case GDK_n:
	399	OnIncr(prs->pTrk);
	400	break;
	401
	402	case GDK_V:
	403	case GDK_v:
	404	OnVersion(w);
	405	break;
	406
	407	case GDK_plus:
	408	prs->srcScale+=0.5;
	409	break;
	410	case GDK_equal:
	411	prs->senScale+=0.5;
	412	break;
	413
	414	case GDK_underscore:
	415	prs->srcScale-=0.5;
	416	break;
	417	case GDK_minus:
	418	prs->senScale-=0.5f;
	419	break;
	420
	421	}// end switch
	422
	423	}// end else
	424
	425
	426	return FALSE;
	427	}
	428
	429	void OnFilter(CG4Trk* pTrk){
	430
	431	// first get current filters to load dialog
	432	float posFilt[4];
	433	float oriFilt[4];
	434
	435	pTrk->GetFilterValues(posFilt,oriFilt);
	436	FilterDlg dlg(posFilt,oriFilt);
	437	int resp=dlg.present_dlg();
	438	if (resp==GTK_RESPONSE_OK){
	439	dlg.GetFilterData(posFilt,oriFilt);
	440	pTrk->SetFilterValues(posFilt,oriFilt);
	441	}
	442
	443	}
	444
	445	void OnIncr(CG4Trk* pTrk){
	446
	447	float p_incr,o_incr;
	448	pTrk->GetSetIncr(p_incr,o_incr,FALSE);
	449	IncrDlg dlg(p_incr,o_incr);
	450	int resp=dlg.present_dlg();
	451	if (resp==GTK_RESPONSE_OK){
	452	dlg.get_increments(p_incr,o_incr);
	453	pTrk->GetSetIncr(p_incr,o_incr,TRUE);
	454	}
	455
	456	}
	457
	458	void OnVersion(GtkWidget* w){
	459
	460	const gchar* authors[]={"James C. Farr, Polhemus Inc.",NULL};
	461
	462	gtk_show_about_dialog(GTK_WINDOW(w),"authors",authors,"comments",g4_comment,"copyright","Copyright © Polhemus 2009-2013","program-name","g4display","version",g4_version,
	463	"website","http://www.polhemus.com/",NULL);
	464
	465	}