Subversion Repositories AndroidProjects

#ifdef _WIN32

#  include <windows.h>

#endif

#include <stdio.h>

#include <stdlib.h>

#include <string.h>

#include <math.h>

#ifndef __APPLE__

#  include <GL/glut.h>

#else

#  include <GLUT/glut.h>

#endif

#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 */

#ifdef _WIN32

char    *vconf = "Data\\WDM_camera_flipV.xml";

#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 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(int argc, char **argv)

{

        //initialize applications

        glutInit(&argc, argv);

    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;

    /* 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;

    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;

}