【正文】 ,rrty[t],rrtz[t])。 if(min min1) { min1 = min。 kmin = t。 } } //計算角度 if((xrand[j]rrtx[kmin])!=0) { float fz = yrand[j]rrty[kmin]。 float fm = xrand[j]rrtx[kmin]。 anglex[j+1] = atan(fz/fm)。 } else anglex[j+1] = pi/2。 float xyDistance = puter_distance(xrand[j],yrand[j],0,rrtx[kmin],rrty[kmin],0)。 anglez[j+1] = atan(xyDistance / (zrand[j]rrtz[kmin]))。 //取步長step和最小距離min1中較小者 if(min1 step) min_step = step 。 else min_step = min1。 det_anglex = anglex[j+1]anglex[j]。 det_anglez = anglez[j+1]anglez[j]。 r = random()/。 //得到10組tempt點 if(det_anglex ph) { xtemp[j]=rrtx[kmin]+min_step*r*cos(anglex[j]+ph)。 ytemp[j]=rrty[kmin]+min_step*r*sin(anglex[j]+ph)。 } else if(det_anglex ph) { xtemp[j]=rrtx[kmin]+min_step*r*cos(anglex[j]ph)。 ytemp[j]=rrty[kmin]+min_step*r*sin(anglex[j]ph)。 } else { xtemp[j]=rrtx[kmin]+min_step*r*cos(det_anglex)。 ytemp[j]=rrty[kmin]+min_step*r*sin(det_anglex)。 } if(det_anglez fy) ztemp[j]=rrtz[kmin]+min_step*r*cos(anglez[j]+fy)。 else if(det_anglez fy) ztemp[j]=rrtz[kmin]+min_step*r*cos(anglez[j]fy)。 else ztemp[j]=rrtz[kmin]+min_step*r*cos(det_anglez)。 if(det_anglex ph) { xtemp[j]=rrtx[kmin]+min_step*r*cos(anglex[j]+ph)。 ytemp[j]=rrty[kmin]+min_step*r*sin(anglex[j]+ph)。 } else if(det_anglex ph) { xtemp[j]=rrtx[kmin]+min_step*r*cos(anglex[j]ph)。 ytemp[j]=rrty[kmin]+min_step*r*sin(anglex[j]ph)。 } //判斷該tempt點是否遇到威脅 for(int k=0。k15。k++) { float TemptThrDis = puter_distance(xtemp[j],ytemp[j],ztemp[j], thr_path[k].lon,thr_path[k].lat,thr_path[k].alt)。 if(TemptThrDis safe_dis) { flag = 0。 break。 } else flag = 1。 } if(flag) { float TemptEndDis = puter_distance(xtemp[j],ytemp[j],ztemp[j],endx,endy,endz)。 if(TemptEndDis min_dis) { min_dis = TemptEndDis。 knew = j。 //new點產(chǎn)生 } } } m = kmin+1。// m++。// printf(rrt樹中m=%d時knew為%d ,m,knew)。 if((knew=0)amp。amp。(knew10)) { rrtx[m] = xtemp[knew]。 rrty[m] = ytemp[knew]。 rrtz[m] = ztemp[knew]。 d[m+1]=puter_distance(rrtx[m],rrty[m],rrtz[m],endx,endy,endz)。 if(d[m+1]d[m]) m。 // else // printf(此時距離為%f\n,d[m+1])。 } } int i=0。 while((rrtx[i]!=0)||(rrty[i]!=0)||(rrtz[i]!=0)) { pre_path[num1+i].lon = rrtx[i]。 printf(pre_path[%d].lon = %f\n,num1+i,pre_path[num1+i].lon)。 pre_path[num1+i].lat = rrty[i]。 pre_path[num1+i].alt = rrtz[i]。 i++。 }// printf(i = %d\n,i)。 for(j=0 。 j99inum 。 j++) { pre_path[num1+i+j].lon = save[j].lon。 printf(pre_path[%d].lon = %f\n,num1+i+j,pre_path[num1+i+j].lon)。 pre_path[num1+i+j].lat = save[j].lat。 pre_path[num1+i+j].alt = save[j].alt。 } // printf(num+i+1+j = %d\n,num+i+1+j)。}void main(){ double t = time(0)*。 init()。 int i=0。 FILE *fp。 if((fp=fopen(,wb+))==NULL) { printf(cannot open\n)。 exit(0)。 }// while((pre_path[i].lon!=0)||(pre_path[i].lat!=0)||(pre_path[i].alt!=0)) while(pre_path[i].lon!=0) { PreThrDistance = puter_distance(pre_path[i].lon,pre_path[i].lat,pre_path[i].alt, thr_path[i].lon,thr_path[i].lat,thr_path[i].alt)。// printf(%f\n,PreThrDistance)。 if(PreThrDistance safe_dis) {// printf(%f,cos(pi/2))。 rrt(i)。 printf(進入RRT算法\n)。 } fprintf(fp,%f %f %f ,pre_path[i].lon,pre_path[i].lat,pre_path[i].alt)。 i++。// fprintf(fp,\n)。 } t=time(0)* t。 printf(耗時%f毫秒,t)。}附錄（二）　Matlab仿真實驗驗證程序function showpath[tx,ty,tz] = textread(39。39。,39。% % %39。)。[tm,tn] = size(tx)。p = polyfit(tx,ty,5)。 for i=1:1:tm tyy(i)=p(1)*tx(i)^5+p(2)*tx(i)^4+p(3)*tx(i)^3+p(4)*tx(i)^2+p(5)*tx(i)+p(6)。 endq = polyfit(tx,tz,5)。 for i=1:1:tm tzz(i)=q(1)*tx(i)^5+q(2)*tx(i)^4+q(3)*tx(i)^3+q(4)*tx(i)^2+q(5)*tx(i)+q(6)。 end [x,y,z]=textread(39。39。,39。% % %39。)。[m,n] = size(x)。r = polyfit(x,y,5)。 for i=1:1:m yy(i)=r(1)*x(i)^5+r(2)*x(i)^4+r(3)*x(i)^3+r(4)*x(i)^2+r(5)*x(i)+r(6)。 ends = polyfit(x,z,5)。 for i=1:1:m zz(i)=s(1)*x(i)^5+s(2)*x(i)^4+s(3)*x(i)^3+s(4)*x(i)^2+s(5)*x(i)+s(6)。 end [px,py,pz]=textread(39。39。,39。% % %39。)。 [pm,pn] = size(px)。u = polyfit(px,py,5)。 for i=1:1:pm pyy(i)=u(1)*px(i)^5+u(2)*px(i)^4+u(3)*px(i)^3+u(4)*px(i)^2+u(5)*px(i)+u(6)。 endv = polyfit(px,pz,5)。 for i=1:1:pm pzz(i)=v(1)*px(i)^5+v(2)*px(i)^4+v(3)*px(i)^3+v(4)*px(i)^2+v(5)*px(i)+v(6)。 end%plot3(tx,tyy,tzz,39。r39。,x,y,z),grid on%figure,plot3(tx,tyy,tzz,39。r39。,x,yy,zz),grid on[a,b,c]=sphere(10)。a=a.*+45。b=b.*+41。c=c.*+22。mesh(a,b,c),hold onplot3(tx,tyy,tzz,39。ro39。,px,py,pz,39。go39。),grid onend