our_math.c

这是一个简单的3d动画mmi,这是模拟器上的

        mprod->M[i][1] =  (x*mb->M[0][1] + y*mb->M[1][1] + z*mb->M[2][1]+(1<<(FIXP15_SHIFT-1)))>>FIXP15_SHIFT ;
        mprod->M[i][2] =  (x*mb->M[0][2] + y*mb->M[1][2] + z*mb->M[2][2]+(1<<(FIXP15_SHIFT-1)))>>FIXP15_SHIFT ;
    }
} 
//------------------------------------------------------------------------

/****************************************/
/*Function: 开根号处理                  */
/*入口参数：被开方数，长整型            */
/*出口参数：开方结果，整型              */
/****************************************/
unsigned int OUR_sqrt_16(unsigned long M)				//250t
{
    unsigned int N, i;
    unsigned long tmp, ttp;   	// 结果、循环计数
    if (M == 0)               		// 被开方数，开方结果也为0
        return 0;
    N = 0;
    tmp = (M >> 30);          	// 获取最高位：B[m-1]
    M <<= 2;
    if (tmp > 1)              		// 最高位为1
    {
        N ++;                 		// 结果当前位为1，否则为默认的0
        tmp -= N;
    }
    for (i=15; i>0; i--)      		// 求剩余的15位
    {
        N <<= 1;              		// 左移一位
        tmp <<= 2;
        tmp += (M >> 30);     	// 假设
        ttp = N;
        ttp = (ttp<<1)+1;
        M <<= 2;
        if (tmp >= ttp)       		// 假设成立
        {
            tmp -= ttp;
            N ++;
        }
    }
    return N;
}
//---------------------------------------------------------------

void OUR_VECTOR3D_Normalize(FIX_VECTOR3D_PTR va)	//400t
{
    UINT	length,length_inv;
    while(OUR_int_abs(va->MM.x) >0x4000 ||	//本来的模不能太大,可能平方后加起溢出
        OUR_int_abs(va->MM.y) >0x4000 ||
        OUR_int_abs(va->MM.z) >0x4000 )
    {
        va->MM.x >>=1;
        va->MM.y >>=1;
        va->MM.z >>=1;
    }
    length = OUR_sqrt_16(va->MM.x*va->MM.x + va->MM.y*va->MM.y + va->MM.z*va->MM.z);//250t
    if (length ==0) 
        return;
    length_inv =1<<31;								// can't use 0xffffffff, cause va->x is always < 0x80000000
    length_inv /=length;
    
    // compute normalized version of vector
    va->MM.x=(va->MM.x*length_inv);
    va->MM.x =( va->MM.x) >>(31-FIXP15_SHIFT);
    
    va->MM.y=(va->MM.y*length_inv);
    va->MM.y =( va->MM.y) >>(31-FIXP15_SHIFT);
    
    va->MM.z=(va->MM.z*length_inv);
    va->MM.z =( va->MM.z) >>(31-FIXP15_SHIFT);
}
//---------------------------------------------------------------

void OUR_FAST_Normalize(FIX_VECTOR3D_PTR va)	//200t
{
    UINT	length,length_inv;
    
    length =OUR_FAST_Length(va);
    if (length ==0) 
        return;
    //length_inv =1<<31;								// can't use 0xffffffff, cause va->x is always < 0x80000000
    length_inv= (1<<31)/length;
    
    // compute normalized version of vector
    va->MM.x=(va->MM.x*length_inv);
    va->MM.x =( va->MM.x) >>(31-FIXP15_SHIFT);
    
    va->MM.y=(va->MM.y*length_inv);
    va->MM.y =( va->MM.y) >>(31-FIXP15_SHIFT);
    
    va->MM.z=(va->MM.z*length_inv);
    va->MM.z =( va->MM.z) >>(31-FIXP15_SHIFT);
}

//---------------------------------------------------------------

FIXP16 OUR_VECTOR3D_Dot(FIX_VECTOR3D_PTR va, FIX_VECTOR3D_PTR vb)//40t
{
    return( (va->MM.x * vb->MM.x) + (va->MM.y * vb->MM.y) + (va->MM.z * vb->MM.z) );
}

//---------------------------------------------------------------

void OUR_VECTOR3D_Cross(FIX_VECTOR3D_PTR va, //80t
                        FIX_VECTOR3D_PTR vb,
                        FIX_VECTOR3D_PTR vn)
{
    
    vn->MM.x =  ( (va->MM.y * vb->MM.z) - (va->MM.z * vb->MM.y) );
    vn->MM.y = -( (va->MM.x * vb->MM.z) - (va->MM.z * vb->MM.x) );
    vn->MM.z =  ( (va->MM.x * vb->MM.y) - (va->MM.y * vb->MM.x) ); 
    
} 
//--------------------------------------------------------------------


//---------------------------------------------------------------

int OUR_FAST_Length(FIX_VECTOR3D_PTR va)	//50t // compute distance with 2% error--writed by mjb on 2008/03/15
{	
    int i=10;  	// 
    UINT temp;  	// used for swaping
    UINT x,y,z; 	// used for algorithm
    
    // make sure values are all positive
    x = OUR_int_abs(va->MM.x);
    y = OUR_int_abs(va->MM.y);
    z = OUR_int_abs(va->MM.z);
    
    // sort values
    if (y < x) OUR_SWAP(x,y,temp)
        if (z < y) OUR_SWAP(y,z,temp)
            if (y < x) OUR_SWAP(x,y,temp)
                
                while(z>(1<<20))
                {
                    x>>=1;
                    y>>=1;
                    z>>=1;
                    i--;
                }
                if(x<(y>>1))
                {
                    temp=(1024*y+195*x)>>10;
                }
                else
                {
                    temp=(829*y+608*x)>>10;
                }
                if(temp<(z>>1))
                {
                    return (1024*z+195*temp)>>(i);
                }
                else
                {
                    return (829*z+608*temp)>>(i);
                }
} 
//--------------------------------------------------------------------

