Subversion Repositories AndroidProjects

#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, double* 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;

}