gsub.c

tracciatore di mani con webcam

                }

                glBegin( GL_QUADS );
                glTexCoord2d( tsx, tsy ); glVertex3d( xx1, yy1, z );
                glTexCoord2d( tex, tsy ); glVertex3d( xx2, yy2, z );
                glTexCoord2d( tex, tey ); glVertex3d( xx3, yy3, z );
                glTexCoord2d( tsx, tey ); glVertex3d( xx4, yy4, z );
                glEnd();
            }
        }
        glEndList();
        flag[mode][win][list] = 0;
    }
    else {
        glCallList( listIndex[mode][win][list]+1 );
    }

    glBindTexture( GL_TEXTURE_2D, 0 );
    glDisable( GL_TEXTURE_2D );
    glPixelStorei( GL_UNPACK_ROW_LENGTH, 0 );
#ifndef _WIN32
    glPixelStorei( GL_UNPACK_IMAGE_HEIGHT, 0 );
#endif
}

void argDispHalfImage( ARUint8 *image, int xwin, int ywin )
{
    if( argDrawMode == AR_DRAW_BY_GL_DRAW_PIXELS ) {
        argDispHalfImageDrawPixels( image, xwin, ywin );
    }
    else {
        if( xwin == 0 && ywin == 0 ) {
            glScissor(0, gWinYsize-(int)(gZoom*gImYsize),
                      (int)(gZoom*gImXsize), (int)(gZoom*gImYsize));
        }
        else {
            glScissor((xwin-1)*gMiniXsize, gWinYsize-gYsize-ywin*gMiniYsize,
                       gMiniXsize, gMiniYsize);
        }
        glEnable( GL_SCISSOR_TEST );
        /* glDisable( GL_DEPTH_TEST ); */
        argDispHalfImageTex( image, xwin, ywin, 0 );
        glDisable( GL_SCISSOR_TEST );
    }
}

static void argDispHalfImageDrawPixels( ARUint8 *image, int xwin, int ywin )
{
    int      sx, sy;
    GLfloat  zoom;

    if( xwin == 0 && ywin == 0 ) {
	zoom = gZoom * 2.0;
        sx = 0;
        sy = gWinYsize - 1;
    }
    if( xwin == 1 && ywin == 0 ) {
	zoom = gZoom * 2.0;
        sx = gXsize;
        sy = gWinYsize - 1;
    }
    else {
        zoom = gZoom / (double)GMINI * 2.0;
        sx = (xwin-1)*gMiniXsize;
        sy = gWinYsize - gYsize - (ywin-1)*gMiniYsize - 1;
    }
    glPixelZoom( zoom, -zoom);
    /* glRasterPos3i( sx, sy, 0 ); */
    glRasterPos3i( sx, sy, -1 );

#ifdef  AR_PIX_FORMAT_ABGR
    glDrawPixels( gImXsize/2, gImYsize/2, GL_ABGR, GL_UNSIGNED_BYTE, image );
#endif
#ifdef  AR_PIX_FORMAT_BGRA
    glDrawPixels( gImXsize/2, gImYsize/2, GL_BGRA, GL_UNSIGNED_BYTE, image );
#endif
#ifdef  AR_PIX_FORMAT_BGR
    glDrawPixels( gImXsize/2, gImYsize/2, GL_BGR, GL_UNSIGNED_BYTE, image );
#endif
#ifdef  AR_PIX_FORMAT_RGBA
    glDrawPixels( gImXsize/2, gImYsize/2, GL_RGBA, GL_UNSIGNED_BYTE, image );
#endif
#ifdef  AR_PIX_FORMAT_RGB
    glDrawPixels( gImXsize/2, gImYsize/2, GL_RGB, GL_UNSIGNED_BYTE, image );
#endif
}