int OUR_Mat_Inverse_3X3(FIX_MATRIX_3X3_PTR m, FIX_MATRIX_3X3_PTR mi)//400t
{
    // this function computes the inverse of a 3x3
    // first compute the determinate to see if there is 
    // an inverse
    //FIX_MATRIX_3X3	mb;
    int det_inv,det;
    
    det = 	 (m->MM.M00>>5)*((m->MM.M11*m->MM.M22 - m->MM.M21*m->MM.M12)>>10) - //80t
        (m->MM.M01>>5)*((m->MM.M10*m->MM.M22 - m->MM.M20*m->MM.M12)>>10) + 
        (m->MM.M02>>5)*((m->MM.M10*m->MM.M21 - m->MM.M20*m->MM.M11)>>10);
    
    if (det==0)
        return(0);
    
    // compute inverse to save divides
    det_inv = (1<<30)/(det>>15);										//80t
#define I_LEFT_MOVE 15
    // compute inverse using m-1 = adjoint(m)/det(m)
    mi->MM.M00 =  (det_inv*((m->MM.M11*m->MM.M22 - m->MM.M21*m->MM.M12)>>15)+(1<<(I_LEFT_MOVE-1)))>>I_LEFT_MOVE;
    mi->MM.M10 = (-det_inv*((m->MM.M10*m->MM.M22 - m->MM.M20*m->MM.M12)>>15)+(1<<(I_LEFT_MOVE-1)))>>I_LEFT_MOVE;
    mi->MM.M20 =  (det_inv*((m->MM.M10*m->MM.M21 - m->MM.M20*m->MM.M11)>>15)+(1<<(I_LEFT_MOVE-1)))>>I_LEFT_MOVE;
    
    mi->MM.M01 = (-det_inv*((m->MM.M01*m->MM.M22 - m->MM.M21*m->MM.M02)>>15)+(1<<(I_LEFT_MOVE-1)))>>I_LEFT_MOVE;
    mi->MM.M11 =  (det_inv*((m->MM.M00*m->MM.M22 - m->MM.M20*m->MM.M02)>>15)+(1<<(I_LEFT_MOVE-1)))>>I_LEFT_MOVE;
    mi->MM.M21 = (-det_inv*((m->MM.M00*m->MM.M21 - m->MM.M20*m->MM.M01)>>15)+(1<<(I_LEFT_MOVE-1)))>>I_LEFT_MOVE;
    
    mi->MM.M02 =  (det_inv*((m->MM.M01*m->MM.M12 - m->MM.M11*m->MM.M02)>>15)+(1<<(I_LEFT_MOVE-1)))>>I_LEFT_MOVE;
    mi->MM.M12 = (-det_inv*((m->MM.M00*m->MM.M12 - m->MM.M10*m->MM.M02)>>15)+(1<<(I_LEFT_MOVE-1)))>>I_LEFT_MOVE;
    mi->MM.M22 =  (det_inv*((m->MM.M00*m->MM.M11 - m->MM.M10*m->MM.M01)>>15)+(1<<(I_LEFT_MOVE-1)))>>I_LEFT_MOVE;
    
    // return success
    return(1);
    
} 
//MMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMM
//MMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMM


void OUR_mmcpy(UINT *des,UINT *src,UINT num)
{
    UINT *max_des;
    //des&=~0x3;
    //src&=~0x3;
    max_des = des+(num);
    for (;des<max_des;)
    {
        *des++=*src++;
    }
}

void OUR_mmset(UINT *des,UINT src,UINT num)
{
    UINT *max_des;
    //des&=~0x3;
    //src&=~0x3;
    max_des = des+(num);
    for (;des<max_des;)
    {
        *des++=src;
    }
}
void OUR_mmset_box16(int x,int y,int w,int h,UCHAR *video_buffer,int lpitch,CLR c)
{
    UINT *des=(UINT*)video_buffer+((y*lpitch+x)>>1);
    UINT *max_des=des+((h*lpitch)>>1);
    int i;
    lpitch=lpitch>>1;
    max_des=des+((h*lpitch));
    //des&=~0x3;
    //src&=~0x3;
    c=(c<<16)+c;
    w=w>>1;
    for (;des<max_des;)
    {
        for(i=0;i<w;i++)
        {
            des[i]=c;
        }
        des+=lpitch;
    }
}
void OUR_BOX_OUT_clear16(int x1,int y1,
                                        CLR C,
                                        UCHAR *video_buffer, int lpitch)                                        
{
    UINT *des=(UINT*)video_buffer;
    int i;
    //C=OUR_RGB32_2_16(C);
    C=(C<<16)+C;
    //the top side
    OUR_mmset(des,C,y1*OUR_D_screem_w/2);
    //the button side
    des=(UINT*)video_buffer+((OUR_D_screem_h-y1-1)*lpitch)/2;
    OUR_mmset(des,C,y1*OUR_D_screem_w/2);
    //the right side
    des=(UINT*)video_buffer+((OUR_D_screem_w-1-x1))/2+((y1-1)*lpitch)/2;
    //C=0xffffffff;
    for(i=0;i<=OUR_D_screem_h-2*y1;i++)
    {
        OUR_mmset(des,C,x1);
        des+=lpitch/2;
    }
}

//FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF
//FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF
#endif
#endif