运动轨迹插值法 改进版

本算法优点：

1、新插入点时，能保证插值曲线与历史曲线保持光滑连接；

2、本算法在插值时没有延迟，现有的抛物线插值算法有一个点的延迟

3、算法思路，以历史初速度的方向为起始速度方向，通过调整运动时间（对应本算法中的插值点数），即可调节插值轨迹

//获取点（x1，y1）和（x2，y2）之间的距离

float GetLen(float x1,float y1, float x2, float y2)

{

return sqrt(double((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2)));

}

bool IsEqual(float num1,float num2)

{

return abs(double (num2 - num1))<0.00001;

}

//归一化两个向量

bool To1(float& dVx, float& dVy,float len)

{

if (IsEqual(dVx,0.0) && IsEqual(dVy,0.0))

return false;

float dTemp =  sqrt(dVx * dVx + dVy*dVy);

dVx =0.1 * dVx / dTemp;

dVy = 0.1 *  dVy / dTemp;



return true;

}

计算向量vector1  （xV1,yV1）;vector2(xV2，yV2）之间的夹角

/*

计算向量vector1  （xV1,yV1）;vector2(xV2，yV2）之间的夹角

*/



float CalculateVectorAngle(float xV1, float yV1, float xV2, float yV2)

{  



if ((0==xV1 && 0 ==yV1) || (0 == xV2 && 0 == yV2))

return 0;

else

return acos((xV1*xV2 + yV1*yV2) / sqrt((xV1*xV1 + yV1*yV1)*(xV2*xV2 + yV2*yV2))) *180 /PI;

}

函数功能：

1、调用该函数时，不传任何参数意味着初始化该算法

2、正常插值时该函数返回均匀化后的插值点

参数说明：

x1，y1，起始插值点

x2，y2，结束插值点

ppX，ppY，数组地址，用于存放得到插值点的x坐标和y坐标

InsertNum 实际得到的插值点数

InsertLen 插值点之间的间隔距离

/*

函数功能：

1、调用该函数时，不传任何参数意味着初始化该算法

2、正常插值时该函数返回均匀化后的插值点

参数说明：

x1，y1，起始插值点

x2，y2，结束插值点

ppX，ppY，数组地址，用于存放得到插值点的x坐标和y坐标

InsertNum 实际得到的插值点数

InsertLen 插值点之间的间隔距离

*/

//bool InsertPoints(float x1 =-1 ,float y1=-1, float x2=-1, float y2=-1, float** ppX=NULL, float**ppY=NULL, int* pInsertNum = NULL, int InsertLen =1)

bool InsertPoints(float x1 ,float y1, float x2, float y2, float** ppX, float**ppY, int* pInsertNum, int InsertLen)

{

//全部使用默认参数时，初始化算法

if (-1==x1 && -1==y1 &&

-1==x2 && -1==y2 &&

NULL==pInsertNum && 

NULL==ppX && NULL==ppY &&

1 == InsertLen)

{

g_LastVx = 0;

g_LastVy =0;

return true;

}

if (IsEqual(g_LastVx,0) && IsEqual(g_LastVy,0))

{

g_LastVx = x2 - x1;

g_LastVy =y2 - y1;

To1(g_LastVx,g_LastVy,GetLen(x1,y1,x2,y2));

}

//传入的两个点相等时不进行插入，直接返回，并将插值点数值为0；

if (IsEqual(x1,x2) && IsEqual(y1, y2))

{

*pInsertNum = 0;

return false;

}



float fAngle = CalculateVectorAngle(g_LastVx, g_LastVy, x2-x1, y2-y1) / 180;

if (0 == fAngle)//角度为0时特殊处理

fAngle = 0.1;

float fLen = GetLen(x1,y1,x2,y2);

if (0 == fLen)//长度为0时特殊处理

fLen = 1;

float dNumT =   6.5*fLen / (1+fAngle);



float ftmplen = 0;

//角度大于100度时(100 / 180 = 0.56),需特殊处理

ftmplen =  (7 + 1.2 * fAngle) * fLen;

if (fAngle >0.56 && dNumT < ftmplen)

dNumT = ftmplen;

//角度小于20度时(20 / 180 = 0.11),需特殊处理

ftmplen = (1 +0.5 * fAngle) * fLen;

if (fAngle < 0.11 && (dNumT > ftmplen))

dNumT = ftmplen;



if (dNumT>MAX_NUM_INSERT_POINTS)//插值点数不能大于算法内部最大的存储点数

dNumT = MAX_NUM_INSERT_POINTS;//插值点数不能小于0

if (dNumT <0)

dNumT =1;

float dtmpNumTT = dNumT * dNumT;//临时变量dtmpNumTT用于存储插入点数的平方，对应s=v0 * t + 1/2 * a * t * t中的t的平方

//计算x,y方向上的加速度

float ax,ay;

ax = 2 * (x2- x1 - g_LastVx * dNumT) / dtmpNumTT;

ay = 2 * (y2 - y1 - g_LastVy * dNumT) / dtmpNumTT;



int iCurPos = 0;

float dCurtX, dCurtY;

while(iCurPos < dNumT)

{

g_InsertPontX[iCurPos] = x1 + g_LastVx * iCurPos + 0.5 * ax * iCurPos * iCurPos;

g_InsertPontY[iCurPos] = y1+ g_LastVy * iCurPos + 0.5 * ay * iCurPos * iCurPos;

if (abs(g_InsertPontX[iCurPos] - x2) < 1 && abs(g_InsertPontY[iCurPos] -y2)<1)

break;//已经达到终点则不再继续进行插值，用于防止方向对准时插值出现冒尖的现象

++iCurPos;

}



dNumT = iCurPos;



g_LastVx = g_LastVx + ax * iCurPos;

g_LastVy = g_LastVy + ay * iCurPos;

if (IsEqual(g_LastVx,0) && IsEqual(g_LastVy,0))

{

g_LastVx = x2 - x1;

g_LastVy =y2 - y1;

}

To1(g_LastVx,g_LastVy,GetLen(x1,y1,x2,y2));



//均匀化插值点

int iFirstPos=1, iSecPos =2;

while(iSecPos < dNumT)

{

if (GetLen(g_InsertPontX[iFirstPos],g_InsertPontY[iFirstPos],g_InsertPontX[iSecPos],g_InsertPontY[iSecPos])>=InsertLen)

{

g_InsertPontX[iFirstPos+1] = g_InsertPontX[iSecPos];

g_InsertPontY[iFirstPos+1] = g_InsertPontY[iSecPos];

++iSecPos;

++iFirstPos;

}

else

++iSecPos;

}

cout<<"dNumT= "<<dNumT<<";   fAngle= "<<fAngle<<";  fLen = "<<fLen<<endl;

*pInsertNum = iFirstPos;

*ppX = g_InsertPontX;

*ppY = g_InsertPontY;

return true;

}