#ifndef _WIN32
static void argDispHalfImageTex( ARUint8 *image, int xwin, int ywin, int mode )
#else
static void argDispHalfImageTex( ARUint8 *wimage, int xwin, int ywin, int mode )
#endif
{
    static int      initf = 1;
    static int      flag[3][MINIWIN_MAX+2];
    static int      listIndex[3][MINIWIN_MAX+2];
#ifdef _WIN32
    static ARUint8  *image = NULL;
#endif
    double   *dist_factor = NULL;
    double   tsx, tsy, tex, tey;
    double   px, py, qx, qy, z;
    double   x1, x2, x3, x4;
    double   y1, y2, y3, y4;
    double   xx1, xx2, xx3, xx4;
    double   yy1, yy2, yy3, yy4;
    int      win;
    int      i, j;

    switch( mode ) {
      case 0: dist_factor = &(gCparam.dist_factor[0]);   break;
    }

#ifdef _WIN32
    if( image == NULL ) {
		arMalloc(image,ARUint8,tex2Xsize*tex2Ysize*AR_PIX_SIZE);
    }
	memcpy(image, wimage, gImXsize*gImYsize*AR_PIX_SIZE/4);
#endif

    if( initf ) {
        for(j=0;j<3;j++) {
            for(i=0;i<=MINIWIN_MAX;i++) flag[j][i] = 1;
        }
        initf = 0;
    }
    if( xwin == 0 && ywin == 0 )      win = 0;
    else if( xwin == 1 && ywin == 0 ) win = 1;
    else win = gMiniXnum * (ywin-1) + xwin + 1;

    glEnable( GL_TEXTURE_2D );
    glMatrixMode(GL_TEXTURE);
    glLoadIdentity();
    glMatrixMode(GL_MODELVIEW);

    glPixelStorei( GL_UNPACK_ROW_LENGTH, gImXsize/2 );
#ifndef _WIN32
    glPixelStorei( GL_UNPACK_IMAGE_HEIGHT, gImYsize/2 );
#endif

    glBindTexture( GL_TEXTURE_2D, glid[2] );
#ifdef  AR_PIX_FORMAT_ABGR
    glTexImage2D( GL_TEXTURE_2D, 0, 3, tex2Xsize, tex2Ysize, 0, GL_ABGR, GL_UNSIGNED_BYTE, image );
#endif
#ifdef  AR_PIX_FORMAT_BGRA
    glTexImage2D( GL_TEXTURE_2D, 0, 3, tex2Xsize, tex2Ysize, 0, GL_BGRA, GL_UNSIGNED_BYTE, image );
#endif
#ifdef  AR_PIX_FORMAT_BGR
    glTexImage2D( GL_TEXTURE_2D, 0, 3, tex2Xsize, tex2Ysize, 0, GL_BGR, GL_UNSIGNED_BYTE, image );
#endif
#ifdef  AR_PIX_FORMAT_RGBA
    glTexImage2D( GL_TEXTURE_2D, 0, 3, tex2Xsize, tex2Ysize, 0, GL_RGBA, GL_UNSIGNED_BYTE, image );
#endif
#ifdef  AR_PIX_FORMAT_RGB
    glTexImage2D( GL_TEXTURE_2D, 0, 3, tex2Xsize, tex2Ysize, 0, GL_RGB, GL_UNSIGNED_BYTE, image );
#endif

    if( flag[mode][win] ) {
        listIndex[mode][win] = glGenLists(1);
        glNewList(listIndex[mode][win], GL_COMPILE_AND_EXECUTE);

        z = -1.0;
        qy = gImYsize * 0 / 20.0;
        tey = ((double)gImYsize / (double)(tex2Ysize*2.0)) * (double)0 / 20.0;
        for( j = 1; j <= 20; j++ ) {
            py = qy;
            tsy = tey;
            qy = gImYsize * j / 20.0;
            tey = ((double)gImYsize / (double)(tex2Ysize*2.0)) * (double)j / 20.0;

            qx = gImXsize * 0 / 20.0;
            tex = ((double)gImXsize / (double)(tex2Xsize*2.0)) * (double)0 / 20.0;
            for( i = 1; i <= 20; i++ ) {
                px = qx;
                tsx = tex;
                qx = gImXsize * i / 20.0;
                tex = ((double)gImXsize / (double)(tex2Xsize*2.0)) * (double)i / 20.0;

                arParamObserv2Ideal( dist_factor, px, py, &x1, &y1 );
                arParamObserv2Ideal( dist_factor, qx, py, &x2, &y2 );
                arParamObserv2Ideal( dist_factor, qx, qy, &x3, &y3 );
                arParamObserv2Ideal( dist_factor, px, qy, &x4, &y4 );

                if( xwin == 0 && ywin == 0 ) {
                    xx1 = x1 * gZoom - 1.0;
                    yy1 = (gWinYsize - 1) - y1 * gZoom;
                    xx2 = x2 * gZoom - 1.0;
                    yy2 = (gWinYsize - 1) - y2 * gZoom;
                    xx3 = x3 * gZoom - 1.0;
                    yy3 = (gWinYsize - 1) - y3 * gZoom;
                    xx4 = x4 * gZoom - 1.0;
                    yy4 = (gWinYsize - 1) - y4 * gZoom;
                }
                else if( xwin == 1 && ywin == 0 ) {
                    xx1 = gXsize + x1 * gZoom - 1.0;
                    yy1 = (gWinYsize - 1) - y1 * gZoom;
                    xx2 = gXsize + x2 * gZoom - 1.0;
                    yy2 = (gWinYsize - 1) - y2 * gZoom;
                    xx3 = gXsize + x3 * gZoom - 1.0;
                    yy3 = (gWinYsize - 1) - y3 * gZoom;
                    xx4 = gXsize + x4 * gZoom - 1.0;
                    yy4 = (gWinYsize - 1) - y4 * gZoom;
                }
                else {
                    xx1 = (xwin-1)*gMiniXsize + x1*gZoom/(double)GMINI - 1.0;
                    xx2 = (xwin-1)*gMiniXsize + x2*gZoom/(double)GMINI - 1.0;
                    xx3 = (xwin-1)*gMiniXsize + x3*gZoom/(double)GMINI - 1.0;
                    xx4 = (xwin-1)*gMiniXsize + x4*gZoom/(double)GMINI - 1.0;
                    yy1 = gWinYsize-gYsize-(ywin-1)*gMiniYsize - y1*gZoom/(double)GMINI - 1.0;
                    yy2 = gWinYsize-gYsize-(ywin-1)*gMiniYsize - y2*gZoom/(double)GMINI - 1.0;
                    yy3 = gWinYsize-gYsize-(ywin-1)*gMiniYsize - y3*gZoom/(double)GMINI - 1.0;
                    yy4 = gWinYsize-gYsize-(ywin-1)*gMiniYsize - y4*gZoom/(double)GMINI - 1.0;
                }

                glBegin( GL_QUADS );
                glTexCoord2d( tsx, tsy ); glVertex3d( xx1, yy1, z );
                glTexCoord2d( tex, tsy ); glVertex3d( xx2, yy2, z );
                glTexCoord2d( tex, tey ); glVertex3d( xx3, yy3, z );
                glTexCoord2d( tsx, tey ); glVertex3d( xx4, yy4, z );
                glEnd();
            }
        }
        glEndList();
        flag[mode][win] = 0;
    }
    else {
        glCallList( listIndex[mode][win] );
    }

    glBindTexture( GL_TEXTURE_2D, 0 );
    glDisable( GL_TEXTURE_2D );
    glPixelStorei( GL_UNPACK_ROW_LENGTH, 0 );
#ifndef _WIN32
    glPixelStorei( GL_UNPACK_IMAGE_HEIGHT, 0 );
#endif
}

