基于多边形模型接触力（PCM）-接触检测

最新推荐文章于 2023-03-22 09:24:18 发布

qq_33276575

最新推荐文章于 2023-03-22 09:24:18 发布

阅读量2.3k

点赞数

文章标签： matlab 算法

本文链接：https://blog.csdn.net/qq_33276575/article/details/125834130

版权

计算接触力的步骤主要分为两个步骤：接触检测以及利用接触模型计算接触力。本文基于多边形模型实现接触检测算法。

实现步骤：

1. 实体模型表面网格划分（本文旨在用三角形划分实体表面）

2. 基于方向包围盒（OBB）的层次包围盒二叉树结构的构造

3. 接触检测

1. 实体模型表面网格划分

基于Comsol软件划分实体网格，并导出网格（包括边界单元），最后导入Matlab

2.OBB层次包围盒二叉树构造

层次包围盒原理（上至下）：顶层的包围盒包含整个表面，基于主成分分析，确定分离轴与分离点将上整个表面分成两部分并生成包围盒，以此类推，直至一个包围盒包含一个三角形单元。

基于matlab的层次包围盒代码：

其中节点信息nodes为N×3矩阵，单元信息element为M×3矩阵

function BVH=BVHConstruction(nodes,element)
level=1;
BVH.Level(level).Nodes(1).Element=element;
BoundingVoulum=CalculatBoundingVolumMeanpointAndAxis(nodes,element);
BVH.Level(level).Nodes(1).BoundingVoulum=BoundingVoulum;
flag=1;
while(flag==1)
    if level==11
     aaa=0;
    end
    numnextnode=0;
    numbernodes=size(BVH.Level(level).Nodes,2);
    level=level+1;
    flag=0;
    for i=1:numbernodes
        disp(['Decompose level: ',num2str(level-1),' Node: ',num2str(i)])
        if i==541
          aaa=0;
        end
        tempelement=BVH.Level(level-1).Nodes(i).Element;
        numele=size(tempelement,1);
        if (numele==0)
           aaa=0;
        end
        if numele~=1
            [leftelement,rightelement,BoundingVoulumL,BoundingVoulumR]=CalculatMeanAndCovariance(nodes,tempelement);
            numnextnode=numnextnode+1;
            BVH.Level(level).Nodes(numnextnode).Element=leftelement;
            BVH.Level(level).Nodes(numnextnode).BoundingVoulum=BoundingVoulumL;
            numnextnode=numnextnode+1;
            BVH.Level(level).Nodes(numnextnode).Element=rightelement;
            BVH.Level(level).Nodes(numnextnode).BoundingVoulum=BoundingVoulumR;
            BVH.Level(level-1).Nodes(i).ChildNodes=[numnextnode-1,numnextnode];
            flag=1;
        else
            BVH.Level(level-1).Nodes(i).ChildNodes=[];
        end
    end
end
end

OBB包围盒生成代码

输入：节点信息，单元信息

输出：OBB包围盒结构体（包围盒方向轴axis，包围盒中心点centerpoint，包围盒半长、半宽、半高parameters）

