www.pudn.com > globalpathplanning.rar > optimizePoint15.asv, change:2012-03-09,size:1987b


%龙格库塔求解公式15 
function lgkt15=optimizePoint15( point, i, flag) 
 
    switch flag 
    
    case 0 
            k1=-formula15(0,tempPoint,flag); 
            k1=k1-1.6*(2*point(i).x-point(i-1).x-point(i+1).x); 
            k2=-formula15(0+0.01/2,tempPoint+0.01*k1/2,flag); 
            k2=k2-1.6*(2*(point(i).x+0.01*k1/2)-point(i-1).x-point(i+1).x); 
            k3=-formula15(0+0.01/2,tempPoint+0.01*k2/2,flag); 
            k3=k3-1.6*(2*(point(i).x+0.01*k2/2)-point(i-1).x-point(i+1).x); 
            k4=-formula15(0+0.01,tempPoint+0.01*k3,flag); 
            k4=k4-1.6*(2*(point(i).x+0.01*k3)-point(i-1).x-point(i+1).x); 
            point(i).x=point(i).x+0.01*(k1+k2+k3+k4)/6; 
            break;                        
     case 1 
            k1=-formula15(0,tempPoint,flag); 
            k1=k1-1.6*(2*point(i).y-point(i-1).y-point(i+1).y); 
            k2=-formula15(0+0.01/2,tempPoint+0.01*k1/2,flag); 
            k2=k2-1.6*(2*(point(i).y+0.01*k1/2)-point(i-1).y-point(i+1).y); 
            k3=-formula15(0+0.01/2,tempPoint+0.01*k2/2,flag); 
            k3=k3-1.6*(2*(point(i).y+0.01*k2/2)-point(i-1).y-point(i+1).y); 
            k4=-formula15(0+0.01,tempPoint+0.01*k3,flag); 
            k4=k4-1.6*(2*(point(i).y+0.01*k3)-point(i-1).y-point(i+1).y); 
            point(i).y=point(i).y+0.01*(k1+k2+k3+k4)/6; 
            break; 
      case 2 
            k1=-formula15(0,tempPoint,flag); 
            k1=k1-1.6*(2*point(i).z-point(i-1).z-point(i+1).z); 
            k2=-formula15(0+0.01/2,tempPoint+0.01*k1/2,flag); 
            k2=k2-1.6*(2*(point(i).z+0.01*k1/2)-point(i-1).z-point(i+1).z); 
            k3=-formula15(0+0.01/2,tempPoint+0.01*k2/2,flag); 
            k3=k3-1.6*(2*(point(i).z+0.01*k2/2)-point(i-1).z-point(i+1).z); 
            k4=-formula15(0+0.01,tempPoint+0.01*k3,flag); 
            k4=k4-1.6*(2*(point(i).z+0.01*k3)-point(i-1).z-point(i+1).z); 
            point(i).z=point(i).z+0.01*(k1+k2+k3+k4)/6; 
            break;              
    end