|
From: Philip L. <phi...@us...> - 2004-11-22 03:39:03
|
Update of /cvsroot/artoolkit/artoolkit/examples/paddleDemo In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv25381 Added Files: paddle.c paddleDemo.vcproj util.c Makefile.in paddle.h paddleDemo.c command_sub.c util.h command_sub.h paddleDemo.dsp Log Message: Added new examples. --- NEW FILE: Makefile.in --- INC_DIR= ../../include LIB_DIR= ../../lib BIN_DIR= ../../bin LDFLAG=@LDFLAG@ -L$(LIB_DIR) LIBS= -lARgsub -lARvideo -lARMulti -lAR @LIBS@ CFLAG= @CFLAG@ -I$(INC_DIR) OBJS = command_sub.o util.o paddle.o HEADDERS = command_sub.h util.h paddle.h all: $(BIN_DIR)/paddleDemo $(BIN_DIR)/paddleDemo: paddleDemo.o $(OBJS) cc -o $(BIN_DIR)/paddleDemo paddleDemo.o $(OBJS) $(LDFLAG) $(LIBS) paddleDemo.o: paddleDemo.c $(HEADDERS) cc -c $(CFLAG) paddleDemo.c command_sub.o: command_sub.c $(HEADDERS) cc -c $(CFLAG) command_sub.c util.o: util.c $(HEADDERS) cc -c $(CFLAG) util.c paddle.o: paddle.c $(HEADDERS) cc -c $(CFLAG) paddle.c clean: rm -f *.o rm -f $(BIN_DIR)/paddleDemo allclean: rm -f *.o rm -f $(BIN_DIR)/paddleDemo rm -f Makefile --- NEW FILE: paddle.h --- #ifndef PADDLE_H #define PADDLE_H #include <AR/ar.h> #define PADDLE_RADIUS 41.0 typedef struct { char name[256]; int marker_id; double width; double center[2]; double trans[3][4]; double transL[3][4]; double transR[3][4]; int active; ARMarkerInfo prevInfo; ARMarkerInfo prevInfoL; ARMarkerInfo prevInfoR; int pcount; int pcountL; int pcountR; } ARPaddleInfo; ARPaddleInfo *paddleInit( char *name ); int paddleGetTrans( ARPaddleInfo *paddleInfo, ARMarkerInfo *markerInfo, int *flagL, int marker_num, ARParam *cparam ); #endif --- NEW FILE: util.c --- #include <stdio.h> #include <math.h> #include "util.h" void get_rot( double a, double b , double c, double trans[3][4] ) { double sina, sinb, sinc, cosa, cosb, cosc; sina = sin(a); cosa = cos(a); sinb = sin(b); cosb = cos(b); sinc = sin(c); cosc = cos(c); trans[0][0] = cosa*cosa*cosb*cosc+sina*sina*cosc+sina*cosa*cosb*sinc-sina*cosa*sinc; trans[0][1] = -cosa*cosa*cosb*sinc-sina*sina*sinc+sina*cosa*cosb*cosc-sina*cosa*cosc; trans[0][2] = cosa*sinb; trans[1][0] = sina*cosa*cosb*cosc-sina*cosa*cosc+sina*sina*cosb*sinc+cosa*cosa*sinc; trans[1][1] = -sina*cosa*cosb*sinc+sina*cosa*sinc+sina*sina*cosb*cosc+cosa*cosa*cosc; trans[1][2] = sina*sinb; trans[2][0] = -cosa*sinb*cosc-sina*sinb*sinc; trans[2][1] = cosa*sinb*sinc-sina*sinb*cosc; trans[2][2] = cosb; return; } int get_angle( double trans[3][4], double *wa, double *wb, double *wc ) { double a, b, c; double sina, cosa, sinb, cosb, sinc, cosc; if( trans[2][2] > 1.0 ) { printf("cos(beta) = %f\n", trans[2][2]); trans[2][2] = -1.0; } else if( trans[2][2] < -1.0 ) { printf("cos(beta) = %f\n", trans[2][2]); trans[2][2] = -1.0; } cosb = trans[2][2]; b = acos( cosb ); sinb = sin( b ); if( b >= 0.000001 || b <= -0.000001) { cosa = trans[0][2] / sinb; sina = trans[1][2] / sinb; if( cosa > 1.0 ) { printf("cos(alph) = %f\n", cosa); cosa = 1.0; sina = 0.0; } if( cosa < -1.0 ) { printf("cos(alph) = %f\n", cosa); cosa = -1.0; sina = 0.0; } if( sina > 1.0 ) { printf("sin(alph) = %f\n", sina); sina = 1.0; cosa = 0.0; } if( sina < -1.0 ) { printf("sin(alph) = %f\n", sina); sina = -1.0; cosa = 0.0; } a = acos( cosa ); if( sina < 0 ) a = -a; sinc = (trans[2][1]*trans[0][2]-trans[2][0]*trans[1][2]) / (trans[0][2]*trans[0][2]+trans[1][2]*trans[1][2]); cosc = -(trans[0][2]*trans[2][0]+trans[1][2]*trans[2][1]) / (trans[0][2]*trans[0][2]+trans[1][2]*trans[1][2]); if( cosc > 1.0 ) { printf("cos(r) = %f\n", cosc); cosc = 1.0; sinc = 0.0; } if( cosc < -1.0 ) { printf("cos(r) = %f\n", cosc); cosc = -1.0; sinc = 0.0; } if( sinc > 1.0 ) { printf("sin(r) = %f\n", sinc); sinc = 1.0; cosc = 0.0; } if( sinc < -1.0 ) { printf("sin(r) = %f\n", sinc); sinc = -1.0; cosc = 0.0; } c = acos( cosc ); if( sinc < 0 ) c = -c; } else { a = b = 0.0; cosa = cosb = 1.0; sina = sinb = 0.0; cosc = trans[0][0]; sinc = trans[1][0]; if( cosc > 1.0 ) { printf("cos(r) = %f\n", cosc); cosc = 1.0; sinc = 0.0; } if( cosc < -1.0 ) { printf("cos(r) = %f\n", cosc); cosc = -1.0; sinc = 0.0; } if( sinc > 1.0 ) { printf("sin(r) = %f\n", sinc); sinc = 1.0; cosc = 0.0; } if( sinc < -1.0 ) { printf("sin(r) = %f\n", sinc); sinc = -1.0; cosc = 0.0; } c = acos( cosc ); if( sinc < 0 ) c = -c; } *wa = a; *wb = b; *wc = c; return(0); } double get_height( double px, double py, double trans[3][4], double boundary[3][2] ) { double x, x0, x1, x2; double y, y0, y1, y2; double d, m00, m01, m10, m11; double rx, ry; x = boundary[0][0]; y = boundary[1][0]; x0 = trans[0][0]*x + trans[0][1]*y + trans[0][3]; y0 = trans[1][0]*x + trans[1][1]*y + trans[1][3]; x = boundary[0][1]; y = boundary[1][0]; x1 = trans[0][0]*x + trans[0][1]*y + trans[0][3]; y1 = trans[1][0]*x + trans[1][1]*y + trans[1][3]; x = boundary[0][0]; y = boundary[1][1]; x2 = trans[0][0]*x + trans[0][1]*y + trans[0][3]; y2 = trans[1][0]*x + trans[1][1]*y + trans[1][3]; x1 -= x0; y1 -= y0; x2 -= x0; y2 -= y0; x0 = px - x0; y0 = py - y0; d = x1*y2 - x2*y1; if( d == 0.0 ) return 0.0; m00 = y2 / d; m01 = -x2 / d; m10 = -y1 / d; m11 = x1 / d; rx = m00*x0 + m01*y0; ry = m10*x0 + m11*y0; if( rx >= 0.0 && rx <= 1.0 && ry >= 0.0 && ry <= 1.0 ) { return trans[2][3] + boundary[2][1]; } return 0.0; } --- NEW FILE: command_sub.c --- #ifdef _WIN32 #include <windows.h> #endif #include <stdio.h> #include <stdlib.h> #include <string.h> #include <math.h> #include <AR/param.h> #include <AR/matrix.h> #include <AR/ar.h> #include "paddle.h" #include "command_sub.h" #include "util.h" #define SHAKE_BUF_SIZE 10 #define PUNCH_BUF_SIZE 5 #define PUSH_BUF_SIZE 2 typedef struct { double mat[3][4]; int f; } SHAKE_BUF_T; typedef struct { double x, y, z; int f; } PUNCH_BUF_T; typedef struct { double x, y, z; int f; } PUSH_BUF_T; static SHAKE_BUF_T shake_buf[SHAKE_BUF_SIZE]; static int shake_buf_num = 0; static PUNCH_BUF_T punch_buf[PUNCH_BUF_SIZE]; static int punch_buf_num = 0; static PUSH_BUF_T push_buf[PUNCH_BUF_SIZE]; static int push_buf_num = 0; int check_shake( double card_trans[3][4], int f ) { ARMat *mat_a, *mat_b, *mat_c; double lxy, lz; int i, j, k; if( shake_buf_num < SHAKE_BUF_SIZE ) { if( f ) { for( j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) { shake_buf[shake_buf_num].mat[j][i] = card_trans[j][i]; } } shake_buf[shake_buf_num].f = 1; } else { shake_buf[shake_buf_num].f = 0; } shake_buf_num++; return 0; } for( i = 1; i < shake_buf_num; i++ ) { shake_buf[i-1] = shake_buf[i]; } if( f ) { for( j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) { shake_buf[shake_buf_num-1].mat[j][i] = card_trans[j][i]; } } shake_buf[shake_buf_num-1].f = 1; } else { shake_buf[shake_buf_num-1].f = 0; return 0; } if( shake_buf[SHAKE_BUF_SIZE-3].f == 0 || shake_buf[0].f == 0 ) return 0; mat_a = arMatrixAlloc( 4, 4 ); mat_b = arMatrixAlloc( 4, 4 ); mat_c = arMatrixAlloc( 4, 4 ); for( j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) { mat_a->m[j*4+i] = card_trans[j][i]; } } mat_a->m[3*4+0] = 0.0; mat_a->m[3*4+1] = 0.0; mat_a->m[3*4+2] = 0.0; mat_a->m[3*4+3] = 1.0; arMatrixSelfInv( mat_a ); for( k = 0 ; k < SHAKE_BUF_SIZE-3; k++ ) { if( shake_buf[k].f == 0 ) continue; for( j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) { mat_b->m[j*4+i] = shake_buf[k].mat[j][i]; } } mat_b->m[3*4+0] = 0.0; mat_b->m[3*4+1] = 0.0; mat_b->m[3*4+2] = 0.0; mat_b->m[3*4+3] = 1.0; arMatrixMul( mat_c, mat_a, mat_b ); lxy = sqrt( (mat_c->m[0*4+3])*(mat_c->m[0*4+3]) +(mat_c->m[1*4+3])*(mat_c->m[1*4+3])); lz = mat_c->m[2*4+3]; if( lxy < 20.0 && lz < 20.0 ) break; } if( k == SHAKE_BUF_SIZE-3 ) { arMatrixFree( mat_a ); arMatrixFree( mat_b ); arMatrixFree( mat_c ); return 0; } for( ; k < SHAKE_BUF_SIZE-1; k++ ) { if( shake_buf[k].f == 0 ) continue; for( j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) { mat_b->m[j*4+i] = shake_buf[k].mat[j][i]; } } mat_b->m[3*4+0] = 0.0; mat_b->m[3*4+1] = 0.0; mat_b->m[3*4+2] = 0.0; mat_b->m[3*4+3] = 1.0; arMatrixMul( mat_c, mat_a, mat_b ); lxy = sqrt( (mat_c->m[0*4+3])*(mat_c->m[0*4+3]) +(mat_c->m[1*4+3])*(mat_c->m[1*4+3])); lz = mat_c->m[2*4+3]; if( lxy > 60.0 && lz < 20.0 ) break; } arMatrixFree( mat_a ); arMatrixFree( mat_b ); arMatrixFree( mat_c ); if( k < SHAKE_BUF_SIZE-1 ) { shake_buf_num = 0; return 1; } return 0; } int check_incline( double card_trans[3][4], double base_trans[3][4], double *angle ) { ARMat *mat_a, *mat_b, *mat_c; double a, b, c; int i, j; mat_a = arMatrixAlloc( 4, 4 ); mat_b = arMatrixAlloc( 4, 4 ); mat_c = arMatrixAlloc( 4, 4 ); for( j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) { mat_b->m[j*4+i] = base_trans[j][i]; } } mat_b->m[3*4+0] = 0.0; mat_b->m[3*4+1] = 0.0; mat_b->m[3*4+2] = 0.0; mat_b->m[3*4+3] = 1.0; for( j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) { mat_a->m[j*4+i] = card_trans[j][i]; } } mat_a->m[3*4+0] = 0.0; mat_a->m[3*4+1] = 0.0; mat_a->m[3*4+2] = 0.0; mat_a->m[3*4+3] = 1.0; arMatrixSelfInv( mat_a ); arMatrixMul( mat_c, mat_a, mat_b ); get_angle( (double (*)[4])(mat_c->m), &a, &b, &c ); arMatrixFree( mat_a ); arMatrixFree( mat_b ); arMatrixFree( mat_c ); if( b > 0.4 ) { *angle = a + 3.141592; return 1; } return 0; } int check_pickup(double card_trans[3][4], double base_trans[3][4], ItemList* itlist,float* angle) { ARMat *t1, *t2, *t3; double x, y, z; double lx, ly; double a, b, c; int ret; int i, j; // printf("checking pickup New \n"); t1 = arMatrixAlloc( 4, 4 ); for( j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) { t1->m[j*4+i] = base_trans[j][i]; } } t1->m[12] = t1->m[13] = t1->m[14] = 0.0; t1->m[15] = 1.0; t2 = arMatrixAlloc( 4, 4 ); for( j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) { t2->m[j*4+i] = card_trans[j][i]; } } t2->m[12] = t2->m[13] = t2->m[14] = 0.0; t2->m[15] = 1.0; if( arMatrixSelfInv(t1) != 0 ) { arMatrixFree( t1 ); arMatrixFree( t2 ); return -1; } // printf("past arMatrixSelfInv\n"); t3 = arMatrixAllocMul(t1, t2); if( t3 == NULL ) { arMatrixFree( t1 ); arMatrixFree( t2 ); return -1; } // printf("past arMatrixAllocMul\n"); x = t3->m[0*4+3]; y = t3->m[1*4+3]; z = t3->m[2*4+3]; // printf("x: %f y: %f z: %f\n",x,y,z); ret = -1; for( i = 0; i < itlist->itemnum; i ++ ){ lx = x - itlist->item[i].pos[0]; ly = y - itlist->item[i].pos[1]; //MB increased by a factor of 10 if( lx*lx + ly*ly < 1000.0 && z < 20.0 ) { ret = i; } } if( ret >= 0 ) { get_angle( (double (*)[4])(t3->m), &a, &b, &c ); *angle = -c; } arMatrixFree( t1 ); arMatrixFree( t2 ); arMatrixFree( t3 ); return ret; } --- NEW FILE: paddle.c --- #include <stdio.h> #include <malloc.h> #include <stdlib.h> #include <AR/ar.h> #include "paddle.h" static ARMarkerInfo prevInfoL; static ARMarkerInfo prevInfoR; static int pcountL = 0; static int pcountR = 0; static char *get_buff( char *buf, int n, FILE *fp ); static int get_marker( ARMarkerInfo *markerInfo, int *markerFlag, int marker_num, int id, ARMarkerInfo *prevInfo, int *pcount ); ARPaddleInfo *paddleInit( char *name ) { FILE *fp; ARPaddleInfo *paddleInfo; char buf[256], buf1[256]; if( (fp=fopen(name, "r")) == NULL ) return(0); arMalloc( paddleInfo, ARPaddleInfo, 1 ); get_buff(buf, 256, fp); if( sscanf(buf, "%s", paddleInfo->name) != 1 ) { fclose(fp); free(paddleInfo); return 0; } get_buff(buf, 256, fp); if( sscanf(buf, "%s", buf1) != 1 ) { fclose(fp); free(paddleInfo); return 0; } if( (paddleInfo->marker_id = arLoadPatt(buf1)) < 0 ) { fclose(fp); free(paddleInfo); return 0; } get_buff(buf, 256, fp); if( sscanf(buf, "%lf", &paddleInfo->width) != 1 ) { fclose(fp); free(paddleInfo); return 0; } paddleInfo->center[0] = 0.0; paddleInfo->center[1] = 0.0; paddleInfo->active = 0; paddleInfo->pcount = 0; paddleInfo->pcountL = 0; paddleInfo->pcountR = 0; fclose(fp); return paddleInfo; } static char *get_buff( char *buf, int n, FILE *fp ) { char *ret; for(;;) { ret = fgets( buf, n, fp ); if( ret == NULL ) return(NULL); if( buf[0] != '\n' && buf[0] != '#' ) return(ret); } } int paddleGetTrans( ARPaddleInfo *paddleInfo, ARMarkerInfo *markerInfo, int *markerFlag, int marker_num, ARParam *cparam ) { int id; id = get_marker( markerInfo, markerFlag, marker_num, paddleInfo->marker_id, &(paddleInfo->prevInfo), &(paddleInfo->pcount) ); if( id == -1 ) { paddleInfo->active = 0; return 0; } arGetTransMat(&markerInfo[id], paddleInfo->center, paddleInfo->width, paddleInfo->trans); markerFlag[id] = 1; paddleInfo->active = 1; return 0; } static int get_marker( ARMarkerInfo *markerInfo, int *markerFlag, int marker_num, int id, ARMarkerInfo *prevInfo, int *pcount ) { double rlen, rlenmin, rarea, diff, diffmin; int cid, cdir; int i, j; cid = -1; for( i = 0; i < marker_num; i++ ) { if( markerInfo[i].id == id && markerFlag[i] == 0 ) { if( cid == -1 ) cid = i; else { if( markerInfo[cid].cf < markerInfo[i].cf ) cid = i; } } } if( cid == -1 ) { if( *pcount == 0 ) return -1; rlenmin = 10.0; for( i = 0; i < marker_num; i++ ) { if( markerFlag[i] ) continue; rarea = (double)prevInfo->area / (double)markerInfo[i].area; if( rarea < 0.7 || rarea > 1.43 ) continue; rlen = ( (markerInfo[i].pos[0] - prevInfo->pos[0]) * (markerInfo[i].pos[0] - prevInfo->pos[0]) + (markerInfo[i].pos[1] - prevInfo->pos[1]) * (markerInfo[i].pos[1] - prevInfo->pos[1]) ) / markerInfo[i].area; if( rlen < 0.5 && rlen < rlenmin ) { rlenmin = rlen; cid = i; } } if( cid >= 0 && markerInfo[cid].cf < prevInfo->cf ) { markerInfo[cid].cf = prevInfo->cf; markerInfo[cid].id = prevInfo->id; diffmin = 10000.0 * 10000.0; cdir = -1; for( i = 0; i < 4; i++ ) { diff = 0.0; for( j = 0; j < 4; j++ ) { diff += (prevInfo->vertex[j][0] - markerInfo[cid].vertex[(i+j)%4][0]) * (prevInfo->vertex[j][0] - markerInfo[cid].vertex[(i+j)%4][0]) + (prevInfo->vertex[j][1] - markerInfo[cid].vertex[(i+j)%4][1]) * (prevInfo->vertex[j][1] - markerInfo[cid].vertex[(i+j)%4][1]); } if( diff < diffmin ) { diffmin = diff; cdir = (prevInfo->dir - i + 4) % 4; } } markerInfo[cid].dir = cdir; *prevInfo = markerInfo[cid]; *pcount = 1; } else { *pcount = 0; return -1; } } else { *prevInfo = markerInfo[cid]; *pcount = 1; } return cid; } --- NEW FILE: util.h --- #ifndef UTIL_H #define UTIL_H void get_rot( double a, double b , double c, double trans[3][4] ); int get_angle( double trans[3][4], double *wa, double *wb, double *wc ); double get_height( double x, double y, double trans[3][4], double boundary[3][2] ); /* RJS */ int wrappedDropOntoMap(char *where, float *inPt, float *outPt); #endif --- NEW FILE: paddleDemo.dsp --- # Microsoft Developer Studio Project File - Name="paddleDemo" - Package Owner=<4> # Microsoft Developer Studio Generated Build File, Format Version 6.00 # ** DO NOT EDIT ** # TARGTYPE "Win32 (x86) Console Application" 0x0103 CFG=paddleDemo - Win32 Debug !MESSAGE This is not a valid makefile. To build this project using NMAKE, !MESSAGE use the Export Makefile command and run !MESSAGE !MESSAGE NMAKE /f "paddleDemo.mak". !MESSAGE !MESSAGE You can specify a configuration when running NMAKE !MESSAGE by defining the macro CFG on the command line. For example: !MESSAGE !MESSAGE NMAKE /f "paddleDemo.mak" CFG="paddleDemo - Win32 Debug" !MESSAGE !MESSAGE Possible choices for configuration are: !MESSAGE !MESSAGE "paddleDemo - Win32 Release" (based on "Win32 (x86) Console Application") !MESSAGE "paddleDemo - Win32 Debug" (based on "Win32 (x86) Console Application") !MESSAGE # Begin Project # PROP AllowPerConfigDependencies 0 # PROP Scc_ProjName "" # PROP Scc_LocalPath "" CPP=cl.exe RSC=rc.exe !IF "$(CFG)" == "paddleDemo - Win32 Release" # PROP BASE Use_MFC 0 # PROP BASE Use_Debug_Libraries 0 # PROP BASE Output_Dir "Release" # PROP BASE Intermediate_Dir "Release" # PROP BASE Target_Dir "" # PROP Use_MFC 0 # PROP Use_Debug_Libraries 0 # PROP Output_Dir "Release" # PROP Intermediate_Dir "Release" # PROP Target_Dir "" # ADD BASE CPP /nologo /W3 /GX /O2 /D "WIN32" /D "NDEBUG" /D "_CONSOLE" /D "_MBCS" /YX /FD /c # ADD CPP /nologo /W3 /GX /O2 /I "..\..\include" /D "WIN32" /D "NDEBUG" /D "_CONSOLE" /D "_MBCS" /FD /c # SUBTRACT CPP /YX # ADD BASE RSC /l 0x1409 /d "NDEBUG" # ADD RSC /l 0x1409 /d "NDEBUG" BSC32=bscmake.exe # ADD BASE BSC32 /nologo # ADD BSC32 /nologo LINK32=link.exe # ADD BASE LINK32 kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib /nologo /subsystem:console /machine:I386 # ADD LINK32 libAR.lib libARvideo.lib libARgsub.lib libARmulti.lib kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib /nologo /subsystem:console /machine:I386 /nodefaultlib:"libc.lib" /out:"../../bin/paddleDemo.exe" /libpath:"..\..\lib" # SUBTRACT LINK32 /nodefaultlib !ELSEIF "$(CFG)" == "paddleDemo - Win32 Debug" # PROP BASE Use_MFC 0 # PROP BASE Use_Debug_Libraries 1 # PROP BASE Output_Dir "Debug" # PROP BASE Intermediate_Dir "Debug" # PROP BASE Target_Dir "" # PROP Use_MFC 0 # PROP Use_Debug_Libraries 1 # PROP Output_Dir "Debug" # PROP Intermediate_Dir "Debug" # PROP Ignore_Export_Lib 0 # PROP Target_Dir "" # ADD BASE CPP /nologo /W3 /Gm /GX /ZI /Od /D "WIN32" /D "_DEBUG" /D "_CONSOLE" /D "_MBCS" /YX /FD /GZ /c # ADD CPP /nologo /W3 /Gm /GX /ZI /Od /I "..\..\include" /D "WIN32" /D "_DEBUG" /D "_CONSOLE" /D "_MBCS" /FD /GZ /c # SUBTRACT CPP /YX # ADD BASE RSC /l 0x1409 /d "_DEBUG" # ADD RSC /l 0x1409 /d "_DEBUG" BSC32=bscmake.exe # ADD BASE BSC32 /nologo # ADD BSC32 /nologo LINK32=link.exe # ADD BASE LINK32 kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib /nologo /subsystem:console /debug /machine:I386 /pdbtype:sept # ADD LINK32 libARd.lib libARvideod.lib libARgsubd.lib libARmultid.lib kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib /nologo /subsystem:console /debug /machine:I386 /nodefaultlib:"libcd.lib" /out:"../../bin/paddleDemod.exe" /pdbtype:sept /libpath:"..\..\lib" # SUBTRACT LINK32 /nodefaultlib !ENDIF # Begin Target # Name "paddleDemo - Win32 Release" # Name "paddleDemo - Win32 Debug" # Begin Group "Source Files" # PROP Default_Filter "cpp;c;cxx;rc;def;r;odl;idl;hpj;bat" # Begin Source File SOURCE=.\command_sub.c # End Source File # Begin Source File SOURCE=.\paddle.c # End Source File # Begin Source File SOURCE=.\paddleDemo.c # End Source File # Begin Source File SOURCE=.\util.c # End Source File # End Group # Begin Group "Header Files" # PROP Default_Filter "h;hpp;hxx;hm;inl" # Begin Source File SOURCE=.\command_sub.h # End Source File # Begin Source File SOURCE=.\paddle.h # End Source File # Begin Source File SOURCE=.\util.h # End Source File # End Group # Begin Group "Resource Files" # PROP Default_Filter "ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe" # End Group # End Target # End Project --- NEW FILE: paddleDemo.c --- #ifdef _WIN32 #include <windows.h> #endif #include <stdio.h> #include <stdlib.h> #include <string.h> #include <math.h> #include <GL/gl.h> #include <GL/glut.h> #include <AR/gsub.h> #include <AR/video.h> #include <AR/param.h> #include <AR/matrix.h> #include <AR/ar.h> #include <AR/arMulti.h> /* set up the video format globals */ /*****************************************************************************/ // modified by Thomas Pintaric, Vienna University of Technology #ifdef _WIN32 char *vconf = "showDlg,flipV"; // see video.h for a list of supported parameters #else char *vconf = ""; #endif /*****************************************************************************/ #define TARGET_NUM 5 #include "paddle.h" #include "command_sub.h" PaddleItemInfo myPaddleItem; ItemList myListItem; int xsize, ysize; int thresh = 100; int count = 0; char *cparam_name = "Data/camera_para.dat"; ARParam cparam; char *config_name = "Data/multi/marker.dat"; ARMultiMarkerInfoT *config; /* paddle information */ int marker_flag[AR_SQUARE_MAX]; ARPaddleInfo *paddleInfo; char *paddle_name = "Data/paddle_data"; GLfloat light_position[] = {100.0,-200.0,200.0,0.0}; GLfloat ambi[] = {0.1, 0.1, 0.1, 0.1}; GLfloat lightZeroColor[] = {0.9, 0.9, 0.9, 0.1}; PaddleItemInfo myPaddleItem; ItemList myListItem; static int draw_paddle( ARPaddleInfo *paddleInfo, PaddleItemInfo *myPaddleItem); static void init(void); static void cleanup(void); static void keyEvent( unsigned char key, int x, int y); static void mainLoop(void); static int draw_paddle( ARPaddleInfo *paddleInfo, PaddleItemInfo *myPaddleItem); static int drawGroundGrid( double trans[3][4], int divisions, float x, float y, float height); static int checkCollision(float Pos1[],float Pos2[], float range); static void findPaddlePosition(float curPaddlePos[], double card_trans[3][4],double base_trans[3][4]); static void drawItems(double trans[3][4],ItemList* list); int main() { //initialize applications init(); arVideoCapStart(); //start the main event loop argMainLoop( NULL, keyEvent, mainLoop ); return 0; } /* keyboard events */ static void keyEvent( unsigned char key, int x, int y) { /* quit if the ESC key is pressed */ if( key == 0x1b ) { printf("*** %f (frame/sec)\n", (double)count/arUtilTimer()); cleanup(); exit(0); } /* change the threshold value when 't' key pressed */ if( key == 't' ) { printf("Enter new threshold value (default = 100): "); scanf("%d",&thresh); while( getchar()!='\n' ); printf("\n"); } /* turn on and off the debug mode with d key */ if( key == 'd' ) { printf("*** %f (frame/sec)\n", (double)count/arUtilTimer()); arDebug = 1 - arDebug; if( arDebug == 0 ) { glClearColor( 0.0, 0.0, 0.0, 0.0 ); glClear(GL_COLOR_BUFFER_BIT); argSwapBuffers(); glClear(GL_COLOR_BUFFER_BIT); argSwapBuffers(); } count = 0; } } /* main loop */ static void mainLoop(void) { ARUint8 *dataPtr; ARMarkerInfo *marker_info; int marker_num; float curPaddlePos[3]; int i; double err; double angle; err=0.; /* grab a video frame */ if( (dataPtr = (ARUint8 *)arVideoGetImage()) == NULL ) { arUtilSleep(2); return; } if( count == 0 ) arUtilTimerReset(); count++; /* detect the markers in the video frame */ if( arDetectMarkerLite(dataPtr, thresh, &marker_info, &marker_num) < 0 ) { cleanup(); exit(0); } argDrawMode2D(); if( !arDebug ) { argDispImage( dataPtr, 0,0 ); } else { argDispImage( dataPtr, 1, 1 ); if( arImageProcMode == AR_IMAGE_PROC_IN_HALF ) argDispHalfImage( arImage, 0, 0 ); else argDispImage( arImage, 0, 0); glColor3f( 1.0, 0.0, 0.0 ); glLineWidth( 1.0 ); for( i = 0; i < marker_num; i++ ) { argDrawSquare( marker_info[i].vertex, 0, 0 ); } glLineWidth( 1.0 ); } arVideoCapNext(); for( i = 0; i < marker_num; i++ ) marker_flag[i] = 0; /* get the paddle position */ paddleGetTrans(paddleInfo, marker_info, marker_flag, marker_num, &cparam); /* draw the 3D models */ glClearDepth( 1.0 ); glClear(GL_DEPTH_BUFFER_BIT); /* get the translation from the multimarker pattern */ if( (err=arMultiGetTransMat(marker_info, marker_num, config)) < 0 ) { argSwapBuffers(); return; } // printf("err = %f\n", err); if(err > 100.0 ) { argSwapBuffers(); return; } //draw a red ground grid drawGroundGrid( config->trans, 15, 150.0, 110.0, 0.0); /* find the paddle position relative to the base */ if (paddleInfo->active) findPaddlePosition(curPaddlePos,paddleInfo->trans,config->trans); /* checking for paddle gesture */ if( paddleInfo->active) { int findItem=-1; if (myPaddleItem.item!=-1) { if( check_incline(paddleInfo->trans, config->trans, &angle) ) { myPaddleItem.x += 2.0 * cos(angle); myPaddleItem.y += 2.0 * sin(angle); if( myPaddleItem.x*myPaddleItem.x + myPaddleItem.y*myPaddleItem.y > 900.0 ) { myPaddleItem.x -= 2.0 * cos(angle); myPaddleItem.y -= 2.0 * sin(angle); myListItem.item[myPaddleItem.item].onpaddle=0; myListItem.item[myPaddleItem.item].pos[0]=curPaddlePos[0]; myListItem.item[myPaddleItem.item].pos[1]=curPaddlePos[1]; myPaddleItem.item = -1; } } } else { if ((findItem=check_pickup(paddleInfo->trans, config->trans,&myListItem, &angle))!=-1) { myPaddleItem.item=findItem; myPaddleItem.x =0.0; myPaddleItem.y =0.0; myPaddleItem.angle = 0.0; myListItem.item[myPaddleItem.item].onpaddle=1; } } } /* draw the item */ drawItems(config->trans,&myListItem); /* draw the paddle */ if( paddleInfo->active ){ draw_paddle(paddleInfo,&myPaddleItem); } argSwapBuffers(); } static void init( void ) { ARParam wparam; int i; /* open the video path */ if( arVideoOpen( vconf ) < 0 ) exit(0); /* find the size of the window */ if( arVideoInqSize(&xsize, &ysize) < 0 ) exit(0); printf("Image size (x,y) = (%d,%d)\n", xsize, ysize); /* set the initial camera parameters */ if( arParamLoad(cparam_name, 1, &wparam) < 0 ) { printf("Camera parameter load error !!\n"); exit(0); } arParamChangeSize( &wparam, xsize, ysize, &cparam ); arInitCparam( &cparam ); printf("*** Camera Parameter ***\n"); arParamDisp( &cparam ); /* load the paddle marker file */ if( (paddleInfo = paddleInit(paddle_name)) == NULL ) { printf("paddleInit error!!\n"); exit(0); } printf("Loaded Paddle File\n"); if( (config = arMultiReadConfigFile(config_name)) == NULL ) { printf("config data load error !!\n"); exit(0); } printf("Loaded Multi Marker File\n"); /* init items */ myListItem.itemnum=4; myListItem.item[0].pos[0]=0.;myListItem.item[0].pos[1]=0.;myListItem.item[0].onpaddle=0; myListItem.item[1].pos[0]=100.;myListItem.item[1].pos[1]=-100.;myListItem.item[1].onpaddle=0; myListItem.item[2].pos[0]=200.;myListItem.item[2].pos[1]=0.;myListItem.item[2].onpaddle=0; myListItem.item[3].pos[0]=0.;myListItem.item[3].pos[1]=0.;myListItem.item[3].onpaddle=1; /* set up the initial paddle contents */ myPaddleItem.item = 3; myPaddleItem.angle = 0.0; myPaddleItem.x = 0.0; myPaddleItem.y = 0.0; /* open the graphics window */ argInit( &cparam, 1.0, 0, 0, 0, 0 ); } /* cleanup function called when program exits */ static void cleanup(void) { arVideoCapStop(); arVideoClose(); argCleanup(); } /* find the position of the paddle card relative to the base and set the dropped blob position to this */ static void findPaddlePosition(float curPaddlePos[], double card_trans[3][4],double base_trans[3][4]) { int i,j; ARMat *mat_a, *mat_b, *mat_c; double x, y, z; //get card position relative to base pattern mat_a = arMatrixAlloc( 4, 4 ); mat_b = arMatrixAlloc( 4, 4 ); mat_c = arMatrixAlloc( 4, 4 ); for( j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) { mat_b->m[j*4+i] = base_trans[j][i]; } } mat_b->m[3*4+0] = 0.0; mat_b->m[3*4+1] = 0.0; mat_b->m[3*4+2] = 0.0; mat_b->m[3*4+3] = 1.0; for( j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) { mat_a->m[j*4+i] = card_trans[j][i]; } } mat_a->m[3*4+0] = 0.0; mat_a->m[3*4+1] = 0.0; mat_a->m[3*4+2] = 0.0; mat_a->m[3*4+3] = 1.0; arMatrixSelfInv( mat_b ); arMatrixMul( mat_c, mat_b, mat_a ); //x,y,z is card position relative to base pattern x = mat_c->m[0*4+3]; y = mat_c->m[1*4+3]; z = mat_c->m[2*4+3]; curPaddlePos[0] = x; curPaddlePos[1] = y; curPaddlePos[2] = z; // printf("Position: %3.2f %3.2f %3.2f\n",x,y,z); arMatrixFree( mat_a ); arMatrixFree( mat_b ); arMatrixFree( mat_c ); } /* draw the paddle */ int draw_paddle( ARPaddleInfo *paddleInfo, PaddleItemInfo *paddleItemInfo ) { double gl_para[16]; int i; GLfloat mat_ambient2[] = {0.0, 1.0, 1.0, 1.0}; GLfloat mat_flash2[] = {0.0, 1.0, 1.0, 1.0}; GLfloat mat_flash_shiny2[]= {50.0}; argDrawMode3D(); glEnable(GL_DEPTH_TEST); glDepthFunc(GL_LEQUAL); argDraw3dCamera( 0, 0 ); argConvGlpara(paddleInfo->trans, gl_para); glMatrixMode(GL_MODELVIEW); glLoadMatrixd( gl_para ); glColor3f( 1.0, 0.0, 0.0 ); glLineWidth(4.0); glBegin(GL_LINE_LOOP); glVertex2f( -25.0, -25.0 ); glVertex2f( 25.0, -25.0 ); glVertex2f( 25.0, 25.0 ); glVertex2f( -25.0, 25.0 ); glEnd(); glColor3f( 0.0, 0.0, 1.0); glBegin(GL_LINE_LOOP); for( i = 0; i < 16; i++ ) { double x, y; x = PADDLE_RADIUS * cos(i*3.141592*2/16); y = PADDLE_RADIUS * sin(i*3.141592*2/16); glVertex2d( x, y ); } glEnd(); glBegin(GL_LINE_LOOP); glVertex2f( -7.5, 0.0 ); glVertex2f( 7.5, 0.0 ); glVertex2f( 7.5, -105.0 ); glVertex2f( -7.5, -105.0 ); glEnd(); glEnable(GL_BLEND); glBlendFunc(GL_ZERO,GL_ONE); glColor4f(1,1,1,0); glBegin(GL_POLYGON); for( i = 0; i < 16; i++ ) { double x, y; x = 40.0 * cos(i*3.141592*2/16); y = 40.0 * sin(i*3.141592*2/16); glVertex2d( x, y ); } glEnd(); glBegin(GL_POLYGON); glVertex2f( -7.5, 0.0 ); glVertex2f( 7.5, 0.0 ); glVertex2f( 7.5, -105.0 ); glVertex2f( -7.5, -105.0 ); glEnd(); glDisable(GL_BLEND); /* draw any objects on the paddle */ if( (i=paddleItemInfo->item) !=-1) { glPushMatrix(); glTranslatef( paddleItemInfo->x, paddleItemInfo->y, 10.0 ); glRotatef( paddleItemInfo->angle * 180.0/3.141592, 0.0, 0.0, 1.0 ); glColor3f(0.0,1.0,0.0); glutSolidSphere(10,10,10); // glutSolidTeapot(10.); glPopMatrix(); } glDisable(GL_DEPTH_TEST); argDrawMode2D(); return 0; } /* draw the items on the ground */ void drawItems(double trans[3][4], ItemList* itlist) { int i; double gl_para[16]; GLfloat light_position[] = {100.0,-200.0,200.0,0.0}; GLfloat ambi[] = {0.1, 0.1, 0.1, 0.1}; GLfloat lightZeroColor[] = {0.9, 0.9, 0.9, 0.1}; GLfloat mat_ambient[] = {0.0, 1.0, 0.0, 1.0}; GLfloat mat_flash2[] = {0.0, 1.0, 1.0, 1.0}; GLfloat mat_flash_shiny2[]= {50.0}; argDrawMode3D(); argDraw3dCamera( 0, 0 ); glEnable(GL_DEPTH_TEST); glDepthFunc(GL_LEQUAL); glEnable(GL_LIGHTING); glEnable(GL_LIGHT0); glLightfv(GL_LIGHT0, GL_POSITION, light_position); glLightfv(GL_LIGHT0, GL_AMBIENT, ambi); glLightfv(GL_LIGHT0, GL_DIFFUSE, lightZeroColor); glMaterialfv(GL_FRONT, GL_SPECULAR, mat_flash2); glMaterialfv(GL_FRONT, GL_SHININESS, mat_flash_shiny2); glMaterialfv(GL_FRONT, GL_AMBIENT, mat_ambient); /* load the camera transformation matrix */ glMatrixMode(GL_MODELVIEW); argConvGlpara(trans, gl_para); glLoadMatrixd( gl_para ); for(i = 0; i < itlist->itemnum; i ++ ) { if (!itlist->item[i].onpaddle) { glPushMatrix(); glTranslatef(itlist->item[i].pos[0],itlist->item[i].pos[1], 10.0 ); glColor3f(0.0,1.0,0.0); glutSolidSphere(10,10,10); glPopMatrix(); } } glDisable( GL_LIGHTING ); glDisable( GL_DEPTH_TEST ); argDrawMode2D(); } /************************************************************************************* ** ** drawGroundGrid - draws a ground plane ** ***************************************************************************************/ int drawGroundGrid( double trans[3][4], int divisions, float x, float y, float height) { double gl_para[16]; int i; float x0,x1,y0,y1; float deltaX, deltaY; argDrawMode3D(); argDraw3dCamera( 0, 0 ); glEnable(GL_DEPTH_TEST); glDepthFunc(GL_LEQUAL); /* load the camera transformation matrix */ glMatrixMode(GL_MODELVIEW); argConvGlpara(trans, gl_para); glLoadMatrixd( gl_para ); glTranslatef(x/2.,-y/2.,0.); //draw the grid glColor3f(1,0,0); glLineWidth(6.0); glBegin(GL_LINE_LOOP); glVertex3f( -x, y, height ); glVertex3f( x, y, height ); glVertex3f( x, -y, height ); glVertex3f( -x, -y, height ); glEnd(); glLineWidth(3.0); //draw a grid of lines //X direction x0 = -x; x1 = -x; y0 = -y; y1 = y; deltaX = (2*x)/divisions; for(i=0;i<divisions;i++){ x0 = x0 + deltaX; glBegin(GL_LINES); glVertex3f(x0,y0,height); glVertex3f(x0,y1,height); glEnd(); } x0 = -x; x1 = x; deltaY = (2*y)/divisions; for(i=0;i<divisions;i++){ y0 = y0 + deltaY; glBegin(GL_LINES); glVertex3f(x0,y0,height); glVertex3f(x1,y0,height); glEnd(); } glLineWidth(1.0); glEnable(GL_LIGHTING); glEnable(GL_LIGHT0); glLightfv(GL_LIGHT0, GL_POSITION, light_position); glLightfv(GL_LIGHT0, GL_AMBIENT, ambi); glLightfv(GL_LIGHT0, GL_DIFFUSE, lightZeroColor); glDisable( GL_LIGHTING ); glDisable( GL_DEPTH_TEST ); argDrawMode2D(); return 0; } --- NEW FILE: paddleDemo.vcproj --- <?xml version="1.0" encoding="Windows-1252"?> <VisualStudioProject ProjectType="Visual C++" Version="7.10" Name="paddleDemo" ProjectGUID="{A363668A-A9B7-4CB4-89C6-67911CE040E0}"> <Platforms> <Platform Name="Win32"/> </Platforms> <Configurations> <Configuration Name="Debug|Win32" OutputDirectory="Debug" IntermediateDirectory="Debug" ConfigurationType="1"> <Tool Name="VCCLCompilerTool" Optimization="0" AdditionalIncludeDirectories="$(ProjectDir)/../../include" PreprocessorDefinitions="WIN32;_DEBUG" BasicRuntimeChecks="3" RuntimeLibrary="1" BrowseInformation="1" WarningLevel="3" DebugInformationFormat="3"/> <Tool Name="VCCustomBuildTool"/> <Tool Name="VCLinkerTool" AdditionalOptions="/DEBUG" AdditionalDependencies="libARd.lib libARMultid.lib libARgsubd.lib libARvideod.lib" OutputFile="$(ProjectDir)/../../bin/$(ProjectName)d.exe" AdditionalLibraryDirectories=""$(ProjectDir)/../../lib"" GenerateDebugInformation="TRUE"/> <Tool Name="VCMIDLTool" PreprocessorDefinitions="_DEBUG" MkTypLibCompatible="TRUE" SuppressStartupBanner="TRUE" TargetEnvironment="1"/> <Tool Name="VCPostBuildEventTool"/> <Tool Name="VCPreBuildEventTool"/> <Tool Name="VCPreLinkEventTool"/> <Tool Name="VCResourceCompilerTool"/> <Tool Name="VCWebServiceProxyGeneratorTool"/> <Tool Name="VCXMLDataGeneratorTool"/> <Tool Name="VCWebDeploymentTool"/> <Tool Name="VCManagedWrapperGeneratorTool"/> <Tool Name="VCAuxiliaryManagedWrapperGeneratorTool"/> </Configuration> <Configuration Name="Release|Win32" OutputDirectory="Release" IntermediateDirectory="Release" ConfigurationType="1" WholeProgramOptimization="TRUE"> <Tool Name="VCCLCompilerTool" GlobalOptimizations="TRUE" InlineFunctionExpansion="1" FavorSizeOrSpeed="1" AdditionalIncludeDirectories="$(ProjectDir)/../../include" PreprocessorDefinitions="WIN32;NDEBUG" RuntimeLibrary="0"/> <Tool Name="VCCustomBuildTool"/> <Tool Name="VCLinkerTool" AdditionalDependencies="libAR.lib libARMulti.lib libARgsub.lib libARvideo.lib" OutputFile="$(ProjectDir)/../../bin/$(ProjectName).exe" AdditionalLibraryDirectories=""$(ProjectDir)/../../lib""/> <Tool Name="VCMIDLTool" PreprocessorDefinitions="NDEBUG" MkTypLibCompatible="TRUE" SuppressStartupBanner="TRUE" TargetEnvironment="1"/> <Tool Name="VCPostBuildEventTool"/> <Tool Name="VCPreBuildEventTool"/> <Tool Name="VCPreLinkEventTool"/> <Tool Name="VCResourceCompilerTool"/> <Tool Name="VCWebServiceProxyGeneratorTool"/> <Tool Name="VCXMLDataGeneratorTool"/> <Tool Name="VCWebDeploymentTool"/> <Tool Name="VCManagedWrapperGeneratorTool"/> <Tool Name="VCAuxiliaryManagedWrapperGeneratorTool"/> </Configuration> </Configurations> <References> </References> <Files> <File RelativePath="paddleDemo.c"> </File> <File RelativePath="paddle.c"> </File> <File RelativePath="paddle.h"> </File> <File RelativePath="command_sub.c"> </File> <File RelativePath="command_sub.h"> </File> <File RelativePath="util.c"> </File> <File RelativePath="util.h"> </File> </Files> <Globals> </Globals> </VisualStudioProject> --- NEW FILE: command_sub.h --- #ifndef COMMAND_SUB_H #define COMMAND_SUB_H #include "paddle.h" /* on item structure */ typedef struct { double pos[2]; int onpaddle; } Item; /* list of items */ typedef struct { int itemnum; Item item[256]; } ItemList; /* item which is on the paddle */ typedef struct { int item; double angle; double x, y; } PaddleItemInfo; /* shaking gesture to remove a pattern */ int check_shake ( double paddleTrans[3][4], int f ); /* inclining gesture to put an object on the ground*/ int check_incline ( double paddleTrans[3][4], double baseTrans[3][4], double *angle ); /* picking gesture to take an object from the ground*/ int check_pickup(double card_trans[3][4], double base_trans[3][4], ItemList* itlist,float* angle); #endif |