void argDrawSquare( double  vertex[4][2], int xwin, int ywin )
{
    argLineSeg( vertex[0][0], vertex[0][1],
                vertex[1][0], vertex[1][1], xwin, ywin );
    argLineSeg( vertex[1][0], vertex[1][1],
                vertex[2][0], vertex[2][1], xwin, ywin );
    argLineSeg( vertex[2][0], vertex[2][1],
                vertex[3][0], vertex[3][1], xwin, ywin );
    argLineSeg( vertex[3][0], vertex[3][1],
                vertex[0][0], vertex[0][1], xwin, ywin );
}

void argLineSeg( double x1, double y1, double x2, double y2, int xwin, int ywin )
{
    float   ox, oy;
    double  xx1, yy1, xx2, yy2;

    if( argDrawMode == AR_DRAW_BY_TEXTURE_MAPPING ) {
        xx1 = x1;  yy1 = y1;
        xx2 = x2;  yy2 = y2;
    }
    else {
        arParamIdeal2Observ( gCparam.dist_factor, x1, y1, &xx1, &yy1 );
        arParamIdeal2Observ( gCparam.dist_factor, x2, y2, &xx2, &yy2 );
    }

    xx1 *= gZoom; yy1 *= gZoom;
    xx2 *= gZoom; yy2 *= gZoom;

    if( xwin == 0 && ywin == 0 ) {
        ox = 0;
        oy = gWinYsize-1;
        glBegin(GL_LINES);
          glVertex2f( ox+xx1, oy-yy1 );
          glVertex2f( ox+xx2, oy-yy2 );
        glEnd();
    }
    else {
        ox = (xwin-1)*gMiniXsize;
        oy = gWinYsize - gYsize -(ywin-1)*gMiniYsize - 1;
        glBegin(GL_LINES);
          glVertex2f( ox+xx1/GMINI, oy-yy1/GMINI );
          glVertex2f( ox+xx2/GMINI, oy-yy2/GMINI );
        glEnd();
    }

    glFlush();
}

void argLineSegHMD( double x1, double y1, double x2, double y2 )
{
    float   ox, oy;

    ox = 0;
    oy = gWinYsize - gYsize;
    glBegin(GL_LINES);
      glVertex2f( ox+x1, oy+y1 );
      glVertex2f( ox+x2, oy+y2 );
    glEnd();
}


