1142 lines
27 KiB
C
1142 lines
27 KiB
C
|
/*
|
|||
|
|
* Created: 2023/03/20
|
|||
|
|
* Author: wangzk zhengmengxing
|
|||
|
|
*/
|
|||
|
|
#include "..\PrjCommon\CommonDef.h"
|
|||
|
|
#include "AttMath.h"
|
|||
|
|
/****************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>[-1<EFBFBD><EFBFBD>1]
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*****************************************************/
|
|||
|
|
TYPE_CAL PoseLimitInOne(const TYPE_CAL X)
|
|||
|
|
{
|
|||
|
|
if (X > 1.0f)
|
|||
|
|
return 1.0f;
|
|||
|
|
if (X < -1.0f)
|
|||
|
|
return -1.0f;
|
|||
|
|
return X;
|
|||
|
|
}
|
|||
|
|
/****************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <EFBFBD><EFBFBD>ƽ<EFBFBD><EFBFBD> <EFBFBD><EFBFBD>Сģʽʹ<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*****************************************************/
|
|||
|
|
//#ifdef MINMODULE_TYPE
|
|||
|
|
//TYPE_CAL Posesqrt( TYPE_CAL m )
|
|||
|
|
//{
|
|||
|
|
//TYPE_CAL x,y;
|
|||
|
|
//INT8 Li;
|
|||
|
|
//
|
|||
|
|
//if(m<0.0F) /*С<><D0A1>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
|||
|
|
//{ return 0.0F; }
|
|||
|
|
//else
|
|||
|
|
//{
|
|||
|
|
//x=1.0F;
|
|||
|
|
//y=(x+m/x)/2.0F;
|
|||
|
|
//Li = 0x0;
|
|||
|
|
//while( Li < 20 )
|
|||
|
|
//{
|
|||
|
|
//x=y;
|
|||
|
|
//y=(x+m/x)/2.0F;
|
|||
|
|
//Li++;
|
|||
|
|
//}
|
|||
|
|
//return x;
|
|||
|
|
//}
|
|||
|
|
//}
|
|||
|
|
//#endif
|
|||
|
|
/****************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>:<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ <EFBFBD><EFBFBD>Сģʽʹ<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*****************************************************/
|
|||
|
|
//#ifdef MINMODULE_TYPE
|
|||
|
|
//TYPE_CAL Poseabsf( TYPE_CAL x )
|
|||
|
|
//{
|
|||
|
|
//TYPE_CAL y;
|
|||
|
|
//if( x > 0.0F )
|
|||
|
|
//{ y = x; }
|
|||
|
|
//else
|
|||
|
|
//{ y = -x; }
|
|||
|
|
//return y; /* <20><><EFBFBD><EFBFBD>ֵ */
|
|||
|
|
//}
|
|||
|
|
//#endif
|
|||
|
|
/****************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>:<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD>Сģʽʹ<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*****************************************************/
|
|||
|
|
//#ifdef MINMODULE_TYPE
|
|||
|
|
//TYPE_CAL Poseacosf(TYPE_CAL x)
|
|||
|
|
//{
|
|||
|
|
//INT8 Li;
|
|||
|
|
//TYPE_CAL Lx, Ly, Lz;
|
|||
|
|
//TYPE_CAL LM[11] ={ 3.141593F, 2.498091F, 2.214298F, 1.982313F, 1.772154F, 1.570796F, 1.369438F, 1.159279F, 0.927295F, 0.643501F, 0.0F };
|
|||
|
|
//
|
|||
|
|
//Ly = x + 1.0F;
|
|||
|
|
//Lz = Ly/0.2F;
|
|||
|
|
//Li = (INT8)(Lz);
|
|||
|
|
//Lx = LM[Li]+( LM[Li+1]-LM[Li] )*( Lz - Li );
|
|||
|
|
//
|
|||
|
|
//return Lx;
|
|||
|
|
//}
|
|||
|
|
//#endif
|
|||
|
|
/****************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>: ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD> 1
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD>1<EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵΪ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>-1<EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵΪ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵΪ<EFBFBD><EFBFBD>
|
|||
|
|
*****************************************************/
|
|||
|
|
char CalPFlag(TYPE_CAL Data)
|
|||
|
|
{
|
|||
|
|
char tmpRes;
|
|||
|
|
|
|||
|
|
tmpRes = 0;
|
|||
|
|
|
|||
|
|
if(POSE_ZERO < Data)
|
|||
|
|
{
|
|||
|
|
tmpRes = 1;
|
|||
|
|
}
|
|||
|
|
else if(-POSE_ZERO > Data)
|
|||
|
|
{
|
|||
|
|
tmpRes = -1;
|
|||
|
|
}else{;}
|
|||
|
|
|
|||
|
|
return tmpRes;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*******************************************
|
|||
|
|
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Nάʸ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 2
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺 x1<EFBFBD><EFBFBD>x2:ʸ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊucn<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵre
|
|||
|
|
ucN<EFBFBD><EFBFBD>ʸ<EFBFBD><EFBFBD><EFBFBD>ij<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>˽<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
ע<EFBFBD>⣺ <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*******************************************/
|
|||
|
|
void Posedot(TYPE_CAL* x1, TYPE_CAL* x2, UINT8 ucN, TYPE_CAL * pRes)
|
|||
|
|
{
|
|||
|
|
UINT8 i;
|
|||
|
|
TYPE_CAL fReturn;
|
|||
|
|
|
|||
|
|
fReturn = 0.0F;
|
|||
|
|
|
|||
|
|
for(i=0;i<ucN;i++)
|
|||
|
|
{
|
|||
|
|
fReturn += x1[i]*x2[i];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
*pRes = fReturn;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*******************************************
|
|||
|
|
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD>3άʸ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 3
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺 x1<EFBFBD><EFBFBD>x2:ʸ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊucn<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵre
|
|||
|
|
ucN<EFBFBD><EFBFBD>ʸ<EFBFBD><EFBFBD><EFBFBD>ij<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>x3<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˽<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
ע<EFBFBD>⣺ <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч
|
|||
|
|
*******************************************/
|
|||
|
|
void cross(TYPE_CAL * x1, TYPE_CAL * x2, TYPE_CAL * x3)
|
|||
|
|
{
|
|||
|
|
x3[0] = x1[1]*x2[2]-x1[2]*x2[1];
|
|||
|
|
x3[1] = x1[2]*x2[0]-x1[0]*x2[2];
|
|||
|
|
x3[2] = x1[0]*x2[1]-x1[1]*x2[0];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*******************************************
|
|||
|
|
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Nάʸ<EFBFBD><EFBFBD>ģֵ 4
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺 xʸ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊucn<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵre
|
|||
|
|
ucN<EFBFBD><EFBFBD>ʸ<EFBFBD><EFBFBD><EFBFBD>ij<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD>ʸ<EFBFBD><EFBFBD>ģ
|
|||
|
|
ע<EFBFBD>⣺ <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*******************************************/
|
|||
|
|
void CalNormal( TYPE_CAL* x, UINT8 ucN, TYPE_CAL * pRes )
|
|||
|
|
{
|
|||
|
|
TYPE_CAL ftmp, tmpX;
|
|||
|
|
UINT8 Li = 0x00;
|
|||
|
|
ftmp = 0.0;
|
|||
|
|
tmpX = 0.0;
|
|||
|
|
|
|||
|
|
for( Li=0; Li<ucN; Li++ )
|
|||
|
|
{
|
|||
|
|
tmpX = x[Li];
|
|||
|
|
ftmp += tmpX*tmpX;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
ftmp = POSE_SQRTF(ftmp);
|
|||
|
|
*pRes = (TYPE_CAL)ftmp;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/****************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Խ<EFBFBD><EFBFBD><EFBFBD> 5
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺 inLine : ʸ<EFBFBD><EFBFBD>
|
|||
|
|
iLen<EFBFBD><EFBFBD> ʸ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>outMatrix :<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
ע<EFBFBD>⣬ָ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>洢<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD>㹻<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Խ<EFBFBD><EFBFBD>,
|
|||
|
|
*****************************************************/
|
|||
|
|
void CalMakeCrossMatrix(TYPE_CAL * inLine, TYPE_CAL * outMatrix, UINT16 iLen)
|
|||
|
|
{
|
|||
|
|
UINT16 i;
|
|||
|
|
memset(outMatrix, 0, iLen * iLen * sizeof(TYPE_CAL));
|
|||
|
|
for(i = 0; i < iLen; ++i)
|
|||
|
|
{
|
|||
|
|
outMatrix[i * iLen + i] = inLine[i];
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*********************************************
|
|||
|
|
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> [h l]*[l*f] 6
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺 A1,A2,<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
H,L,F<EFBFBD><EFBFBD>LΪ<EFBFBD><EFBFBD>ͬά<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>A3<EFBFBD><EFBFBD><EFBFBD><EFBFBD>H * F
|
|||
|
|
ע<EFBFBD>⣺ <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*******************************************/
|
|||
|
|
void MatrixProductHL( TYPE_CAL *A1, TYPE_CAL *A2, TYPE_CAL *A3, UINT8 H, UINT8 L, UINT8 F )
|
|||
|
|
{
|
|||
|
|
UINT8 i,j,k;
|
|||
|
|
|
|||
|
|
memset( &A3[0], 0, H*F*4 );
|
|||
|
|
for (i=0; i<H; i++)
|
|||
|
|
{
|
|||
|
|
for(k=0; k<F; k++)
|
|||
|
|
{
|
|||
|
|
for (j=0; j<L; j++)
|
|||
|
|
{ A3[F*i + k] += A1[L*i + j]*A2[F*j+k]; }
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/********************************************
|
|||
|
|
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD>H Lά<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD> 7
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺 A1,<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>H* L<EFBFBD><EFBFBD>
|
|||
|
|
H,L,ά<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>A2<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>L* H<EFBFBD><EFBFBD>
|
|||
|
|
ע<EFBFBD>⣺ <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*******************************************/
|
|||
|
|
void MatrixTransposeHL(TYPE_CAL * A1, TYPE_CAL * A2, UINT8 H, UINT8 L )
|
|||
|
|
{
|
|||
|
|
UINT8 m, n;
|
|||
|
|
|
|||
|
|
for( m=0; m<L; m++ )
|
|||
|
|
{
|
|||
|
|
for( n=0; n<H; n++ )
|
|||
|
|
{ A2[m*H+n] = A1[n*L+m];}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/****************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> : <EFBFBD><EFBFBD>ά<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 8
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD> cI: ԭʼʵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
ef: <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD> cs<EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־
|
|||
|
|
cO<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
ע<EFBFBD>⣺
|
|||
|
|
*****************************************************/
|
|||
|
|
void InvsymMatrix3(TYPE_CAL *cI, TYPE_CAL *cO, UINT8 *cs, TYPE_CAL * ef)
|
|||
|
|
{
|
|||
|
|
TYPE_CAL A[3][3];
|
|||
|
|
UINT8 j;
|
|||
|
|
TYPE_CAL x,n;
|
|||
|
|
TYPE_CAL tmpef;
|
|||
|
|
|
|||
|
|
tmpef = *ef;
|
|||
|
|
n= 0.0F;
|
|||
|
|
memcpy( &A[0][0], cI, 9 * sizeof(TYPE_CAL));
|
|||
|
|
*cs = POSE_OK;
|
|||
|
|
|
|||
|
|
for(j = 0;j < 3;j++)
|
|||
|
|
{
|
|||
|
|
if(j == 2)
|
|||
|
|
n += A[0][j] * A[1][0] * A[2][1];
|
|||
|
|
else if(j == 1)
|
|||
|
|
n += A[0][j] * A[1][j+1] * A[2][0];
|
|||
|
|
else
|
|||
|
|
n += A[0][j] * A[1][j+1] * A[2][j+2];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
for(j = 0;j < 3;j++)
|
|||
|
|
{
|
|||
|
|
if(j == 2)
|
|||
|
|
n -= A[2][j] * A[1][0] * A[0][1];
|
|||
|
|
else if(j==1)
|
|||
|
|
n -= A[2][j] * A[1][j+1] * A[0][0];
|
|||
|
|
else
|
|||
|
|
n -= A[2][j] * A[1][j+1] * A[0][j+2];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if( POSE_ABSF(n) > tmpef )
|
|||
|
|
{
|
|||
|
|
x = 1.0F/n;
|
|||
|
|
|
|||
|
|
cO[0] = ( A[1][1] * A[2][2] - A[1][2] * A[2][1] )*x;
|
|||
|
|
cO[4] = ( A[0][0] * A[2][2] - A[0][2] * A[2][0] )*x;
|
|||
|
|
cO[8] = ( A[0][0] * A[1][1] - A[0][1] * A[1][0] )*x;
|
|||
|
|
|
|||
|
|
cO[1] = (-A[0][1] * A[2][2] + A[0][2] * A[2][1] )*x;
|
|||
|
|
cO[2] = ( A[0][1] * A[1][2] - A[0][2] * A[1][1] )*x;
|
|||
|
|
cO[5] = (-A[0][0] * A[1][2] + A[0][2] * A[1][0] )*x;
|
|||
|
|
|
|||
|
|
cO[3] = (-A[1][0] * A[2][2] + A[2][0] * A[1][2] )*x;
|
|||
|
|
cO[6] = ( A[1][0] * A[2][1] - A[2][0] * A[1][1] )*x;
|
|||
|
|
cO[7] = (-A[0][0] * A[2][1] + A[2][0] * A[0][1] )*x;
|
|||
|
|
}
|
|||
|
|
else { *cs = POSE_NO; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/****************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Բ<EFBFBD>ֵ<EFBFBD><EFBFBD>ʽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ 9
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺 x <EFBFBD><EFBFBD>X<EFBFBD><EFBFBD>ʸ<EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
y<EFBFBD><EFBFBD> Y <EFBFBD><EFBFBD>ʸ<EFBFBD><EFBFBD>
|
|||
|
|
iLen<EFBFBD><EFBFBD>ʸ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
Xi<EFBFBD><EFBFBD> <EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD>ֵ<EFBFBD>ĵ<EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD><EFBFBD>õ<EFBFBD><EFBFBD>IJ<EFBFBD>ֵ
|
|||
|
|
ע<EFBFBD>⣺ <EFBFBD><EFBFBD><EFBFBD><EFBFBD>x<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*****************************************************/
|
|||
|
|
//#ifndef MINMODULE_TYPE
|
|||
|
|
//void CalInterPolation(TYPE_CAL * x, TYPE_CAL * y, UINT32 iLen, TYPE_CAL *Xi,TYPE_CAL* tmpTF)
|
|||
|
|
//{
|
|||
|
|
//TYPE_CAL result = 0;
|
|||
|
|
////TYPE_CAL Xm[3];
|
|||
|
|
//TYPE_CAL tmpXi;
|
|||
|
|
//UINT32 i;
|
|||
|
|
//UINT32 tmpPos;
|
|||
|
|
//
|
|||
|
|
//tmpXi = *Xi;
|
|||
|
|
//
|
|||
|
|
//if(0 == iLen)
|
|||
|
|
//{
|
|||
|
|
//result = 0;
|
|||
|
|
//}
|
|||
|
|
//else if(1 == iLen)
|
|||
|
|
//{
|
|||
|
|
//result = y[0];
|
|||
|
|
//}
|
|||
|
|
//else if(2 == iLen)
|
|||
|
|
//{
|
|||
|
|
//if(CalPFlag(x[0] - x[1]) != 0)
|
|||
|
|
//{
|
|||
|
|
//result = (y[0]*( tmpXi - x[1]) - y[1] * (tmpXi - x[0])) / (x[0] - x[1]);
|
|||
|
|
//}
|
|||
|
|
//else
|
|||
|
|
//result = y[0];
|
|||
|
|
//}
|
|||
|
|
//else
|
|||
|
|
//{
|
|||
|
|
//if(tmpXi <= x[0])
|
|||
|
|
//{
|
|||
|
|
//tmpPos = 0;
|
|||
|
|
//}
|
|||
|
|
//else if(tmpXi >= x[iLen - 1])
|
|||
|
|
//{
|
|||
|
|
//tmpPos = iLen - 2;
|
|||
|
|
//}
|
|||
|
|
//else
|
|||
|
|
//{
|
|||
|
|
//tmpPos = 0;
|
|||
|
|
///*<2A><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>λ<EFBFBD><CEBB>*/
|
|||
|
|
//for(i = 1; i < iLen; ++i)
|
|||
|
|
//{
|
|||
|
|
//if(tmpXi <= x[i])
|
|||
|
|
//{
|
|||
|
|
///*ȷ<><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD>*/
|
|||
|
|
//tmpPos = i - 1;
|
|||
|
|
//break;
|
|||
|
|
//}
|
|||
|
|
//}
|
|||
|
|
//}
|
|||
|
|
//
|
|||
|
|
//if(CalPFlag(x[tmpPos] - x[tmpPos + 1]) != 0)
|
|||
|
|
//{
|
|||
|
|
//result = y[tmpPos] * (tmpXi - x[tmpPos + 1]) - y[tmpPos + 1] * (tmpXi - x[tmpPos]);
|
|||
|
|
//result = result / (x[tmpPos] - x[tmpPos + 1]);
|
|||
|
|
//}
|
|||
|
|
//else
|
|||
|
|
//result = y[tmpPos];
|
|||
|
|
//}
|
|||
|
|
//*tmpTF = result;
|
|||
|
|
//}
|
|||
|
|
//#endif
|
|||
|
|
|
|||
|
|
/****************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʸ<EFBFBD><EFBFBD><EFBFBD>н<EFBFBD> 10
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺 x <EFBFBD><EFBFBD>ʸ<EFBFBD><EFBFBD>
|
|||
|
|
y<EFBFBD><EFBFBD> ʸ<EFBFBD><EFBFBD>
|
|||
|
|
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD>pRet
|
|||
|
|
*****************************************************/
|
|||
|
|
UINT8 CalVecArc(TYPE_CAL * VecX, TYPE_CAL * VecY, TYPE_CAL *pRet)
|
|||
|
|
{
|
|||
|
|
TYPE_CAL tmpModL1, tmpModL2, tmpDot, tmpUse;
|
|||
|
|
|
|||
|
|
if((NULL == VecX) || (NULL == VecY) || (NULL == pRet))
|
|||
|
|
return (1);
|
|||
|
|
CalNormal(VecX, 3, &tmpModL1);
|
|||
|
|
CalNormal(VecY, 3, &tmpModL2);
|
|||
|
|
Posedot(VecX, VecY, 3, &tmpDot);
|
|||
|
|
|
|||
|
|
if(POSE_ZERO > tmpModL1 * tmpModL2)
|
|||
|
|
return (1);
|
|||
|
|
|
|||
|
|
tmpUse = tmpDot / (tmpModL1 * tmpModL2);
|
|||
|
|
|
|||
|
|
if(POSE_ABSF(tmpUse) > 1.0F)
|
|||
|
|
{
|
|||
|
|
if(1 == CalPFlag(tmpUse))
|
|||
|
|
{
|
|||
|
|
tmpUse = 1.0F;
|
|||
|
|
}
|
|||
|
|
else if(-1 == CalPFlag(tmpUse) )
|
|||
|
|
{
|
|||
|
|
tmpUse = -1.0F;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
*pRet = POSE_ACOSF(tmpUse);
|
|||
|
|
|
|||
|
|
return (0);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/******************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD> Nάʸ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶദ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*****************************************************/
|
|||
|
|
void unitary(TYPE_CAL* pVector, UINT8 ucN)
|
|||
|
|
{
|
|||
|
|
UINT8 i;
|
|||
|
|
TYPE_CAL ftemp;
|
|||
|
|
|
|||
|
|
CalNormal( pVector, ucN, &ftemp); /* ȡʸ<C8A1><CAB8><EFBFBD><EFBFBD>ģ*/
|
|||
|
|
if (ftemp > POSE_ZERO)
|
|||
|
|
{
|
|||
|
|
for (i=0; i<ucN; i++)
|
|||
|
|
{ pVector[i] = pVector[i]/ftemp; } /* ʸ<><CAB8><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>*/
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/***********************************************
|
|||
|
|
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Nά<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺N<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ά<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>A<EFBFBD><EFBFBD>N * Nά<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
************************************************/
|
|||
|
|
//#ifndef MINMODULE_TYPE
|
|||
|
|
//void MatrixIcons( TYPE_CAL *A, UINT8 N )
|
|||
|
|
//{
|
|||
|
|
//UINT8 i;
|
|||
|
|
//memset( &A[0], 0, N*N*4 );
|
|||
|
|
//
|
|||
|
|
//for( i=0; i<N; i++ )
|
|||
|
|
//{ A[i*N+i] = 1.0F; } /*<2A>Խ<EFBFBD><D4BD><EFBFBD>Ԫ<EFBFBD><D4AA>*/
|
|||
|
|
//}
|
|||
|
|
//#endif
|
|||
|
|
|
|||
|
|
/****************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>: n<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӵ<EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺 cd<EFBFBD><EFBFBD>ʸ<EFBFBD><EFBFBD>
|
|||
|
|
n<EFBFBD><EFBFBD>ʸ<EFBFBD><EFBFBD>ά<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> cd:<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɵ<EFBFBD>ʸ<EFBFBD><EFBFBD>
|
|||
|
|
*****************************************************/
|
|||
|
|
void SortUint16( UINT16 *cd, UINT8 n )
|
|||
|
|
{
|
|||
|
|
UINT8 Li, Lj;
|
|||
|
|
UINT16 tmpf;
|
|||
|
|
for( Li=0; Li<n; Li++ )
|
|||
|
|
{
|
|||
|
|
for( Lj=0; Lj < n-Li-1; Lj++ )
|
|||
|
|
{
|
|||
|
|
if( cd[Lj]<cd[Lj+1] )
|
|||
|
|
{
|
|||
|
|
tmpf = cd[Lj];
|
|||
|
|
cd[Lj] = cd[Lj+1];
|
|||
|
|
cd[Lj+1] = tmpf;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/********************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD>MNά<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Nʸ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺pMatrix<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>M * N
|
|||
|
|
pVector:ʸ<EFBFBD><EFBFBD>Nά
|
|||
|
|
pReturn<EFBFBD><EFBFBD>Mάʸ<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD>
|
|||
|
|
*******************************************************/
|
|||
|
|
void MatrixPlusVectorN(TYPE_CAL* pMatrix, TYPE_CAL* pVector, TYPE_CAL* pReturn, UINT8 M, UINT8 N )
|
|||
|
|
{
|
|||
|
|
UINT16 i,j;
|
|||
|
|
for (i=0; i<M; i++)
|
|||
|
|
{
|
|||
|
|
pReturn[i] = 0.0F;
|
|||
|
|
for (j=0; j<N; j++)
|
|||
|
|
{
|
|||
|
|
pReturn[i] += pMatrix[i*N+j] * pVector[j];
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/***************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD>Ǿ<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺cq<EFBFBD><EFBFBD>4Ԫ<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ca:<EFBFBD><EFBFBD>̬<EFBFBD>Ǿ<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
***************************************************/
|
|||
|
|
void Q2Att( TYPE_CAL *cq, TYPE_CAL *ca )
|
|||
|
|
{
|
|||
|
|
ca[0] = cq[0]*cq[0]-cq[1]*cq[1]-cq[2]*cq[2]+cq[3]*cq[3];
|
|||
|
|
ca[1] = 2.0F*( cq[0]*cq[1]+cq[2]*cq[3] );
|
|||
|
|
ca[2] = 2.0F*( cq[0]*cq[2]-cq[1]*cq[3] );
|
|||
|
|
|
|||
|
|
ca[3] = 2.0F*( cq[0]*cq[1]-cq[2]*cq[3] );
|
|||
|
|
ca[4] = -cq[0]*cq[0]+cq[1]*cq[1]-cq[2]*cq[2]+cq[3]*cq[3];
|
|||
|
|
ca[5] = 2.0F*( cq[1]*cq[2]+cq[0]*cq[3] );
|
|||
|
|
|
|||
|
|
ca[6] = 2.0F*( cq[0]*cq[2]+cq[1]*cq[3] );
|
|||
|
|
ca[7] = 2.0*( cq[1]*cq[2]-cq[0]*cq[3] );
|
|||
|
|
ca[8] = -cq[0]*cq[0]-cq[1]*cq[1]+cq[2]*cq[2]+cq[3]*cq[3];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
/*****************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺att<EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>A:<EFBFBD><EFBFBD>̬<EFBFBD>Ǿ<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
***************************************************/
|
|||
|
|
void Att2M( TYPE_CAL *att, TYPE_CAL *A )
|
|||
|
|
{
|
|||
|
|
TYPE_CAL s0, c0, s1, c1, s2, c2;
|
|||
|
|
s0 = POSE_SINF( att[0] );
|
|||
|
|
c0 = POSE_COSF( att[0] );
|
|||
|
|
s1 = POSE_SINF( att[1] );
|
|||
|
|
c1 = POSE_COSF( att[1] );
|
|||
|
|
s2 = POSE_SINF( att[2] );
|
|||
|
|
c2 = POSE_COSF( att[2] );
|
|||
|
|
|
|||
|
|
A[0] = c1*c2-s0*s1*s2;
|
|||
|
|
A[1] = c1*s2+s0*s1*c2;
|
|||
|
|
A[2] = -c0*s1;
|
|||
|
|
|
|||
|
|
A[3] = -c0*s2;
|
|||
|
|
A[4] = c0*c2;
|
|||
|
|
A[5] = s0;
|
|||
|
|
|
|||
|
|
A[6] = s1*c2+s0*c1*s2;
|
|||
|
|
A[7] = s1*s2-s0*c1*c2;
|
|||
|
|
A[8] = c0*c1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/***************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺M:<EFBFBD><EFBFBD>̬<EFBFBD>Ǿ<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ca<EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD>
|
|||
|
|
***************************************************/
|
|||
|
|
//#ifndef MINMODULE_TYPE
|
|||
|
|
//void M2Att( TYPE_CAL *M , TYPE_CAL *ca )
|
|||
|
|
//{
|
|||
|
|
//if(POSE_ABSF(M[5]) > 1.0F)
|
|||
|
|
//{
|
|||
|
|
//if(1 == CalPFlag(M[5]))
|
|||
|
|
//{
|
|||
|
|
//M[5] = 1.0F;
|
|||
|
|
//}
|
|||
|
|
//else if(-1 == CalPFlag(M[5]))
|
|||
|
|
//{
|
|||
|
|
//M[5] = -1.0F;
|
|||
|
|
//}
|
|||
|
|
//}
|
|||
|
|
//
|
|||
|
|
//ca[0] = POSE_ASINF( M[5] ); /*A[1][2]*/
|
|||
|
|
//
|
|||
|
|
//if(POSE_ABSF(POSE_ABSF(ca[0]) - POSE_05PI) < POSE_ZERO)
|
|||
|
|
//{
|
|||
|
|
//ca[1] = 0.0;
|
|||
|
|
//ca[2] = 0.0;
|
|||
|
|
//return;
|
|||
|
|
//}
|
|||
|
|
//
|
|||
|
|
//ca[1] = POSE_ATAN2F( -M[2], M[8] ); /*2013<31><33>12<31><32>11<31><31><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD>Ҫ<EFBFBD>Ķ<EFBFBD><C4B6><EFBFBD><EFBFBD><EFBFBD>*/
|
|||
|
|
//ca[2] = POSE_ATAN2F( -M[3], M[4] ); /* -A[1][0] A[1][1]*/
|
|||
|
|
//}
|
|||
|
|
//#endif
|
|||
|
|
|
|||
|
|
/******************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><EFBFBD><EFBFBD>Ӧ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺iM3<EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>cq:4Ԫ<EFBFBD><EFBFBD>
|
|||
|
|
*******************************************************/
|
|||
|
|
void Att2Q( TYPE_CAL *iM3, TYPE_CAL *cq )
|
|||
|
|
{
|
|||
|
|
TYPE_CAL Ltmp;
|
|||
|
|
TYPE_CAL tmpCp[4];
|
|||
|
|
//TYPE_CAL tmpUse = 0;
|
|||
|
|
UINT16 k, tmpNO;
|
|||
|
|
|
|||
|
|
tmpCp[0] = POSE_ABSF(1.0 + iM3[0]+iM3[4]+iM3[8]);
|
|||
|
|
tmpCp[1] = POSE_ABSF(1.0 + iM3[0]-iM3[4]-iM3[8]);
|
|||
|
|
tmpCp[2] = POSE_ABSF(1.0 - iM3[0]+iM3[4]-iM3[8]);
|
|||
|
|
tmpCp[3] = POSE_ABSF(1.0 - iM3[0]-iM3[4]+iM3[8]);
|
|||
|
|
|
|||
|
|
Ltmp = 0;
|
|||
|
|
tmpNO = 0;
|
|||
|
|
for(k = 0; k < 4; ++k)
|
|||
|
|
{
|
|||
|
|
if(Ltmp < tmpCp[k])
|
|||
|
|
{
|
|||
|
|
Ltmp = tmpCp[k];
|
|||
|
|
tmpNO = k;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Ltmp = POSE_SQRTF(Ltmp) * 0.5;
|
|||
|
|
if(0 == CalPFlag(Ltmp))
|
|||
|
|
{
|
|||
|
|
cq[0] = 0.0;
|
|||
|
|
cq[1] = 0.0;
|
|||
|
|
cq[2] = 0.0;
|
|||
|
|
cq[3] = 1.0;
|
|||
|
|
}
|
|||
|
|
else if(0 == tmpNO)
|
|||
|
|
{
|
|||
|
|
cq[3] = Ltmp;
|
|||
|
|
cq[0] = ( iM3[5] - iM3[7] )/4.0F/Ltmp; /* A^T12 - A^T21 */
|
|||
|
|
cq[1] = ( iM3[6] - iM3[2] )/4.0F/Ltmp; /* A^T20 - A^T02 */
|
|||
|
|
cq[2] = ( iM3[1] - iM3[3] )/4.0F/Ltmp; /* A^T01 - A^T10 */
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
else if(1 == tmpNO)
|
|||
|
|
{
|
|||
|
|
cq[0] = Ltmp;
|
|||
|
|
cq[1] = ( iM3[1] + iM3[3] )/4.0F/Ltmp;
|
|||
|
|
cq[2] = ( iM3[2] + iM3[6] )/4.0F/Ltmp;
|
|||
|
|
cq[3] = ( iM3[5] - iM3[7] )/4.0F/Ltmp;
|
|||
|
|
}
|
|||
|
|
else if(2 == tmpNO)
|
|||
|
|
{
|
|||
|
|
cq[1] = Ltmp;
|
|||
|
|
cq[2] = ( iM3[5] + iM3[7] )/4.0F/Ltmp;
|
|||
|
|
cq[3] = ( iM3[6] - iM3[2] )/4.0F/Ltmp;
|
|||
|
|
cq[0] = ( iM3[1] + iM3[3] )/4.0F/Ltmp;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
cq[2] = Ltmp;
|
|||
|
|
cq[3] = ( iM3[1] - iM3[3] )/4.0F/Ltmp;
|
|||
|
|
cq[0] = ( iM3[2] + iM3[6] )/4.0F/Ltmp;
|
|||
|
|
cq[1] = ( iM3[5] + iM3[7] )/4.0F/Ltmp;
|
|||
|
|
}
|
|||
|
|
/*<2A>ж<EFBFBD>q4<71><34><EFBFBD>ڻ<EFBFBD><DABB><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>š<EFBFBD>С<EFBFBD><D0A1>0<EFBFBD><30>q1<71><31>q2<71><32>q3<71><33>q4<71><34><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1> 20130717 */
|
|||
|
|
if(cq[3] < 0.0)
|
|||
|
|
{
|
|||
|
|
cq[0] = -cq[0];
|
|||
|
|
cq[1] = -cq[1];
|
|||
|
|
cq[2] = -cq[2];
|
|||
|
|
cq[3] = -cq[3];
|
|||
|
|
}
|
|||
|
|
/*unitary( cq, 4);*/
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/********************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺p<EFBFBD><EFBFBD>4Ԫ<EFBFBD><EFBFBD>
|
|||
|
|
kind<EFBFBD><EFBFBD>1Ϊ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>rM4:4Ԫ<EFBFBD>ؾ<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*******************************************************/
|
|||
|
|
//#ifndef MINMODULE_TYPE
|
|||
|
|
//void Q2Q44( TYPE_CAL *p, TYPE_CAL *rM4, UINT8 kind )
|
|||
|
|
//{ /*Q2=Q*Q1 */
|
|||
|
|
//if( kind == 1 ) /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> Ӧ<><D3A6><EFBFBD><EFBFBD>Q2=A(Q1)*Q */
|
|||
|
|
//{
|
|||
|
|
//rM4[0] = p[3]; rM4[1] = p[2]; rM4[2] =-p[1]; rM4[3] = p[0];
|
|||
|
|
//rM4[4] =-p[2]; rM4[5] = p[3]; rM4[6] = p[0]; rM4[7] = p[1];
|
|||
|
|
//rM4[8] = p[1]; rM4[9] =-p[0]; rM4[10]= p[3]; rM4[11]= p[2];
|
|||
|
|
//rM4[12]=-p[0]; rM4[13]=-p[1]; rM4[14]=-p[2]; rM4[15]= p[3];
|
|||
|
|
//}
|
|||
|
|
//else /* Ӧ<><D3A6><EFBFBD><EFBFBD> Q2=A(Q)*Q1 */
|
|||
|
|
//{
|
|||
|
|
//rM4[0] = p[3]; rM4[1] =-p[2]; rM4[2] = p[1]; rM4[3] = p[0];
|
|||
|
|
//rM4[4] = p[2]; rM4[5] = p[3]; rM4[6] =-p[0]; rM4[7] = p[1];
|
|||
|
|
//rM4[8] =-p[1]; rM4[9] = p[0]; rM4[10]= p[3]; rM4[11]= p[2];
|
|||
|
|
//rM4[12]=-p[0]; rM4[13]=-p[1]; rM4[14]=-p[2]; rM4[15]= p[3];
|
|||
|
|
//}
|
|||
|
|
//}
|
|||
|
|
//#endif
|
|||
|
|
|
|||
|
|
/*********************************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Aoi<EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD>ɹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵת<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺uR<EFBFBD><EFBFBD>uV<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>á<EFBFBD><EFBFBD>ٶ<EFBFBD>ʸ<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Aoi[3]
|
|||
|
|
ע<EFBFBD>⣺
|
|||
|
|
*********************************************************************/
|
|||
|
|
void CalTransMatrix_IO( TYPE_CAL *cuR, TYPE_CAL *cuV, TYPE_CAL * cAoi )
|
|||
|
|
{
|
|||
|
|
TYPE_CAL uvec[3],vec[3],rvec[3];
|
|||
|
|
|
|||
|
|
uvec[0] = -cuR[0];
|
|||
|
|
uvec[1] = -cuR[1];
|
|||
|
|
uvec[2] = -cuR[2];
|
|||
|
|
unitary(&uvec[0], 3);
|
|||
|
|
memcpy( cAoi+6, &uvec[0], 3 * sizeof(TYPE_CAL) );
|
|||
|
|
|
|||
|
|
cross( cuV, cuR , &vec[0] ); /*<2A><><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>*/
|
|||
|
|
unitary( &vec[0], 3 );
|
|||
|
|
memcpy( cAoi+3, &vec[0], 3 * sizeof(TYPE_CAL) );
|
|||
|
|
|
|||
|
|
cross( &vec[0], &uvec[0] , &rvec[0]);
|
|||
|
|
memcpy( cAoi, &rvec[0], 3 * sizeof(TYPE_CAL) );
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/********************************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>㾭γ<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺cuR :<EFBFBD><EFBFBD>ǰλ<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>alf:<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
dlt<EFBFBD><EFBFBD>γ<EFBFBD><EFBFBD>
|
|||
|
|
*********************************************************************/
|
|||
|
|
//#ifndef MINMODULE_TYPE
|
|||
|
|
//BOOL CalEarthLonLat(TYPE_CAL * cuR, TYPE_CAL * alf, TYPE_CAL * dlt)
|
|||
|
|
//{
|
|||
|
|
//TYPE_CAL tmpNor, tmpUse;
|
|||
|
|
//BOOL result = TRUE;
|
|||
|
|
///*<2A><><EFBFBD>㾭γ<E3BEAD><CEB3>*/
|
|||
|
|
//CalNormal(cuR, 3, &tmpNor);
|
|||
|
|
//
|
|||
|
|
///*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>*/
|
|||
|
|
//if((0 == CalPFlag(cuR[0])) || (0 == CalPFlag(tmpNor)))
|
|||
|
|
//return FALSE;
|
|||
|
|
//
|
|||
|
|
//*alf = POSE_ATAN2F(cuR[1] , cuR[0]);
|
|||
|
|
//
|
|||
|
|
//tmpUse = cuR[2] / tmpNor;
|
|||
|
|
//if(POSE_ABSF(tmpUse) > 1.0F)
|
|||
|
|
//{
|
|||
|
|
//if(1 == CalPFlag(tmpUse))
|
|||
|
|
//{
|
|||
|
|
//tmpUse = 1.0F;
|
|||
|
|
//}
|
|||
|
|
//else if(-1 == CalPFlag(tmpUse))
|
|||
|
|
//{
|
|||
|
|
//tmpUse = -1.0F;
|
|||
|
|
//}
|
|||
|
|
//}
|
|||
|
|
//
|
|||
|
|
//*dlt = POSE_ASINF(tmpUse);
|
|||
|
|
//return result;
|
|||
|
|
//}
|
|||
|
|
//#endif
|
|||
|
|
|
|||
|
|
/********************************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Aie<EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺cuR <EFBFBD><EFBFBD>ǰλ<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Aie[3] <EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
ע<EFBFBD>⣺
|
|||
|
|
*********************************************************************/
|
|||
|
|
//#ifndef MINMODULE_TYPE
|
|||
|
|
//void CalTransMatrix_IG( TYPE_CAL * cuR, TYPE_CAL *cAie )
|
|||
|
|
//{
|
|||
|
|
////TYPE_CAL sla, sra;
|
|||
|
|
//TYPE_CAL sl,cl, sr, cr;
|
|||
|
|
//TYPE_CAL tmpAlf, tmpDlt; /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>γ<EFBFBD><CEB3>*/
|
|||
|
|
//
|
|||
|
|
///*<2A><><EFBFBD>㾭γ<E3BEAD><CEB3>*/
|
|||
|
|
//if(!CalEarthLonLat(cuR, &tmpAlf, &tmpDlt))
|
|||
|
|
//{
|
|||
|
|
//return;
|
|||
|
|
//}
|
|||
|
|
//
|
|||
|
|
//sl = POSE_SINF( tmpDlt );
|
|||
|
|
//cl = POSE_COSF( tmpDlt );
|
|||
|
|
//sr = POSE_SINF( tmpAlf );
|
|||
|
|
//cr = POSE_COSF( tmpAlf );
|
|||
|
|
//
|
|||
|
|
//cAie[0] =-sl*cr; /* Aie[3][3] */
|
|||
|
|
//cAie[1] =-sl*sr;
|
|||
|
|
//cAie[2] = cl;
|
|||
|
|
//cAie[3] =-sr;
|
|||
|
|
//cAie[4] = cr;
|
|||
|
|
//cAie[5] = 0.0;
|
|||
|
|
//cAie[6] =-cl*cr;
|
|||
|
|
//cAie[7] =-cl*sr;
|
|||
|
|
//cAie[8] =-sl;
|
|||
|
|
//}
|
|||
|
|
//#endif
|
|||
|
|
///********************************************************************
|
|||
|
|
//<2F><><EFBFBD>ܣ<EFBFBD><DCA3>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD>ڹ<EFBFBD><DAB9><EFBFBD>ϵ<EFBFBD>Ľ<EFBFBD><C4BD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//<2F><><EFBFBD>룺Omg0:<3A><><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Omgoi, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʸ<EFBFBD><CAB8>
|
|||
|
|
//*********************************************************************/
|
|||
|
|
//#ifndef MINMODULE_TYPE
|
|||
|
|
//void CalArcSpdOI(TYPE_CAL Omg0, TYPE_CAL * Omgoi)
|
|||
|
|
//{
|
|||
|
|
//Omgoi[0] = 0.0;
|
|||
|
|
//Omgoi[1] = -Omg0;
|
|||
|
|
//Omgoi[2] = 0.0;
|
|||
|
|
//}
|
|||
|
|
//#endif
|
|||
|
|
|
|||
|
|
/********************************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺Qb1<EFBFBD><EFBFBD>Qb2<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>4Ԫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>4ά<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Qres<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>
|
|||
|
|
*********************************************************************/
|
|||
|
|
//#ifndef MINMODULE_TYPE
|
|||
|
|
//void CalQPlusQ(TYPE_CAL * Qb1, TYPE_CAL * Qb2, TYPE_CAL * Qres)
|
|||
|
|
//{
|
|||
|
|
//Qres[0] = Qb1[3] * Qb2[0] + Qb1[0] * Qb2[3] + Qb1[1] * Qb2[2] - Qb1[2] * Qb2[1];
|
|||
|
|
//Qres[1] = Qb1[3] * Qb2[1] + Qb1[1] * Qb2[3] + Qb1[2] * Qb2[0] - Qb1[0] * Qb2[2];
|
|||
|
|
//Qres[2] = Qb1[3] * Qb2[2] + Qb1[2] * Qb2[3] + Qb1[0] * Qb2[1] - Qb1[1] * Qb2[0];
|
|||
|
|
//Qres[3] = Qb1[3] * Qb2[3] - Qb1[0] * Qb2[0] - Qb1[1] * Qb2[1] - Qb1[2] * Qb2[2];
|
|||
|
|
//}
|
|||
|
|
//#endif
|
|||
|
|
|
|||
|
|
/********************************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺Qb1<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>4Ԫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>4ά<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Qres<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>
|
|||
|
|
*********************************************************************/
|
|||
|
|
void CalQuatInv(TYPE_CAL *Qb1, TYPE_CAL *Qres)
|
|||
|
|
{
|
|||
|
|
Qres[0] = -Qb1[0];
|
|||
|
|
Qres[1] = -Qb1[1];
|
|||
|
|
Qres[2] = -Qb1[2];
|
|||
|
|
Qres[3] = Qb1[3];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/************************ZMXADD****************************************/
|
|||
|
|
|
|||
|
|
/********************************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD>CRC<EFBFBD><EFBFBD>ʽ<EFBFBD><EFBFBD>
|
|||
|
|
*********************************************************************/
|
|||
|
|
const UINT8 CRC_TABLE_8[256]={
|
|||
|
|
0x00,0x07,0x0E,0x09,0x1C,0x1B,0x12,0x15,0x38,0x3F,0x36,0x31,0x24,0x23,0x2A,0x2D,
|
|||
|
|
0x70,0x77,0x7E,0x79,0x6C,0x6B,0x62,0x65,0x48,0x4F,0x46,0x41,0x54,0x53,0x5A,0x5D,
|
|||
|
|
0xE0,0xE7,0xEE,0xE9,0xFC,0xFB,0xF2,0xF5,0xD8,0xDF,0xD6,0xD1,0xC4,0xC3,0xCA,0xCD,
|
|||
|
|
0x90,0x97,0x9E,0x99,0x8C,0x8B,0x82,0x85,0xA8,0xAF,0xA6,0xA1,0xB4,0xB3,0xBA,0xBD,
|
|||
|
|
0xC7,0xC0,0xC9,0xCE,0xDB,0xDC,0xD5,0xD2,0xFF,0xF8,0xF1,0xF6,0xE3,0xE4,0xED,0xEA,
|
|||
|
|
0xB7,0xB0,0xB9,0xBE,0xAB,0xAC,0xA5,0xA2,0x8F,0x88,0x81,0x86,0x93,0x94,0x9D,0x9A,
|
|||
|
|
0x27,0x20,0x29,0x2E,0x3B,0x3C,0x35,0x32,0x1F,0x18,0x11,0x16,0x03,0x04,0x0D,0x0A,
|
|||
|
|
0x57,0x50,0x59,0x5E,0x4B,0x4C,0x45,0x42,0x6F,0x68,0x61,0x66,0x73,0x74,0x7D,0x7A,
|
|||
|
|
0x89,0x8E,0x87,0x80,0x95,0x92,0x9B,0x9C,0xB1,0xB6,0xBF,0xB8,0xAD,0xAA,0xA3,0xA4,
|
|||
|
|
0xF9,0xFE,0xF7,0xF0,0xE5,0xE2,0xEB,0xEC,0xC1,0xC6,0xCF,0xC8,0xDD,0xDA,0xD3,0xD4,
|
|||
|
|
0x69,0x6E,0x67,0x60,0x75,0x72,0x7B,0x7C,0x51,0x56,0x5F,0x58,0x4D,0x4A,0x43,0x44,
|
|||
|
|
0x19,0x1E,0x17,0x10,0x05,0x02,0x0B,0x0C,0x21,0x26,0x2F,0x28,0x3D,0x3A,0x33,0x34,
|
|||
|
|
0x4E,0x49,0x40,0x47,0x52,0x55,0x5C,0x5B,0x76,0x71,0x78,0x7F,0x6A,0x6D,0x64,0x63,
|
|||
|
|
0x3E,0x39,0x30,0x37,0x22,0x25,0x2C,0x2B,0x06,0x01,0x08,0x0F,0x1A,0x1D,0x14,0x13,
|
|||
|
|
0xAE,0xA9,0xA0,0xA7,0xB2,0xB5,0xBC,0xBB,0x96,0x91,0x98,0x9F,0x8A,0x8D,0x84,0x83,
|
|||
|
|
0xDE,0xD9,0xD0,0xD7,0xC2,0xC5,0xCC,0xCB,0xE6,0xE1,0xE8,0xEF,0xFA,0xFD,0xF4,0xF3
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
/****************************************************************
|
|||
|
|
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡCRCУ<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD>crcУ<EFBFBD><EFBFBD>ֵ
|
|||
|
|
ע<EFBFBD>⣺
|
|||
|
|
****************************************************************/
|
|||
|
|
UINT8 DoGetCrcCheckU8(UINT8 * BeginAddr,UINT8 CheckLength)
|
|||
|
|
{
|
|||
|
|
UINT8 result;
|
|||
|
|
UINT8 i;
|
|||
|
|
|
|||
|
|
result = 0xFF;
|
|||
|
|
for(i = 0; i < CheckLength; ++i) /* culculate the CRC */
|
|||
|
|
{
|
|||
|
|
result = CRC_TABLE_8[(UINT8)(result ^ BeginAddr[i])];
|
|||
|
|
}
|
|||
|
|
return result;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/****************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>24λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݻ<EFBFBD>ȡ<EFBFBD>з<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺<EFBFBD><EFBFBD><EFBFBD><EFBFBD>8λ
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>32λ<EFBFBD>з<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*****************************************************/
|
|||
|
|
float PoseGetInt32From24(UINT8* Ldata)
|
|||
|
|
{
|
|||
|
|
UINT32 L24,tmpL24;
|
|||
|
|
float tmp_f =0.0f;
|
|||
|
|
L24 = ((*(Ldata+0))*256+(*(Ldata+1)))*256+(*(Ldata+2));
|
|||
|
|
if( (*(Ldata+0) & 0x80) != 0x0)
|
|||
|
|
{
|
|||
|
|
tmpL24 = ~L24 ;
|
|||
|
|
L24 = (tmpL24 &0xffffff)+1;
|
|||
|
|
tmp_f = ((float)L24)*(-1.0f);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
tmp_f = (float)L24;
|
|||
|
|
}
|
|||
|
|
return(tmp_f);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
/********************************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD>õ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڹ<EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>Ľ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺cuRλ<EFBFBD>ã<EFBFBD>cuV<EFBFBD>ٶ<EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Woi<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڹ<EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>Ľ<EFBFBD><EFBFBD>ٶ<EFBFBD> GMEARTH
|
|||
|
|
*********************************************************************/
|
|||
|
|
void AglSpdOofICal(TYPE_CAL *cuR, TYPE_CAL *cuV, TYPE_CAL *Woi)
|
|||
|
|
{
|
|||
|
|
UINT8 i;
|
|||
|
|
TYPE_CAL RR;
|
|||
|
|
TYPE_CAL tmpw[3];
|
|||
|
|
TYPE_CAL tmpwo;
|
|||
|
|
TYPE_CAL tmpRXV[3];
|
|||
|
|
CalNormal(cuR, 3, &RR);
|
|||
|
|
RR=RR*RR;
|
|||
|
|
cross(cuR,cuV,tmpRXV);
|
|||
|
|
if (RR > POSE_ZERO)
|
|||
|
|
{
|
|||
|
|
for(i=0;i<3;i++)
|
|||
|
|
{
|
|||
|
|
tmpw[i] =tmpRXV[i]/RR;
|
|||
|
|
}
|
|||
|
|
CalNormal(tmpw, 3, &tmpwo);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
Woi[1]=-0.00096466; //zmx20231010
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
memset(Woi,0,3*sizeof(TYPE_CAL));
|
|||
|
|
Woi[1]=-tmpwo;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/****************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>:<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>Ϊ 0----2PI
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺Դ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
|
|||
|
|
*****************************************************/
|
|||
|
|
//TYPE_CAL CalTo2PAI(TYPE_CAL inData)
|
|||
|
|
//{
|
|||
|
|
//UINT32 tmpCt;
|
|||
|
|
//TYPE_CAL result;
|
|||
|
|
//
|
|||
|
|
//if(inData > POSE_ZERO)
|
|||
|
|
//{
|
|||
|
|
//tmpCt = inData / POSE_2PI;
|
|||
|
|
//result = inData - tmpCt * POSE_2PI;
|
|||
|
|
//}
|
|||
|
|
//else
|
|||
|
|
//{
|
|||
|
|
//tmpCt = (-inData) / POSE_2PI;
|
|||
|
|
//result = inData + POSE_2PI * (tmpCt + 1);
|
|||
|
|
//}
|
|||
|
|
//if(result < 0.0)
|
|||
|
|
//result = 0.0;
|
|||
|
|
//if(result > POSE_2PI)
|
|||
|
|
//result = POSE_2PI;
|
|||
|
|
//
|
|||
|
|
//return result;
|
|||
|
|
//}
|
|||
|
|
|
|||
|
|
/******************************************************************
|
|||
|
|
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> AttLongKutaCal
|
|||
|
|
*<EFBFBD><EFBFBD> <EFBFBD><EFBFBD>: <EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>
|
|||
|
|
*<EFBFBD><EFBFBD> <EFBFBD><EFBFBD>: <EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>
|
|||
|
|
*˵ <EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD><EFBFBD>-<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ּ<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*******************************************************************/
|
|||
|
|
void AttLongKutaCal(TYPE_CAL * pWbxl, TYPE_CAL * pQbxl, TYPE_CAL * pQbx)
|
|||
|
|
{
|
|||
|
|
TYPE_CAL tmpK[4][4], tmpOmg[16];
|
|||
|
|
TYPE_CAL tmpUse[4];
|
|||
|
|
TYPE_CAL tmpDT;
|
|||
|
|
UINT32 i;
|
|||
|
|
|
|||
|
|
tmpOmg[0] = 0.0;
|
|||
|
|
tmpOmg[1] = pWbxl[2];
|
|||
|
|
tmpOmg[2] = -pWbxl[1];
|
|||
|
|
tmpOmg[3] = pWbxl[0];
|
|||
|
|
tmpOmg[4] = -pWbxl[2];
|
|||
|
|
tmpOmg[5] = 0.0;
|
|||
|
|
tmpOmg[6] = pWbxl[0];
|
|||
|
|
tmpOmg[7] = pWbxl[1];
|
|||
|
|
tmpOmg[8] = pWbxl[1];
|
|||
|
|
tmpOmg[9] = -pWbxl[0];
|
|||
|
|
tmpOmg[10] = 0.0;
|
|||
|
|
tmpOmg[11] = pWbxl[2];
|
|||
|
|
tmpOmg[12] = -pWbxl[0];
|
|||
|
|
tmpOmg[13] = -pWbxl[1];
|
|||
|
|
tmpOmg[14] = -pWbxl[2];
|
|||
|
|
tmpOmg[15] = 0.0;
|
|||
|
|
|
|||
|
|
//K1
|
|||
|
|
MatrixPlusVectorN(tmpOmg, pQbxl, tmpK[0], 4, 4);
|
|||
|
|
for (i = 0; i < 4; ++i)
|
|||
|
|
{
|
|||
|
|
tmpK[0][i] /= 2.0;
|
|||
|
|
}
|
|||
|
|
//K2
|
|||
|
|
for (i = 0; i < 4; ++i)
|
|||
|
|
{
|
|||
|
|
tmpUse[i] = pQbxl[i] + 0.5 * tmpK[0][i];
|
|||
|
|
}
|
|||
|
|
MatrixPlusVectorN(tmpOmg, tmpUse, tmpK[1], 4, 4);
|
|||
|
|
for (i = 0; i < 4; ++i)
|
|||
|
|
{
|
|||
|
|
tmpK[1][i] /= 2.0;
|
|||
|
|
}
|
|||
|
|
//K3
|
|||
|
|
for (i = 0; i < 4; ++i)
|
|||
|
|
{
|
|||
|
|
tmpUse[i] = pQbxl[i] + 0.5 * tmpK[1][i];
|
|||
|
|
}
|
|||
|
|
MatrixPlusVectorN(tmpOmg, tmpUse, tmpK[2], 4, 4);
|
|||
|
|
for (i = 0; i < 4; ++i)
|
|||
|
|
{
|
|||
|
|
tmpK[2][i] /= 2.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//K4
|
|||
|
|
for (i = 0; i < 4; ++i)
|
|||
|
|
{
|
|||
|
|
tmpUse[i] = pQbxl[i] + tmpK[2][i];
|
|||
|
|
}
|
|||
|
|
MatrixPlusVectorN(tmpOmg, tmpUse, tmpK[3], 4, 4);
|
|||
|
|
for (i = 0; i < 4; ++i)
|
|||
|
|
{
|
|||
|
|
tmpK[3][i] /= 2.0;
|
|||
|
|
}
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
tmpDT = ATTPERIOD_S;
|
|||
|
|
for (i = 0; i < 4; ++i)
|
|||
|
|
{
|
|||
|
|
pQbx[i] = pQbxl[i] +
|
|||
|
|
tmpDT * (tmpK[0][i] + 2.0 * tmpK[1][i] + 2.0 * tmpK[2][i] + tmpK[3][i]) / 6.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/***************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD>3-1-2ŷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺ŷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʸ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ں<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
***************************************************/
|
|||
|
|
void Eul312A2Quat( TYPE_CAL *ca, TYPE_CAL *cq )
|
|||
|
|
{
|
|||
|
|
cq[0] = POSE_SINF(ca[0]/2.0)*POSE_COSF(ca[1]/2.0)*POSE_COSF(ca[2]/2.0)
|
|||
|
|
-POSE_COSF(ca[0]/2.0)*POSE_SINF(ca[1]/2.0)*POSE_SINF(ca[2]/2.0);
|
|||
|
|
cq[1] = POSE_COSF(ca[0]/2.0)*POSE_SINF(ca[1]/2.0)*POSE_COSF(ca[2]/2.0)
|
|||
|
|
+POSE_SINF(ca[0]/2.0)*POSE_COSF(ca[1]/2.0)*POSE_SINF(ca[2]/2.0);
|
|||
|
|
cq[2] = POSE_SINF(ca[0]/2.0)*POSE_SINF(ca[1]/2.0)*POSE_COSF(ca[2]/2.0)
|
|||
|
|
+POSE_COSF(ca[0]/2.0)*POSE_COSF(ca[1]/2.0)*POSE_SINF(ca[2]/2.0);
|
|||
|
|
cq[3] = POSE_COSF(ca[0]/2.0)*POSE_COSF(ca[1]/2.0)*POSE_COSF(ca[2]/2.0)
|
|||
|
|
-POSE_SINF(ca[0]/2.0)*POSE_SINF(ca[1]/2.0)*POSE_SINF(ca[2]/2.0);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/***************************************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺M:<EFBFBD><EFBFBD>̬<EFBFBD>Ǿ<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ca<EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD>
|
|||
|
|
***************************************************/
|
|||
|
|
void Matrix2Eul312A( TYPE_CAL *M , TYPE_CAL *ca )
|
|||
|
|
{
|
|||
|
|
if(POSE_ABSF(M[5]) > 1.0F)
|
|||
|
|
{
|
|||
|
|
if(1 == CalPFlag(M[5]))
|
|||
|
|
{
|
|||
|
|
M[5] = 1.0F;
|
|||
|
|
}
|
|||
|
|
else if(-1 == CalPFlag(M[5]))
|
|||
|
|
{
|
|||
|
|
M[5] = -1.0F;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
ca[0] = POSE_ASINF( M[5] );
|
|||
|
|
|
|||
|
|
if(POSE_ABSF(POSE_ABSF(ca[0]) - POSE_05PI) < POSE_ZERO)
|
|||
|
|
{
|
|||
|
|
ca[1] = 0.0;
|
|||
|
|
ca[2] = 0.0;
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
ca[1] = POSE_ATAN2F( -M[2], M[8] );
|
|||
|
|
ca[2] = POSE_ATAN2F( -M[3], M[4] );
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void DeviAngleLimit(TYPE_CAL * Ang)
|
|||
|
|
{
|
|||
|
|
if(POSE_ABSF(*Ang)>(270.0*ANG2RADIAN))
|
|||
|
|
{
|
|||
|
|
if(*Ang>0) *Ang-=360.0*ANG2RADIAN;
|
|||
|
|
else *Ang+=360.0*ANG2RADIAN;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/********************************
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ1<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>룺nΪ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
eff_bitΪ<EFBFBD><EFBFBD>Чλ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֻ<EFBFBD>ӵ<EFBFBD>λ<EFBFBD>㣩
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD>
|
|||
|
|
*******************************************************/
|
|||
|
|
UINT8 count_onebit(UINT8 n,UINT8 eff_bit)
|
|||
|
|
{
|
|||
|
|
UINT8 count = 0;
|
|||
|
|
UINT8 i = 0;
|
|||
|
|
for(i = 0; i < eff_bit; i++)
|
|||
|
|
{
|
|||
|
|
if (((n >> i) & 1) == 1)
|
|||
|
|
{
|
|||
|
|
count++;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
return count;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
///**********************************
|
|||
|
|
//* \fn uint32_t get_uint32from8_be(const uint8_t *psrc_data)
|
|||
|
|
//* \brief <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ַ<EFBFBD><D6B7>ȡ32bit<69><74><EFBFBD><EFBFBD>(<28><><EFBFBD><EFBFBD>)<29><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ijЩ<C4B3>ܹ<EFBFBD><DCB9><EFBFBD><EFBFBD>ʴ洢<CAB4>ж<EFBFBD><D0B6><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Բ<EFBFBD><D4B2><EFBFBD>ֱ<EFBFBD>ӽ<EFBFBD>8λָ<CEBB><D6B8>ǿ<EFBFBD><C7BF>ת<EFBFBD><D7AA>Ϊ32λ
|
|||
|
|
//* \param psrc_data <20><>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//* \return 32λ<32><EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
////*********************************/
|
|||
|
|
//UINT32 get_uint32from8_be(const UINT8 *psrc_data)
|
|||
|
|
//{
|
|||
|
|
//UINT32 result;
|
|||
|
|
//
|
|||
|
|
//result = *psrc_data;
|
|||
|
|
//result = result << 8;
|
|||
|
|
//result |= *(psrc_data + 1);
|
|||
|
|
//result = result << 8;
|
|||
|
|
//result |= *(psrc_data + 2);
|
|||
|
|
//result = result << 8;
|
|||
|
|
//result |= *(psrc_data + 3);
|
|||
|
|
//
|
|||
|
|
//return result;
|
|||
|
|
//}
|
|||
|
|
//
|
|||
|
|
/////********************************
|
|||
|
|
////* <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֽڴ<D6BD><DAB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>pData<74><61>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>float<61><74><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>float<61><74><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7>Ϸ<EFBFBD><CFB7><EFBFBD>ֻ<EFBFBD><D6BB><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>float<61><74><EFBFBD>Ͳŷ<CDB2><C5B7><EFBFBD>TRUE,<2C><><EFBFBD>Ϸ<EFBFBD><CFB7><EFBFBD><EFBFBD><EFBFBD>FALSE<53><45>
|
|||
|
|
////* <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5><EFBFBD><EFBFBD>
|
|||
|
|
////*******************************************************/
|
|||
|
|
//BOOL get_floatto8_be(UINT8 * pData, TYPE_CAL * Data)
|
|||
|
|
//{
|
|||
|
|
//UNN un;
|
|||
|
|
//UINT32 tDenCheck = get_uint32from8_be(pData);
|
|||
|
|
//
|
|||
|
|
////un.result[3] = *pData & 0xff;
|
|||
|
|
////un.result[2] = (*(pData + 1) & 0xff);
|
|||
|
|
////un.result[1] = (*(pData + 2) & 0xff);
|
|||
|
|
////un.result[0] = (*(pData + 3) & 0xff);
|
|||
|
|
//
|
|||
|
|
//un.result[0] = *pData & 0xff;
|
|||
|
|
//un.result[1] = (*(pData + 1) & 0xff);
|
|||
|
|
//un.result[2] = (*(pData + 2) & 0xff);
|
|||
|
|
//un.result[3] = (*(pData + 3) & 0xff);
|
|||
|
|
//memcpy(&un.Lf, &un.result, 4*sizeof(UINT8));
|
|||
|
|
///* <20><><EFBFBD><EFBFBD><EFBFBD>ӶԷǷ<D4B7><C7B7><EFBFBD><EFBFBD><EFBFBD>NAN INF<4E>ȵ<EFBFBD><C8B5>ж<EFBFBD> */
|
|||
|
|
//if( !FLOAT_NAN_CHECK(un.Lf) || !FLOAT_INF_CHECK(un.Lf) /* <20><>ΪNANʱ<4E><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ΪTRUE */
|
|||
|
|
//|| ( ( (tDenCheck&0x7f800000) == 0) && ( (tDenCheck&0x807fffff) != 0) ) /* Denormal float check */
|
|||
|
|
//)
|
|||
|
|
//{ return FALSE; }
|
|||
|
|
//else
|
|||
|
|
//{
|
|||
|
|
//if ( ( 0 < un.Lf ) && (un.Lf < 1.0E-15F ) )
|
|||
|
|
//{
|
|||
|
|
//un.Lf = 1.0E-15F;
|
|||
|
|
//}
|
|||
|
|
//else if (( 0 > un.Lf ) && (un.Lf > -1.0E-15F ))
|
|||
|
|
//{
|
|||
|
|
//un.Lf = -1.0E-15F;
|
|||
|
|
//}
|
|||
|
|
//else
|
|||
|
|
//{
|
|||
|
|
//;
|
|||
|
|
//}
|
|||
|
|
//*Data = un.Lf;
|
|||
|
|
//return TRUE;
|
|||
|
|
//}
|
|||
|
|
//}
|