makeroadtopology.m

用于计算多体系统动力学的程序,可用于多体系统动力学的计算和仿镇,也可用于机器人的设计和计算

% 形成可以计算的拓扑数据，即路及相关数组
function MakeRoadTopology

% 声明全局变量
GlobalVariables

s(1:nBodies+1,1:nBodies)=0;  
road1(1:(nBodies*(nBodies+1)))=0;     
road2(1:(nBodies*nBodies))=0;   
roadnumber(1:nBodies)=0;
for i=1:nBodies
	iplus(i)=joint(i).inbody;
end		
% 形成关联矩阵
for  i=1:nBodies+1
	for j=1:nBodies
		if  (i-1)==iplus(j)
			s(i,j)=1;
		elseif (i-1)==j
			s(i,j)=-1;
		else 
			s(i,j)=0;
		end
	end
end 
% 形成系统的路有关数组road,其中1元存放路的条数,2到road(1)+1元存放每条路最后一个元素在数组中的位置,则road(3)-road(2)为第二条路元素的总数目
% 数组roadnumber的2*i-1元存放第i个刚体首次所在的路数,2*i元存放第i个刚体在该路的位置

k=0;
for i=1:nBodies
	kk=0;
	% 计算系统端点的个数
	for   j=1:nBodies
		if s(i+1,j)~=0
			kk=kk+1;
		end
    end
	if kk==1
		k=k+1;
		% road1表示系统的末端物体
		road1(k+1)=i;
	end
end

road1(1)=k;
road(1:nBodies*(nBodies+1))=0;

road(1)=k;
ll=road(1)+1;
for i=1:road(1)
	mm=0;
	road2=0;
	l=road1(i+1);
	while l~=0  
		mm=mm+1;
		road2(mm)=l;
		l=iplus(l);
	end
	for j=1:mm
		road(ll+j)=road2(mm-j+1);
	end
    % ll是每条路最后一个元素在road的位置。
	ll=ll+mm;	  
	road(i+1)=ll;
end

% 形成T矩阵
t=0;
for i=1:road(1)
	if i==1
		begin=road(1)+1;
		n1=road(2)-road(1)-1;
	else 
		begin=road(i);
		n1=road(i+1)-road(i);
	end
	for j=begin+1:begin+n1
		for k=j:begin+n1
			t(road(j),road(k))=1;
		end
	end
end

% pos(2*i-1)记录BO到该物体的路上的所有物体在数组Pos中的位置，pos（2*nBodies...)记录从B0 到每个物体的路上的所有物体的序号。
% Pos（2*i）记录该路上的物体在数组dtotaL上的最末位置   % 记录Bo到每个物体上的物体序号及个数   
o=0;
o1=2*nBodies;
					    
for i=1:nBodies
	l=0;
    l1=0;
	for j=1:i
		if  t(j,i)==1
			l=l+joint(j).trfree+joint(j).rofree;
			l1=l1+1;
			pos(o1+l1)=j;
        end 
	end
	o1=o1+l1; 
	if i==1
		pos(2*i-1)=o1;
	else
		pos(2*i-1)=pos(2*(i-1)-1)+l1;
	end
	o=o+l;   
	pos(2*i)=o;
end

% 记录第i个铰在那些物体路上,记录最后一个物体的位置
o=nBodies;
for i=1:nBodies
	l=0;
	for j=i:nBodies
		if t(i,j)==1
			l=l+1;
			pos2(o+l)=j;
		end
	end
	o=o+l;
	pos2(i)=o;
end

% 形成力元的有关数祖
if nef~=0
	for i=1:nef
		for j=1:nBodies
			if j==ef(i).ek
				se(j,i)=1;
			end
			if j==ef(i).eek
				se(j,i)=-1;
			end
		end
	end
end
j=0;
for i=1:road(1)
	if i==1
		for j=road(1)+2:road(2)
		end
	else 
		for j=road(j-1)+1:road(j)
		end
	end
end
'☆ 形成可以计算的拓扑数据，即路及相关数组程序 MakeRoadTopology.m 运行结束 √'