zhoujl/rag_agent
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
7aa3ce32949f0133ae87f5ce39beac4256737495
rag_agent/RAG-TEST-TOOLS/PrjAttCtrlMng/AttMath.c

1142 lines
27 KiB
C
Raw Normal View History

增加代码知识库；修复文档处理内容；增加API设置
2026-05-16 20:20:10 +08:00
/*
* 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;
//}
//}
Reference in New Issue Copy Permalink