📄 our_math.c
字号:
{
//有一个4舍5入的过程
FIXP16 x,y,z;
x=va->M[0];
y=va->M[1];
z=va->M[2];
vprod->M[0] = (x*mb->M[0][0] + y*mb->M[1][0] + z*mb->M[2][0] +(1<<(FIXP15_SHIFT-1)))>>FIXP15_SHIFT;
vprod->M[1] = (x*mb->M[0][1] + y*mb->M[1][1] + z*mb->M[2][1] +(1<<(FIXP15_SHIFT-1)))>>FIXP15_SHIFT;
vprod->M[2] = (x*mb->M[0][2] + y*mb->M[1][2] + z*mb->M[2][2] +(1<<(FIXP15_SHIFT-1)))>>FIXP15_SHIFT;
}
//------------------------------------------------------------------------
void OUR_FIX_VECTOR3D_SUB( FIX_VECTOR3D_PTR va, //20t
FIX_VECTOR3D_PTR vb,
FIX_VECTOR3D_PTR vprod)
{
vprod->M[0] = va->M[0]-vb->M[0];
vprod->M[1] = va->M[1]-vb->M[1];
vprod->M[2] = va->M[2]-vb->M[2];
}
//------------------------------------------------------------------------
void OUR_FIX_VECTOR3D_ADD( FIX_VECTOR3D_PTR va, //20t
FIX_VECTOR3D_PTR vb,
FIX_VECTOR3D_PTR vprod)
{
vprod->M[0] = va->M[0]+vb->M[0];
vprod->M[1] = va->M[1]+vb->M[1];
vprod->M[2] = va->M[2]+vb->M[2];
}
//------------------------------------------------------------------------
void OUR_VECTOR3D_SUB( OUR_3D_POINT_PTR va,
OUR_3D_POINT_PTR vb,
OUR_3D_POINT_PTR vprod)
{
// vprod->M[0] = va->M[0]-vb->M[0];
// vprod->M[1] = va->M[1]-vb->M[1];
// vprod->M[2] = va->M[2]-vb->M[2];
}
//*********************************************************************
//P1=P0 x M
//参数1:
//return:
//注意: ma的所有值绝对值<0x8000=32768
// mb的所有值绝对值<0x8000
//*********************************************************************
void OUR_FIX_Mat_Mul_3X3( FIX_MATRIX_3X3_PTR ma, //320t
FIX_MATRIX_3X3_PTR mb,
FIX_MATRIX_3X3_PTR mprod)
{
FIXP16 x,y,z;
sint32 i;
for(i=0;i<3;i++)
{
x=ma->M[i][0];
y=ma->M[i][1];
z=ma->M[i][2];
mprod->M[i][0] = (x*mb->M[0][0] + y*mb->M[1][0] + z*mb->M[2][0]+(1<<(FIXP15_SHIFT-1)))>>FIXP15_SHIFT ;
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
{
uint32 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
{
uint32 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; //
int temp; // used for swaping
uint32 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;
sint32 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);
}
//FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF
//FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -