倒车轨迹理论实现方法

倒车轨迹理论实现方法帅文王文梁关键字：倒车轨迹视角转换前言：倒车轨迹是近两年部分国产汽车导航设备上新出现的一个功能，其借助方向盘转角信息将汽车可能的后退路线叠加到后视摄像头的输出上并标注出距离，以直观形象化的形式协助驾驶人员调整选择倒车路线，减少驾驶人员特别是新手的误判断，对使用者是一个不错的实用功能。倒车轨迹在智能倒车领域内属于辅助倒车系统中的一种，虽然其还无法到达智能化倒车，但是其实用性和辅助性上对汽车智能化单元技术方面是一个有效的补充。本文将基于使用为目的，从经验角度并结合基本数学推导分析倒车轨迹的原理、实现过程并给出实际使用过程中需要的操作点。由于本文非侧重于数学理论，对部分数学细节在不影响实际结论情况下不做深入探讨。倒车轨迹的基本原理从日常经验可知，以自行车为例，如果前轮有一定转角，在维持转角不变状态和无轴向移动前提下自行车走过的路径将会以某个圆点为中心旋转，同样的状态也会出现在汽车上。其走过路径如图1。摄像头后轮刖轮團1倒车路线示青團图中假设车轮不会出现轴向移动，故如果保持车轮转角不变的情况下，每个车轮只能沿着垂直其车轴的方向行进，这里取前后轮的轴心作为轨迹跟踪点实际过程中两个前轮轴心不会出现平行，则轨迹应该是以前后轮轴向线的焦点为圆心的圆。图中为为前轮同水平方向的夹角，记前后轮轴距为L,后轮轴长为W后轮距离车尾的距离为D,从几何关系可知，后轮轴心的运动轨迹可以描述为以半径Lcot()的圆周运动。两个后轮的轨迹分别为Lcot()-W/2和Lcot()+W/2的圆。这里的推导过程采用经验法结合几何推算，完全从数学角度的推算过程请参考资料1。图中的x方向和y方向不同于一般习惯主要是考虑后面的视角变换。从等式可以看到，当接近0度时候行进轨迹近似直线，接近90度时半径呈缩小趋势，符合我们日常经验值。视角转换从倒车公式推导出的路线图为行进路线的俯视图，实际显示给操作者的路线应该是从车内观察点观察到的轨迹，驾驶人员看到的运动轨迹实际为以车尾摄像头为中心点坐标的图像描述图一中车尾位置的原点。将摄像头位置定为坐标零点，则轨迹上的任意点位置公式为：x+Lcot()222+(y+D)=(Lcot()1上面推导的轨迹仍然是基于俯视条件下的轨迹，看到的应该为处于一定视角观察的轨迹，故需要进行一定角度的转换才能切换到实际观察到的图像。假设摄像头的可视角范围为2a，摄像头距离地面h，摄像头中心线同水平面的夹角为B，输出屏幕的高度为H,这里假设摄像头相对于屏幕为一个点，会造成实际计算结果的一定偏差，关于偏差的细节数学计算不属本文讨论的重点。我们实际观察到的Yr为地面y在显示屏H上的投影，y方向的转换过程如图二：上图中为了便于计算，将经过摄像头采集后输出的显示器直接投影到同一个图中，这和实际输出没有区别。上图中的虚线为水平线，点线为屏幕的中心点到摄像头的连线，B为中心线和水平线的夹角将上图进行简化可转为以下数学问题,应的边长，简化后的计算如图三。等腰三角形中同顶点成已知角度所对團三简化的计軍示青團Yr对应的角度为a+B-B，其中B=arctg(h/y)通过几何运算可得：其中9带入上式最终得：62=90-a曲0甘Xstn(耳+E)2sinolr_5诚农十0-mxr肌h;r)*UJ-r=；arcig(h.iT)2sin.上面等式中姜求yhcts(&+p)f表现在实际场星中为摂像头对车屋郢有咅盼盲区曲刼燎亂从上面公式可以看出当摄慷头处于水平位置时候g对于爭趙向于梔穷远(応渥(加对=0儿上面等式简化知Yr约等于H為即水平摆放的摄像头斷柏摄的所有地面團像不会超过一半屏辜。X方向的缩放比较简单，仅为等比例维放。设屏幕克眄贝山工旷x=X尸陀2同样受限于可视角農限制，龙的值小于yrga在可观区闾内的计負才有青义1000#pragmaonce#endif/_MSC_VER1000#includeRearCalibration.h/*structMypointintx;inty;typedefstructMypointMyPoint;*/classCBackTrackpublic:boolInitObject(CRearCalibration*pCali,floatfAngle,floatfAxleDist,floatfWheelDist,intlWidth,intlHeight);voidDrawExtendSpot(unsignedchar*lpDIBBits,intlWidth,intlHeight,boolbInverse=false);voidDrawExtendTrack(unsignedchar*lpDIBBits,intlWidth,intlHeight);voidDrawBackSpot(unsignedchar*lpDIBBits,intlWidth,intlHeight);voidDrawBackTrack(unsignedchar*lpDIBBits,intlWidth,intlHeight,boolbInverse=false);/*voidWay2_Back_Track_Draw(char*lpDIBBits,intlWidth,intlHeight);voidBack_Track_Draw(char*lpDIBBits,intlWidth,intlHeight);voidCalibration_Track_Draw(CRearCalibration*pCali,char*lpDIBBits,intlWidth,intlHeight);voidGetBackTrackDraw(CRearCalibration*pCali,char*lpDIBBits,intlWidth,intlHeight,floatfAngle);*/lHeight);private:lHeight);voidDrawTrack(unsignedchar*lpDIBBits,intlWidth,intCBackTrack();virtualCBackTrack();voidInverseBmp(unsignedchar*lpDIBBits,intlWidth,intCRearCalibration*m_pCali;floatm_fAngle;floatm_VBack;intm_iTlong;intm_step;voidGetExtendUV();voidComputerExtendXY();voidGetTfromView(intlWidth,intlHeight);voidComputerXY();voidComputerUV();CPointGetUfromVofExtend(shortv);CPointGetUfromV(shortv);voidGetMarkXU();shortRelationYV(shortv);intm_iNT;intm_iFT;floatm_fLX40;floatm_fLY40;floatm_fRX40;floatm_fRY40;floatm_fLeX40;floatm_fLeY40;floatm_fReX40;floatm_fReY40;intm_iLU40;intm_iLV40;：0=8|xvJe8q-iuJl/o二和旳八一山()|oeji|oea9:和引丄和。日oIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIuoipnjisaci/uoipnjisuoo/)!Pue#M3N_9na3aMOUeuipp#ri|厂二吋IzTSIH丄Jeqo5囘smilzTSIH丄QPun#ona3a_pp)!#hMMIBlu.epnpui#.qoejioea.epnpui#M冯epis“epnpui#/ssep|oeji|oea9o屮jouoiieiueiueidiu!:dcbpe丄日/CaamoNl6aLV9908992Z_98V_Va_L69e_8VS3WV3H_19Vdl19Va_XdV)P9U!ppi/)!Pue#_：迪说乙rrag。乙一山a何i|M吨1一山1现!8|xvJe8i-iuieo|j:0A9d!-AXotdnoy厂山A；0切/芮厂山AXotdnoi厂山A：OldA3厂山AJotdny厂山Am_LRearWheel=0;m_iTlong=0;GetMarkXU();CBackTrack:CBackTrack()/分步骤进行编程/*voidCBackTrack:GetTfromView()/floatVBack=138.9;/相机视野的最近点和最远点距离汽车后轴的距离，Ny要大于100，由于现在假定相机安装在后轴中心，故先付值100intNy,Fy;Ny=100;Fy=330;/中间变量floata,b;intt;/判断分母中为零的项，a不可能为零，因为前轴中心的转向角永远小于50度if(sin(m_fAngle)!=0)a=m_LRearAxle/(float)(tan(m_fAngle)-m_LRearWheel/2;b=m_LRearAxle/m_VBack/(float)(sin(m_fAngle);m_iNT=(int)(b*(float)(asin(Ny/a)+0.5f);m_iFT=(int)(b*(float)(asin(Fy/a)+0.5f);/当这两项为零时，车辆没有倒车，因此可以只把后轮的延长线画出elsem_iNT=(int)(Ny/m_VBack+0.5f);m_iFT=(int)(Fy/m_VBack+0.5f);/computertimejumpstepm_step=(m_iFT-m_iNT)/30;m_iTlong=0;for(t=m_iNT;tm_iFT;t=t+m_step)m_iTlong+;*/voidCBackTrack:GetTfromView(intlWidth,intlHeight)/floatVBack=138.9;/相机视野的最近点和最远点距离汽车后轴的距离，Ny要大于100，由于现在假定相机安装在后轴中心，故先付值100intNy,Fy;Ny=100;Fy=380;/intt;/中间变量floata,b;CPointpl,pr;m_iNT=(int)(Ny/m_VBack+0.5f);m_iFT=(int)(Fy/m_VBack+0.5f);/*floata,b;a=m_LRearAxle/(float)tan(m_fAngle);b=m_LRearAxle/m_VBack/(float)sin(m_fAngle);dom_fLX0=-(a-m_LRearWheel/2)*(float)(cos(m_iNT/b)+a;m_fLY0=(a-m_LRearWheel/2)*(float)(sin(m_iNT/b);pl=m_pCali-GetUVfromXY(m_fLX0,m_fLY0);m_fRX0=-(a+m_LRearWheel/2)*(float)(cos(m_iNT/b)+a;m_fRY0=(a+m_LRearWheel/2)*(float)(sin(m_iNT/b);pr=m_pCali-GetUVfromXY(m_fRX0,m_fRY0);m_iNT+;while(pl.x1|pl.y1|pr.x1|pr.y1);dom_fLX0=-(a-m_LRearWheel/2)*(float)(cos(m_iFT/b)+a;m_fLY0=(a-m_LRearWheel/2)*(float)(sin(m_iFT/b);pl=m_pCali-GetUVfromXY(m_fLX0,m_fLY0);m_fRX0=-(a+m_LRearWheel/2)*(float)(cos(m_iFT/b)+a;m_fRY0=(a+m_LRearWheel/2)*(float)(sin(m_iFT/b);pr=m_pCali-GetUVfromXY(m_fRX0,m_fRY0);m_iFT-;pr.ywhile(pl.xlWidth|pl.ylHeight|pr.xlWidth|lHeight);*/computertimejumpstepm_step=(m_iFT-m_iNT)/38;m_iTlong=0;for(t=m_iNT;tm_iFT;t=t+m_step)m_iTlong+;/*ComputerXY()计算倒车轨迹点参数是方向盘转角，此处暂为前轴中心点的转角*/voidCBackTrack:ComputerXY()floata,b;a=m_LRearAxle/(float)(tan(m_fAngle);b=m_VBack*(float)(sin(m_fAngle)/m_LRearAxle;intt;intl=0;for(t=m_iNT;tm_iFT;t=t+m_step)/leftrearwheelbacktrackm_fLXl=-(a-m_LRearWheel/2)*(float)(cos(bt)+a;m_fLYl=(a-m_LRearWheel/2)*(float)(sin(bt);/rightrearwheelbacktrackm_fRXl=-(a+m_LRearWheel/2)*(float)(cos(b*t)+a;m_fRYl=(a+m_LRearWheel/2)*(float)(sin(b*t);l+;/*车辆后轮延长线，无论方向盘转角等参数如何变，延长线上的点是不变的，因此可分开来写，尤其是当前轴中心点的转角为零时，在图像上只显示后轮延长线，因此需要单独写出来*/voidCBackTrack:ComputerExtendXY()intt;intl=0;for(t=m_iNT;tm_iFT;t=t+m_step)/leftrearwheelextendm_fLeXl=m_LRearWheel/2;m_fLeYl=t*m_VBack;/rightrearwheelextendm_fReXl=-m_LRearWheel/2;m_fReYl=m_fLeYl;l+;voidCBackTrack:ComputerUV()intt;intl=0;CPointp;for(t=m_iNT;tm_iFT;t=t+m_step)/leftrearwheelbacktrackinimagep=m_pCali-GetUVfromXY(m_fLXl,m_fLYl);m_iLUl=p.x;m_iLVl=p.y;/rightrearwheelbacktrackinimagep=m_pCali-GetUVfromXY(m_fRXl,m_fRYl);m_iRUl=p.x;m_iRVl=p.y;l+;voidCBackTrack:GetExtendUV()intt;intl=0;CPointp;for(t=m_iNT;tm_iFT;t=t+m_step)/leftrearwheelextendinimagep=m_pCali-GetUVfromXY(m_fLeXl,m_fLeYl);m_iLeUl=p.x;m_iLeVl=p.y;/rightrearwheelextendinimagep=m_pCali-GetUVfromXY(m_fReXl,m_fReYl);m_iReUl=p.x;m_iReVl=p.y;l+;voidCBackTrack:DrawBackSpot(unsignedchar*lpDIBBits,intlWidth,intlHeight)intt;longlLineBytes=WIDTHBYTES(lWidth*8);for(t=0;tm_iTlong;t+)/leftwheeltrackspot*(unsignedchar*)lpDIBBits+lLineBytes*m_iLVt+m_iLUt)=255;*(unsignedchar*)lpDIBBits+lLineBytes*(m_iLVt-1)+m_iLUt)=255;*(unsignedchar*)lpDIBBits+lLineBytes*(m_iLVt+1)+m_iLUt)=255;*(unsignedchar*)lpDIBBits+lLineBytes*m_iLVt+m_iLUt-1)=255;*(unsignedchar*)lpDIBBits+lLineBytes*m_iLVt+m_iLUt+1)=255;/rightwheeltrackspot*(unsignedchar*)lpDIBBits+lLineBytes*m_iRVt+m_iRUt)=255;*(unsignedchar*)lpDIBBits+lLineBytes*(m_iRVt-1)+m_iRUt)=255;*(unsignedchar*)lpDIBBits+lLineBytes*(m_iRVt+1)+m_iRUt)=255;*(unsignedchar*)lpDIBBits+lLineBytes*m_iRVt+m_iRUt-1)=255;*(unsignedchar*)lpDIBBits+lLineBytes*m_iRVt+m_iRUt+1)=255;voidCBackTrack:DrawExtendSpot(unsignedchar*lpDIBBits,intlWidth,intlHeight,boolbInverse)if(bInverse)InverseBmp(lpDIBBits,lWidth,lHeight);intt;longlLineBytes=WIDTHBYTES(lWidth*8);for(t=0;tm_iTlong;t+)/leftwheeltrackspot*(unsignedchar*)lpDIBBits+lLineBytes*m_iLeVt+m_iLeUt)=255;*(unsignedchar*)lpDIBBits+lLineBytes*(m_iLeVt-1)+m_iLeUt)=255;*(unsignedchar*)lpDIBBits+lLineBytes*(m_iLeVt+1)+m_iLeUt)=255;*(unsignedchar*)lpDIBBits+lLineBytes*m_iLeVt+m_iLeUt-1)=255;*(unsignedchar*)lpDIBBits+lLineBytes*m_iLeVt+m_iLeUt+1)=255;/rightwheeltrackspot*(unsignedchar*)lpDIBBits+lLineBytes*m_iReVt+m_iReUt)=255;*(unsignedchar*)lpDIBBits+lLineBytes*(m_iReVt-1)+m_iReUt)=255;*(unsignedchar*)lpDIBBits+lLineBytes*(m_iReVt+1)+m_iReUt)=255;*(unsignedchar*)lpDIBBits+lLineBytes*m_iReVt+m_iReUt-1)=255;*(unsignedchar*)lpDIBBits+lLineBytes*m_iReVt+m_iReUt+1)=255;if(bInverse)InverseBmp(lpDIBBits,lWidth,lHeight);voidCBackTrack:DrawBackTrack(unsignedchar*lpDIBBits,intlWidth,intlHeight,boolbInverse)if(bInverse)InverseBmp(lpDIBBits,lWidth,lHeight);intt,tj;floatk;intU,offset;longlLineBytes=WIDTHBYTES(lWidth*8);for(t=0;tm_iTlong-1;t+)/drawleftwheelbacktrackk=(float)(m_iLUt-m_iLUt+1)/(float)(m_iLVt-m_iLVt+1);if(m_iLVtm_iLVt+1)for(tj=m_iLVt;tjm_iLVt+1;tj+)offset=lLineBytes*tj;U=(int)(m_iLUt-k*(m_iLVt-tj);if(UlWidth-1&tjlHeight-1&tj1&U1)*(unsignedchar*)lpDIBBits+offset+U)=255;*(unsignedchar*)lpDIBBits+offset-lLineBytes+U)=255;*(unsignedchar*)lpDIBBits+offset+lLineBytes+U)=255;*(unsignedchar*)lpDIBBits+offset+U+1)=255;*(unsignedchar*)lpDIBBits+offset+U-1)=255;elseif(m_iLVtm_iLVt+1)for(tj=m_iLVt;tjm_iLVt+1;tj-)offset=lLineBytes*tj;U=(int)(m_iLUt-k*(m_iLVt-tj);if(UlWidth-1&tjlHeight-1&tj1&U1)*(unsignedchar*)lpDIBBits+offset+U)=255;*(unsignedchar*)lpDIBBits+offset-lLineBytes+U)=255;*(unsignedchar*)lpDIBBits+offset+lLineBytes+U)=255;*(unsignedchar*)lpDIBBits+offset+U+1)=255;*(unsignedchar*)lpDIBBits+offset+U-1)=255;/drawrightwheelbacktrackk=(float)(m_iRUt-m_iRUt+1)/(float)(m_iRVt-m_iRVt+1);if(m_iRVtm_iRVt+1)for(tj=m_iRVt;tjm_iRVt+1;tj+)offset=lLineBytes*tj;U=(int)(m_iRUt-k*(m_iRVt-tj);if(UlWidth-1&tjlHeight-1&tj1&U1)*(unsignedchar*)lpDIBBits+offset+U)=255;*(unsignedchar*)lpDIBBits+offset-lLineBytes+U)=255;*(unsignedchar*)lpDIBBits+offset+lLineBytes+U)=255;*(unsignedchar*)lpDIBBits+offset+U+1)=255;*(unsignedchar*)lpDIBBits+offset+U-1)=255;elseif(m_iRVtm_iRVt+1)for(tj=m_iRVt;tjm_iRVt+1;tj-)offset=lLineBytes*tj;U=(int)(m_iRUt-k*(m_iRVt-tj);tj1&U1)offset+U)=255;offset-lLineBytes+U)=255;offset+lLineBytes+U)=255;offset+U+1)=255;offset+U-1)=255;if(UlWidth-1&tjlHeight-1&*(unsignedchar*)lpDIBBits+*(unsignedchar*)lpDIBBits+*(unsignedchar*)lpDIBBits+*(unsignedchar*)lpDIBBits+*(unsignedchar*)lpDIBBits+if(bInverse)InverseBmp(lpDIBBits,lWidth,lHeight);voidCBackTrack:DrawExtendTrack(unsignedchar*lpDIBBits,intlWidth,intlHeight)intt,tj;floatk;intV;longlLineBytes=WIDTHBYTES(lWidth*8);for(t=0;tm_iTlong-1;t+)/drawleftwheelextendtrackif(m_iLeUtm_iLeUt+1)for(tj=m_iLeUt;tjm_iLeUt+1;tj-)k=(float)(m_iLeVt-m_iLeVt+1)/(float)(m_iLeUt-m_iLeUt+1);V=(int)(k*tj+m_iLeVt-k*m_iLeUt);if(tjlWidth-1&VlHeight-1&tj1&V1)*(unsignedchar*)lpDIBBits+lLineBytes*V+tj)=255;*(unsignedchar*)lpDIBBits+lLineBytes*(V-1)+tj)=255;*(unsignedchar*)lpDIBBits+lLineBytes*(V+1)+tj)=255;*(unsignedchar*)lpDIBBits+lLineBytes*V+tj+1)=255;*(unsignedchar*)lpDIBBits+lLineBytes*V+tj-1)=255;elsefor(tj=m_iLeUt;tjm_iLeUt+1;tj+)k=(float)(m_iLeVt-m_iLeVt+1)/(float)(m_iLeUt-m_iLeUt+1);V=(int)(k*tj+m_iLeVt-k*m_iLeUt);if(tjlWidth-1&VlHeight-1&tj1&V1)*(unsignedchar*)lpDIBBits+lLineBytes*V+tj)=255;*(unsignedchar*)lpDIBBits+lLineBytes*(V-1)+tj)=255;*(unsignedchar*)lpDIBBits+lLineBytes*(V+1)+tj)=255;*(unsignedchar*)lpDIBBits+lLineBytes*V+tj+1)=255;*(unsignedchar*)lpDIBBits+lLineBytes*V+tj-1)=255;/drawrightwheelextendtrackif(m_iReUtm_iReUt+1)for(tj=m_iReUt;tjm_iReUt+1;tj-)k=(float)(m_iReVt-m_iReVt+1)/(float)(m_iReUt-m_iReUt+1);V=(int)(k*tj+m_iReVt-k*m_iReUt);if(tjlWidth-1&VlHeight-1&tj1&V1)*(unsignedchar*)lpDIBBits+lLineBytes*V+tj)=255;*(unsignedchar*)lpDIBBits+lLineBytes*(V-1)+tj)=255;*(unsignedchar*)lpDIBBits+lLineBytes*(V+1)+tj)=255;*(unsignedchar*)lpDIBBits+lLineBytes*V+tj+1)=255;*(unsignedchar*)lpDIBBits+lLineBytes*V+tj-1)=255;elsefor(tj=m_iReUt;tjm_iReUt+1;tj+)k=(float)(m_iReVt-m_iReVt+1)/(float)(m_iReUt-m_iReUt+1);V=(int)(k*tj+m_iReVt-k*m_iReUt);if(tjlWidth-1&VlHeight-1&tj1&V1)*(unsignedchar*)lpDIBBits+lLineBytes*V+tj)=255;*(unsignedchar*)lpDIBBits+lLineBytes*(V-1)+tj)=255;*(unsignedchar*)lpDIBBits+lLineBytes*(V+1)+tj)=255;*(unsignedchar*)lpDIBBits+lLineBytes*V+tj+1)=255;*(unsignedchar*)lpDIBBits+lLineBytes*V+tj-1)=255;boolCBackTrack:InitObject(CRearCalibration*pCali,floatfAngle,floatfAxleDist,floatfWheelDist,intlWidth,intlHeight)if(!pCali-IfDoneCali()returnfalse;m_LRearAxle=fAxleDist;m_LRearWheel=fWheelDist;m_pCali=pCali;m_fAngle=fAngle;GetTfromView(lWidth,lHeight);ComputerExtendXY();GetExtendUV();ComputerXY();ComputerUV();returntrue;voidCBackTrack:InverseBmp(unsignedchar*lpDIBBits,intlWidth,intlHeight)unsignedchar*pData=newunsignedcharlWidth*lHeight;inti,j,off1=0,off2=(lHeight-1)*lWidth;for(i=0;ilHeight;i+)for(j=0;j416)k=(float)(28)/(float)(416-480);b=72-k*480;elseif(v=416&v322)k=(float)(40)/(float)(322-416);b=100-k*416;elseif(v=322&v276)k=(float)(40)/(float)(276-322);b=140-k*322;elseif(v=276&v248)k=(float)(40)/(float)(248-276);/-1.42857b=180-k*276;elseif(v=248&v230)k=(float)(40)/(float)(230-248);/-2.22222b=220-k*248;/771.1111elseif(v=230&v216)k=(float)(40)/(float)(216-230);/-2.85714b=260-k*230;/917.143elseif(v=216&v206)k=(float)(40)/(float)(206-216);/-4.000b=300-k*216;/1164.00elseif(v=206&v198)k=(float)(40)/(float)(198-206);/-5.0000b=340-k*206;/1370.00elseif(v=198&v186)k=(float)(80)/(float)(186-198);/=-6.6667;b=380.0f-k*198.0f;/1700.00elseif(v=186&v=177)k=(float)(80)/(float)(177-186);b=460-k*186;y=(short)(k*v+b+0.5);returny;/*GetMarkXU()找两条直线作为标线参考本处去X=40和X=-40这两条参考线*/voidCBackTrack:GetMarkXU()shorti;shortl=0;shortU1Top,U1Bott,U2Top,U2Bott;floatk1,k2,b1,b2;U1Top=273;U1Bott=124+4;U2Top=362;U2Bott=508+6;k1=(198.0f-410.0f)/(float)(U1Top-U1Bott);k2=(198.0f-410.0f)/(float)(U2Top-U2Bott);b1=198.0f-k1*U1Top;b2=198.0f-k2*U2Top;for(i=198;i=480;i+)m_U1i-198=(short)(i-b1)/k1+0.5);m_U2i-198=(short)(i-b2)/k2+0.5);/*GetUfromV()根据输入的V值，采用标线法求出V行上倒车轨迹在该行对应的点入口：第几行*/CPointCBackTrack:GetUfromV(shortv)floatxr,xl,y;floata,b,c,temp1,temp2;shortUr,Ul;CPointp;y=RelationYV(v);a=m_LRearAxle/(float)tan(m_fAngle);b=a+m_LRearWheel/2;c=a-m_LRearWheel/2;temp1=(float)sqrt(b*b-y*y);temp2=(float)sqrt(c*c-y*y);if(m_fAngle0)xr=temp1-a;xl=temp2-a;Ur=(short)(xr+40)*(m_U1v-198-m_U2v-198)/80.0+m_U2v-198+0.5);Ul=(short)(xl+40)*(m_U1v-198-m_U2v-198)/80.0+m_U2v-198+0.5);p.x=Ur;p.y=Ul;returnp;/*GetUfromVofExtend(shortv)根据输入的V值，采用标线法求出V行上两后轮延长线在该行对应的点入口：第几行*/CPointCBackTrack:GetUfromVofExtend(shortv)floatxr,xl;shortUr,Ul;CPointp;xr=m_LRearWheel/2;xl=-m_LRearWheel/2;Ur=(short)(float)(xr+40)*(float)(m_U1v-198-m_U2v-198)/80.0+m_U2v-198+0.5);Ul=(short)(float)(xl+40)*(float)(m_U1v-198-m_U2v-198)/80.0+m_U2v-198+0.5);p.x=Ur;p.y=Ul;returnp;DrawTrack()将轨迹线和延长线同时画出*/voidCBackTrack:DrawTrack(unsignedchar*lpDIBBits,intlWidth,intlHeight)longlLineBytes=WIDTHBYTES(lWidth*8);CPointp;intur,ul,uer,uel;shorti;intoffset=100;lHeight-=offset;for(i=198;i=0&ur=lWidth)*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i)+ur)=0;*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i+1)+ur)=0;*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i-1)+ur)=0;*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i)+ur+1)=0;*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i)+ur-1)=0;*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i)+ur+2)=0;*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i)+ur-2)=0;if(ul=0&ul=lWidth)*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i)+ul)=0;*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i+1)+ul)=0;*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i-1)+ul)=0;*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i)+ul+1)=0;*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i)+ul-1)=0;*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i)+ul+2)=0;*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i)+ul-2)=0;p=GetUfromVofExtend(i);uer=p.x;uel=p.y;if(uer=0&uer=lWidth)*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i)+uer)=155;*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i+1)+uer)=155;*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i-1)+uer)=155;*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i)+uer+1)=155;*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i)+uer-1)=155;*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i)+uer+2)=155;*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i)+uer-2)=155;if(uel=0&uel=lWidth)*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i)+uel)=155;*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i+1)+uel)=155;*(unsignedchar*)lpDIBBits+lLineBytes(lHeight-i-1)+uel)=155;(lHeight-i)+uel+1)=155;(lHeight-i)+uel-1)=155;(lHeight-i)+uel+2)=155;(lHeight-i)+uel-2)=155;*(unsignedchar*)lpDIBBits+lLineBytes*(unsignedchar*)lpDIBBits+lLineBytes*(unsignedchar*)lpDIBBits+lLineBytes*(unsignedchar*)lpDIBBits+lLineBytes