static void argInitStencil(void)
{
    int     offset;
    int     i;

    glEnable(GL_STENCIL_TEST);
    glClearStencil(0);
    glClear(GL_STENCIL_BUFFER_BIT);
    glLineWidth(1.0);

    offset = gWinYsize - gYsize;

#if REVERSE_LR
    glStencilFunc(GL_ALWAYS, LEFTEYE, LEFTEYE);
    glStencilOp(GL_REPLACE, GL_REPLACE, GL_REPLACE);
    glBegin(GL_LINES);
    for( i = 0; i < gYsize; i+=2 ) {
        glVertex2f( 0.0,       (float)(i+offset) );
        glVertex2f( gWinXsize, (float)(i+offset) );
    }
    glEnd();

    glStencilFunc(GL_ALWAYS, RIGHTEYE, RIGHTEYE);
    glStencilOp(GL_REPLACE, GL_REPLACE, GL_REPLACE);
    glBegin(GL_LINES);
    for( i = 1; i < gYsize; i+=2 ) {
        glVertex2f( 0.0,       (float)(i+offset) );
        glVertex2f( gWinXsize, (float)(i+offset) );
    }
    glEnd();
#else
    glStencilFunc(GL_ALWAYS, LEFTEYE, LEFTEYE);
    glStencilOp(GL_REPLACE, GL_REPLACE, GL_REPLACE);
    glBegin(GL_LINES);
    for( i = 1; i < gYsize; i+=2 ) {
        glVertex2f( 0.0,       (float)(i+offset) );
        glVertex2f( gWinXsize, (float)(i+offset) );
    }
    glEnd();

    glStencilFunc(GL_ALWAYS, RIGHTEYE, RIGHTEYE);
    glStencilOp(GL_REPLACE, GL_REPLACE, GL_REPLACE);
    glBegin(GL_LINES);
    for( i = 0; i < gYsize; i+=2 ) {
        glVertex2f( 0.0,       (float)(i+offset) );
        glVertex2f( gWinXsize, (float)(i+offset) );
    }
    glEnd();
#endif

    glStencilFunc (GL_ALWAYS, 0, 0);
    glStencilOp (GL_KEEP, GL_KEEP, GL_KEEP);
    glDisable(GL_STENCIL_TEST);
}

void argLoadHMDparam( ARParam *lparam, ARParam *rparam )
{
    argConvGLcpara( lparam, AR_GL_CLIP_NEAR, AR_GL_CLIP_FAR, gl_lpara );
    argConvGLcpara( rparam, AR_GL_CLIP_NEAR, AR_GL_CLIP_FAR, gl_rpara );

    gl_hmd_para_flag = 1;
}


static void argSetStencil( int flag )
{
    if( flag == 0 ) {
        glDisable(GL_STENCIL_TEST);
        glStencilFunc (GL_ALWAYS, 0, 0);
        glStencilOp (GL_KEEP, GL_KEEP, GL_KEEP);
    }
    else {
        glEnable(GL_STENCIL_TEST);
        glStencilFunc (GL_EQUAL, flag, flag);
        glStencilOp (GL_KEEP, GL_KEEP, GL_KEEP);
    }
}

void argConvGLcpara( ARParam *param, double gnear, double gfar, double m[16] )
{
    argConvGLcpara2( param->mat, param->xsize, param->ysize, gnear, gfar, m );
}

static void argConvGLcpara2( double cparam[3][4], int width, int height, double gnear, double gfar, double m[16] )
{
    double   icpara[3][4];
    double   trans[3][4];
    double   p[3][3], q[4][4];
    int      i, j;

    if( arParamDecompMat(cparam, icpara, trans) < 0 ) {
        printf("gConvGLcpara: Parameter error!!\n");
        exit(0);
    }

    for( i = 0; i < 3; i++ ) {
        for( j = 0; j < 3; j++ ) {
            p[i][j] = icpara[i][j] / icpara[2][2];
        }
    }
    q[0][0] = (2.0 * p[0][0] / width);
    q[0][1] = (2.0 * p[0][1] / width);
    q[0][2] = ((2.0 * p[0][2] / width)  - 1.0);
    q[0][3] = 0.0;

    q[1][0] = 0.0;
    q[1][1] = (2.0 * p[1][1] / height);
    q[1][2] = ((2.0 * p[1][2] / height) - 1.0);
    q[1][3] = 0.0;

    q[2][0] = 0.0;
    q[2][1] = 0.0;
    q[2][2] = (gfar + gnear)/(gfar - gnear);
    q[2][3] = -2.0 * gfar * gnear / (gfar - gnear);

    q[3][0] = 0.0;
    q[3][1] = 0.0;
    q[3][2] = 1.0;
    q[3][3] = 0.0;

    for( i = 0; i < 4; i++ ) {
        for( j = 0; j < 3; j++ ) {
            m[i+j*4] = q[i][0] * trans[0][j]
                     + q[i][1] * trans[1][j]
                     + q[i][2] * trans[2][j];
        }
        m[i+3*4] = q[i][0] * trans[0][3]
                 + q[i][1] * trans[1][3]
                 + q[i][2] * trans[2][3]
                 + q[i][3];
    }
}