www.pudn.com > globalpathplanning.rar > isInObject.asv, change:2012-03-12,size:2940b


%判断所有的点是否在物体内部  将C++中的布尔运算全部改为0 1 运算  
 
function Y=isInObject(rpoint)	 
    %定义全局变量,保存权重等参数值 
 
    isrpointInside(1,:)=ones(1,total);%定义标记,判断是否38个路径结点是否都在物体外部,1表在物体内部	 
	for k=1:100  %设置结束条件,比如最大循环次数k 
      
		isAllOutside=1;%定义标记,38个点只要存在一点在内部,则标记为0,初始1假定都在外部 
                       
		for i=2:total-1  %对点P2 to P total判断,若点在物体内部flag[i]标记为1 
             
			 for j=1:6 %判断每个点			 
				  temp=rpoint(i).x*weightx(j)+rpoint(i).y*weighty(j)+rpoint(i).z*weightz(j)+H(j); 
					 if temp<=0            %if表达式成立则代表点是在外部的。				  
					   isrpointInside(i)=0;%只要有一个不符合,则在物体外部,设置点Pi对应的标记为0 
					   break;%判断出该点在外部,跳出此次循环 
                     end 
             end 
			 if j > 6 
			 %temp都大于0,此中的点是在内部。 
				 isAllOutside=0;%j>6表示j循环正常结束,此节点为内部部点,设置为0 
				 isrpointInside(i)=1;			  
             end    
        end 
         for i = 2:total-1 
			 if isrpointInside(1,i)==1             %点为内部的点,用公式15优化	 
		  
				for coord = 0;2 %此中coord代表选取哪一个坐标轴上的值 
                    optimizerpoint15(rpoint,i,coord);   %这里函数调用的时候直接传递结构体rpoint,i表示第i个点,coord表示x,y,z 
                end 
             else                                %外部点则用公式18优化 
			 %公式18计算			 
				k1 = -0.1*(2*rpoint(i).x -rpoint(i-1).x -rpoint(i+1).x );  
				k2 = -0.1*(2*(rpoint(i).x+0.01*k1/2) -rpoint(i-1).x -rpoint(i+1).x ); 
				k3 = -0.1*(2*(rpoint(i).x+0.01*k2/2) -rpoint(i-1).x -rpoint(i+1).x ); 
				k4 = -0.1*(2*(rpoint(i).x+0.01*k3) -rpoint(i-1).x -rpoint(i+1).x ); 
				rpoint(i).x = rpoint(i).x +0.01*(k1+2*k2+2*k3+k4)/6; 
 
				k1 = -0.1*(2*rpoint(i).y -rpoint(i-1).y -rpoint(i+1).y );  
				k2 = -0.1*(2*(rpoint(i).y+0.01*k1/2) -rpoint(i-1).y -rpoint(i+1).y ); 
				k3 = -0.1*(2*(rpoint(i).y+0.01*k2/2) -rpoint(i-1).y -rpoint(i+1).y ); 
				k4 = -0.1*(2*(rpoint(i).y+0.01*k3) -rpoint(i-1).y -rpoint(i+1).y ); 
				rpoint(i).y = rpoint(i).y +0.01*(k1+2*k2+2*k3+k4)/6; 
 
				k1 = -0.1*(2*rpoint(i).z -rpoint(i-1).z -rpoint(i+1).z );  
				k2 = -0.1*(2*(rpoint(i).z+0.01*k1/2) -rpoint(i-1).z -rpoint(i+1).z ); 
				k3 = -0.1*(2*(rpoint(i).z+0.01*k2/2) -rpoint(i-1).z -rpoint(i+1).z ); 
				k4 = -0.1*(2*(rpoint(i).z+0.01*k3) -rpoint(i-1).z -rpoint(i+1).z ); 
				rpoint(i).z = rpoint(i).z +0.01*(k1+2*k2+2*k3+k4)/6;			  
             end 
			isrpointInside(i)=0;%重新初始化标记,方便下一步循环判断	 
         end 
     
    end 
        %%所有点都在外部时 输出结果 
		if isAllOutside==1 	 
			for count=1;total-1			 
				fprintf('最终结果为:'); 
				fprintf('x : %f',rpoint(count).x ); 
				fprintf('y : %f',rpoint(count).y ); 
				fprintf('z : %f',rpoint(count).z);               
            end 
             Y=1;          %所有点都在外部时,赋值Y=1 
        end  
end