function BoundingVoulumR=CalculatBoundingVolumMeanpointAndAxis(nodes,element)
nodesindex=unique(reshape(element,[],1));
nodesR=nodes(nodesindex,:);
C=cov(nodesR);
[eigvector,eigvalue]=eig(C);
eigvector=[eigvector(:,1)/norm(eigvector(:,1)),eigvector(:,2)/norm(eigvector(:,2)),eigvector(:,3)/norm(eigvector(:,3))];
eigvector(:,3)=cross(eigvector(:,1),eigvector(:,2));
nodesR=inv(eigvector)*(nodesR');
minXR=min(nodesR(1,:));
minYR=min(nodesR(2,:));
minZR=min(nodesR(3,:));
maxXR=max(nodesR(1,:));
maxYR=max(nodesR(2,:));
maxZR=max(nodesR(3,:));
LengthR=abs(maxXR-minXR)/2;
WidthR=abs(maxYR-minYR)/2;
HigthR=abs(maxZR-minZR)/2;
center=eigvector*[(maxXR+minXR)/2;(maxYR+minYR)/2;(maxZR+minZR)/2];
BoundingVoulumR.centerpoint=center;
BoundingVoulumR.axis=eigvector;
BoundingVoulumR.parameters=[LengthR,WidthR,HigthR];
end

单元个数不等于1的拆分程序：

function [leftelement,rightelement,BoundingVoulumL,BoundingVoulumR]=CalculatMeanAndCovariance(nodes,element)
numele=size(element,1);
p=nodes(element(:,1),:);
q=nodes(element(:,2),:);
r=nodes(element(:,3),:);
splitpoint=(sum(p+q+r)/(3*numele))';
C=zeros(3,3);
p_hat=p-splitpoint';
q_hat=q-splitpoint';
r_hat=r-splitpoint';
for j=1:3
    for k=1:3
        C(j,k)=sum(p_hat(:,j).*p_hat(:,k)+q_hat(:,j).*q_hat(:,k)+r_hat(:,j).*r_hat(:,k))/(3*numele);
    end
end
[eigvector,eigvalue]=eig(C);
eigvalue=diag(eigvalue);
[~,sortindex]=sort(abs(eigvalue),'descend');
eigvector=[eigvector(:,1)/norm(eigvector(:,1)),eigvector(:,2)/norm(eigvector(:,2)),eigvector(:,3)/norm(eigvector(:,3))];
eigvector(:,3)=cross(eigvector(:,1),eigvector(:,2));
% splitaxis=eigvector(:,maxvalueindex);
elementcenterpoints=(p+q+r)/3;
% crossareas=cross((p-q)',(p-r)');
% areas=sqrt(sum(crossareas.^2));
% areas=repmat(areas',1,3);
% elementcenterpoints=areas.*elementcenterpoints;

% figure(1)
% hold on
% pdeplot(nodes(:,1:2)',element')
% plot(elementcenterpoints(:,1),elementcenterpoints(:,2),'o')
% plot(splitpoint(1),splitpoint(2),'ro')
% plot([splitpoint(1),splitpoint(1)+eigvector(1,maxvalueindex)],[splitpoint(2),splitpoint(2)+eigvector(2,maxvalueindex)])

elementcenterpoints=inv(eigvector)*(elementcenterpoints'-splitpoint);
for i=1:3
    leftindex=find(elementcenterpoints(sortindex(i),:)<0);
    if ~isempty(leftindex)&&length(leftindex)~=size(element,1)
       break;
    end
end
leftelement=element(leftindex,:);
rightelement=element;
rightelement(leftindex,:)=[];

BoundingVoulumL=CalculatBoundingVolumMeanpointAndAxis(nodes,leftelement);

BoundingVoulumR=CalculatBoundingVolumMeanpointAndAxis(nodes,rightelement);
end

3.接触检测

接触检测主要分为包围盒间的接触检测、三角形的接触检测以及三角形相交直线端点的求解

3.1整体检测程序：

function [disjointflag,ContactPaireNodes,ContactPaireLevel,ContactPaireLines]=BodyContact_Detection(nodesA,nodesB,BodyABVH,BodyBBVH,qrA,qrB)
ContactPaireNodes=[1;1];
ContactPaireLevel=[1;1];
ExstFlag=0;
while(ExstFlag==0)
   tempComtactPaire=[];
   tempContactPaireLevel=[];
   ExstFlag=1;
   disjointflagarry=[];
   for i=1:size(ContactPaireNodes,2)
       BoundingVolumA=BodyABVH.Level(ContactPaireLevel(1,i)).Nodes(ContactPaireNodes(1,i)).BoundingVoulum;
       BoundingVolumB=BodyBBVH.Level(ContactPaireLevel(2,i)).Nodes(ContactPaireNodes(2,i)).BoundingVoulum;
       disjointflag=SAT_Collosion_Detection(BoundingVolumA,BoundingVolumB,qrA,qrB); 
       disjointflagarry=[disjointflagarry,disjointflag];
       if disjointflag==0
           childnodesA=BodyABVH.Level(ContactPaireLevel(1,i)).Nodes(ContactPaireNodes(1,i)).ChildNodes;
           childnodesB=BodyBBVH.Level(ContactPaireLevel(2,i)).Nodes(ContactPaireNodes(2,i)).ChildNodes;
           if ~isempty(childnodesA)&&~isempty(childnodesB)
               tempComtactPaire=[tempComtactPaire,[childnodesA(1),childnodesA(1),childnodesA(2),childnodesA(2);repmat(childnodesB,1,2)]];
               tempContactPaireLevel=[tempContactPaireLevel,[ones(1,4)*ContactPaireLevel(1,i)+1;ones(1,4)*ContactPaireLevel(2,i)+1]];
               ExstFlag=0;
           elseif isempty(childnodesA)&&~isempty(childnodesB)
               tempComtactPaire=[tempComtactPaire,[ContactPaireNodes(1,i),ContactPaireNodes(1,i);childnodesB]];
               tempContactPaireLevel=[tempContactPaireLevel,[ones(1,2)*ContactPaireLevel(1,i);ones(1,2)*ContactPaireLevel(2,i)+1]];
               ExstFlag=0;
           elseif ~isempty(childnodesA)&&isempty(childnodesB)
               tempComtactPaire=[tempComtactPaire,[childnodesA;ContactPaireNodes(2,i),ContactPaireNodes(2,i)]];
               tempContactPaireLevel=[tempContactPaireLevel,[ones(1,2)*ContactPaireLevel(1,i)+1;ones(1,2)*ContactPaireLevel(2,i)]];
               ExstFlag=0;
           elseif isempty(childnodesA)&&isempty(childnodesB)
               tempComtactPaire=[tempComtactPaire,[ContactPaireNodes(1,i);ContactPaireNodes(2,i)]];
               tempContactPaireLevel=[tempContactPaireLevel,[ContactPaireLevel(1,i);ContactPaireLevel(2,i)]];
           end
       end
   end
   ContactPaireNodes=tempComtactPaire;
   ContactPaireLevel=tempContactPaireLevel;
end
if(~isempty(ContactPaireNodes))
    disjointflag=0;
end
ContactPaireLines=[];
if disjointflag==0
    numcontactpair=size(ContactPaireNodes,2);
    nointersetindex=[];
    for i=1:numcontactpair
        trianleAelement=BodyABVH.Level(ContactPaireLevel(1,i)).Nodes(ContactPaireNodes(1,i)).Element;
        trianleBelement=BodyBBVH.Level(ContactPaireLevel(2,i)).Nodes(ContactPaireNodes(2,i)).Element;
        [intersertflag,insertpoint1,insertpoint2]=Triangle_Triangle_Detection(nodesA,nodesB,qrA,qrB,trianleAelement,trianleBelement);
        if intersertflag==0
            nointersetindex=[nointersetindex,i];
            continue;
        end
        ContactPaireLines=[ContactPaireLines,[insertpoint1;insertpoint2]];
    end
    ContactPaireLevel(:,nointersetindex)=[];
    ContactPaireNodes(:,nointersetindex)=[];
    if isempty(ContactPaireLevel)
        disjointflag=1;
        ContactPaireLines=[];
    end
else 
    ContactPaireLevel=[];
    ContactPaireNodes=[];
end
end

3.2 包围盒接触检测算法基于分离轴（STA）算法，代码如下：

function disjointflag=SAT_Collosion_Detection(BoundingVolumA,BoundingVolumB,qrA,qrB)
qrrA=qrA(1:3);
qrrB=qrB(1:3);
qthetaA=qrA(4:end);
qthetaB=qrB(4:end);
RA=Rotationmatrix(qthetaA);
RB=Rotationmatrix(qthetaB);
AxisA=RA*BoundingVolumA.axis;
AxisB=RB*BoundingVolumB.axis;
ParametersA=BoundingVolumA.parameters;
ParametersB=BoundingVolumB.parameters;
CenterpointA=qrrA+RA*BoundingVolumA.centerpoint;
CenterpointB=qrrB+RB*BoundingVolumB.centerpoint;
potantial_separateAxises=[AxisA,AxisB];
tempAxisA=[repmat(AxisA(:,1),1,3),repmat(AxisA(:,2),1,3),repmat(AxisA(:,3),1,3)];
tempAxisB=repmat(AxisB,1,3);
potantial_separateAxises=[potantial_separateAxises,cross(tempAxisA,tempAxisB)];
T=CenterpointB-CenterpointA;
disjointflag=0;
for i=1:size(potantial_separateAxises,2)
    Laxis=potantial_separateAxises(:,i);
    rA=sum(abs((repmat(ParametersA,3,1).*AxisA)'*Laxis));
    rB=sum(abs((repmat(ParametersB,3,1).*AxisB)'*Laxis));
    tempTA=abs(T'*Laxis);
    temprArB=rA+rB;
    if tempTA<1e-10
        tempTA=0;
    end
    if temprArB<1e-10
        temprArB=0;
    end
    if(tempTA>temprArB)
        disjointflag=1;
        break;
    end
end
end

3.3 三角形相交代码如下：

function [intersertflag,insertpoint1,insertpoint2]=Triangle_Triangle_Detection(nodesA,nodesB,qrA,qrB,Triagnle_eleA,Triagnle_eleB)
qrrA=qrA(1:3);
qrrB=qrB(1:3);
qthetaA=qrA(4:end);
qthetaB=qrB(4:end);
RA=Rotationmatrix(qthetaA);
RB=Rotationmatrix(qthetaB);
VerticesA=qrrA+RA*nodesA(Triagnle_eleA,:)';
VerticesB=qrrB+RB*nodesB(Triagnle_eleB,:)';
N2=cross(VerticesB(:,2)-VerticesB(:,1),VerticesB(:,3)-VerticesB(:,1));
d2=-N2'*VerticesB(:,1);
dvi=N2'*VerticesA+d2;
N1=cross(VerticesA(:,2)-VerticesA(:,1),VerticesA(:,3)-VerticesA(:,1));
d1=-N1'*VerticesA(:,1);
dvj=N1'*VerticesB+d1;
dvi(find(abs(dvi)<1e-10))=0;
dvj(find(abs(dvj)<1e-10))=0;
tempsigndvi=unique(sign(dvi));
tempsigndvi(find(tempsigndvi==0))=[];
lengthsigndvi=length(tempsigndvi);
tempsigndvj=unique(sign(dvj));
tempsigndvj(find(tempsigndvj==0))=[];
lengthsigndvj=length(tempsigndvj);
indexdvi=find(dvi==0);
indexdvj=find(dvj==0);
insertpoint1=[];
insertpoint2=[];
intersertflag=1;
if lengthsigndvi==1|| lengthsigndvi==0   %&&isempty(indexdvi)
    intersertflag=0;
    return;
end
if lengthsigndvj==1|| lengthsigndvj==0   %&&isempty(indexdvj)
   intersertflag=0;
   return;
end
D=cross(N1,N2);
D=D/norm(D);
if lengthsigndvi==2||lengthsigndvj==2
    %%% select original point
    for i=1:3
        if i==1        
            A=[N1(1),N1(2);N2(1),N2(2)];
            tempindex=[1,2];
            if rank(A)==1
              continue;
            end
        elseif i==2
            A=[N1(1),N1(3);N2(1),N2(3)];
            tempindex=[1,3];
            if rank(A)==1
                continue;
            end
        elseif i==3
            A=[N1(2),N1(3);N2(2),N2(3)];
            tempindex=[2,3];
            if rank(A)==1
                continue;
            end
        end
         b=-[d1;d2];
         oxyz=A\b;
         pointO=zeros(3,1);
         pointO(tempindex)=oxyz;
    end
    pv1=D'*(VerticesA-pointO);
    pv2=D'*(VerticesB-pointO);
    
    
    tempsigndvi=sign(dvi);
    tempsigndvi(indexdvi)=1;
    indexsigndvi1=find(tempsigndvi==1);
    indexsigndvi_1=find(tempsigndvi==-1);
    if(length(indexsigndvi1)==2)
        upsideindex=indexsigndvi1;
        downsideindex=indexsigndvi_1;
    else
        upsideindex=indexsigndvi_1;
        downsideindex=indexsigndvi1;
    end
    t11=pv1(:,upsideindex(1))+(pv1(:,downsideindex)-pv1(upsideindex(1)))*dvi(upsideindex(1))/(dvi(upsideindex(1))-dvi(downsideindex));
    t12=pv1(:,upsideindex(2))+(pv1(:,downsideindex)-pv1(upsideindex(2)))*dvi(upsideindex(2))/(dvi(upsideindex(2))-dvi(downsideindex));
    
    
    tempsigndvj=sign(dvj);
    tempsigndvj(indexdvj)=1;
    indexsigndvj1=find(tempsigndvj==1);
    indexsigndvj_1=find(tempsigndvj==-1);
    if(length(indexsigndvj1)==2)
        upsideindex=indexsigndvj1;
        downsideindex=indexsigndvj_1;
    else
        upsideindex=indexsigndvj_1;
        downsideindex=indexsigndvj1;
    end
    t21=pv2(:,upsideindex(1))+(pv2(:,downsideindex)-pv2(upsideindex(1)))*dvj(upsideindex(1))/(dvj(upsideindex(1))-dvj(downsideindex));
    t22=pv2(:,upsideindex(2))+(pv2(:,downsideindex)-pv2(upsideindex(2)))*dvj(upsideindex(2))/(dvj(upsideindex(2))-dvj(downsideindex));
    
    tempt1=[t11,t12];
    tempt2=[t21,t22];
    if max(tempt1)<=min(tempt2)||min(tempt1)>=max(tempt2)
        intersertflag=0;
        return;
    end
    [tempt]=sort([t11,t12,t21,t22], 'ascend' );
    insertpoint1=pointO+tempt(2)*D;
    insertpoint2=pointO+tempt(3)*D;  
end
end

4. 实验分析

其中红色曲线包围渗透区域